CODEWITHSUNDEEP
c#gpsangle
Htekin
Htekin

Reputation: 29

Yaw rate calculation from GPS Heading

I have a dual GPS mounted on robot and getting data at 20Hz. I got heading data from GNTRA sentence which gives the heading from dual antenna. I am trying to get yaw rate of vehicle. I am having problem when I turn to left side of North. Normally, I am trying to get difference of two measurement and divide it with time interval. I am not good at trigonometry. For example, when I am driving to 5 degree of heading and making turn to left to 350 degree.I need to measure -15 degree/ time_interval. But I am measuring 345/time_interval with my code. I am making mistake and I could not figure out how to solve it. Can anybody help me? Thanks

        public double calc_yawrate(heading)
        {
         mywatch.Stop();
         double t = (double)mywatch.ElapsedMilliseconds / 1000;
         mywatch.Reset();
         mywatch.Start();


         Speed = Speed / 3.6;
         Speed = Math.Round(Speed, 2);
         double yaw = heading;
         double yawrate;
         yawrate = (yaw- yawpast) / t;
         yawpast = yaw;
        }

Upvotes: 0

Views: 439

Answers (1)

shingo
shingo

Reputation: 27086

It seems like you want the difference in range [-180°, 180°], then you can add/minus the value by 360° until it's in range.

var diff = yaw - yawpast;

if(diff > 180)
{
    do
    {
        diff -= 360;
    }
    while(diff <= 180);
}
else if(diff < -180)
{
    do
    {
        diff += 360;
    }
    while(diff >= -180);
}

yawrate = diff / t;

Upvotes: 1

Related Questions