Ask Your Question
1

How can I compute yaw rate from yaw angle?

asked 2020-11-18 14:59:49 -0600

Hi, I want to find a way to compute the yaw rate using the yaw angle. I have the idea of using the Euler forward method, however, I have encountered some issues like the fact that a full rotation is given by [0-pi, pi-0] and also considering multiple rotations (positives or negatives). The following code only works fine for 1 rotation.

void BebopVel::attitudeCallback(const bebop_msgs::Ardrone3PilotingStateAttitudeChanged &att_msg){

    double yaw = normalize_angle(-att_msg.yaw);
    std::cout << "Curr heading: " << yaw*180/M_PI << std::endl;

    if(!got_first_att_msg_){

        ROS_INFO("got first attitude message");

        prev_attitude_time_ = att_msg.header.stamp;
        prev_yaw_ = yaw;

        got_first_att_msg_ = true;
        return;
    }

    // get current values
    curr_attitude_time_ = att_msg.header.stamp;
    curr_yaw_ = yaw;

    // compute time difference between samples
    ros::Duration delta_t_attitude = curr_attitude_time_ - prev_attitude_time_;
    double ts_attitude = delta_t_attitude.toSec();      // convert from ROS time to seconds

    // compute yaw difference between samples
    double delta_yaw = curr_yaw_ - prev_yaw_;

    // compute yaw rate
    yaw_rate_ = delta_yaw / ts_attitude;

    // assign current values to previous
    prev_attitude_time_ = curr_attitude_time_;
    prev_yaw_ = curr_yaw_;
}


double BebopVel::normalize_angle(double ang){
// Normalizes the angle to be 0 to 2*pi
// It takes and returns radians.

    double norm_ang = fmod(ang, 2.0*M_PI);

    if(norm_ang < 0){
        return norm_ang + 2.0*M_PI;
    }

    return norm_ang;
}

PD: Sorry for my poor English.

edit retag flag offensive close merge delete

Comments

1

Well when you normalize the angle you are throwing away all the information about multiple rotations, is that function necessary in this context? Or, you could modify the function to return the number of rotations (forward or backward) then use that in the euler equation.

rmmilner gravatar image rmmilner  ( 2020-11-18 17:01:21 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-11-19 03:45:33 -0600

felixN gravatar image

If I'm not mistakent, with your current code, you should get correct resuts when you are not just at this moment finishing one turn.

I would suggest you replace

// compute yaw difference between samples
double delta_yaw = curr_yaw_ - prev_yaw_;

with

// compute yaw difference between samples
double delta_yaw = fmod(curr_yaw_ - prev_yaw_ ,  2.0*M_PI);

It should then be able to deal with finishing one turn.

The only problem that will remain, is if you do more that pi rads between 2 successive calls to the function (not sure if it can happen in your system). If you need to be able to do more than half a turn between 2 calls, then you might try to remove the normalization (normalize_angle) and remove my fmod : if this works or not will depend if your att_msg.yaw evolving continuously or if it can have 2*pi jumps (in this last case, I think nothing can be done)

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2020-11-18 14:59:49 -0600

Seen: 196 times

Last updated: Nov 19 '20