AP_AHRS: fixed wrap of yaw in AHRS_View

8 years ago · 6b6ad6ddf2
1 changed files with 3 additions and 0 deletions
--- a/libraries/AP_AHRS/AP_AHRS_View.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_View.cpp
@ -62,6 +62,9 @@ void AP_AHRS_View::update(bool skip_ins_update)
				@@ -62,6 +62,9 @@ void AP_AHRS_View::update(bool skip_ins_update)
    roll_sensor  = degrees(roll) * 100;
    pitch_sensor = degrees(pitch) * 100;
    yaw_sensor   = degrees(yaw) * 100;
+    if (yaw_sensor < 0) {
+        yaw_sensor += 36000;
+    }

    ahrs.calc_trig(rot_body_to_ned,
                   trig.cos_roll, trig.cos_pitch, trig.cos_yaw,