Revision history [back]

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)