use new get_roll_deg and friends

Author: peterbarker

AntennaTracker/system.cpp

@@ -190,7 +190,7 @@ void Tracker::set_mode(Mode &newmode, const ModeReason reason)
 #endif
     gcs().send_message(MSG_HEARTBEAT);
 
-    nav_status.bearing = ahrs.yaw_sensor * 0.01f;
+    nav_status.bearing = ahrs.get_yaw_deg();
 }
 
 bool Tracker::set_mode(const uint8_t new_mode, const ModeReason reason)

ArduCopter/autoyaw.cpp

@@ -81,7 +81,7 @@ void Mode::AutoYaw::set_mode(Mode yaw_mode)
 
     case Mode::LOOK_AHEAD:
         // Commanded Yaw to automatically look ahead.
-        _look_ahead_yaw = copter.ahrs.yaw_sensor * 0.01;  // cdeg -> deg
+        _look_ahead_yaw = copter.ahrs.get_yaw_deg();
         break;
 
     case Mode::RESETTOARMEDYAW:

ArduPlane/commands_logic.cpp

@@ -492,7 +492,7 @@ void Plane::do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd)
     } else {
         // use yaw based bearing hold
         steer_state.hold_course_cd = wrap_360_cd(ahrs.yaw_sensor);
-        bearing = ahrs.yaw_sensor * 0.01f;
+        bearing = ahrs.get_yaw_deg();
         next_WP_loc.offset_bearing(bearing, 1000); // push it out 1km
     }
 

ArduPlane/mode_takeoff.cpp

@@ -82,7 +82,7 @@ void ModeTakeoff::update()
     const float dist = target_dist;
     if (!takeoff_mode_setup) {
         const uint16_t altitude = plane.relative_ground_altitude(false,true);
-        const float direction = degrees(ahrs.get_yaw_rad());
+        const float direction = ahrs.get_yaw_deg();
         // see if we will skip takeoff as already flying
         if (plane.is_flying() && (millis() - plane.started_flying_ms > 10000U) && ahrs.groundspeed() > 3) {
             if (altitude >= alt) {

ArduPlane/quadplane.cpp

@@ -2547,7 +2547,7 @@ void QuadPlane::vtol_position_controller(void)
             target_speed_xy_cms = diff_wp_norm * target_speed * 100;
             target_accel_cms = diff_wp_norm * (-target_accel*100);
             target_yaw_deg = degrees(diff_wp_norm.angle());
-            const float yaw_err_deg = wrap_180(target_yaw_deg - degrees(plane.ahrs.get_yaw_rad()));
+            const float yaw_err_deg = wrap_180(target_yaw_deg - plane.ahrs.get_yaw_deg());
             bool overshoot = (closing_groundspeed < 0 || fabsf(yaw_err_deg) > 60);
             if (overshoot && !poscontrol.overshoot) {
                 gcs().send_text(MAV_SEVERITY_INFO,"VTOL Overshoot d=%.1f cs=%.1f yerr=%.1f",
@@ -2996,7 +2996,7 @@ void QuadPlane::assign_tilt_to_fwd_thr(void) {
 */
 float QuadPlane::get_scaled_wp_speed(float target_bearing_deg) const
 {
-    const float yaw_difference = fabsf(wrap_180(degrees(plane.ahrs.get_yaw_rad()) - target_bearing_deg));
+    const float yaw_difference = fabsf(wrap_180(plane.ahrs.get_yaw_deg() - target_bearing_deg));
     const float wp_speed = wp_nav->get_default_speed_xy() * 0.01;
     if (yaw_difference > 20) {
         // this gives a factor of 2x reduction in max speed when

ArduPlane/takeoff.cpp

@@ -211,7 +211,7 @@ void Plane::takeoff_calc_pitch(void)
             // increase the robustness of hand launches, particularly
             // in cross-winds. If we start to roll over then we reduce
             // pitch demand until the roll recovers
-            float roll_error_rad = radians(constrain_float(labs(nav_roll_cd - ahrs.roll_sensor) * 0.01, 0, 90));
+            float roll_error_rad = radians(constrain_float(labs(nav_roll_cd*0.01 - ahrs.get_roll_deg()), 0, 90));
             float reduction = sq(cosf(roll_error_rad));
             nav_pitch_cd *= reduction;
         }

ArduSub/turn_counter.cpp

@@ -8,11 +8,11 @@ void Sub::update_turn_counter()
     // Determine state
     // 0: 0-90 deg, 1: 90-180 deg, 2: -180--90 deg, 3: -90--0 deg
     uint8_t turn_state;
-    if (ahrs.get_yaw_rad() >= 0.0f && ahrs.get_yaw_rad() < radians(90)) {
+    if (ahrs.get_yaw_rad() >= 0.0f && ahrs.get_yaw_deg() < 90) {
         turn_state = 0;
-    } else if (ahrs.get_yaw_rad() > radians(90)) {
+    } else if (ahrs.get_yaw_deg() > 90) {
         turn_state = 1;
-    } else if (ahrs.get_yaw_rad() < -radians(90)) {
+    } else if (ahrs.get_yaw_deg() < -90) {
         turn_state = 2;
     } else {
         turn_state = 3;

libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp

@@ -20,10 +20,10 @@ void AC_AttitudeControl_Multi_6DoF::rate_controller_run() {
     // if 6DoF control, always point directly up
     // this stops horizontal drift due to error between target and true attitude
     if (lateral_enable) {
-        roll_deg = degrees(AP::ahrs().get_roll_rad());
+        roll_deg = AP::ahrs().get_roll_deg();
     }
     if (forward_enable) {
-        pitch_deg = degrees(AP::ahrs().get_pitch_rad());
+        pitch_deg = AP::ahrs().get_pitch_deg();
     }
     _motors.set_roll_pitch(roll_deg,pitch_deg);
 

libraries/AC_Avoidance/AC_Avoidance_Logging.cpp

@@ -20,7 +20,7 @@ void AP_OABendyRuler::Write_OABendyRuler(const uint8_t type, const bool active,
         type        : type,
         active      : active,
         target_yaw  : (uint16_t)wrap_360(target_yaw),
-        yaw         : (uint16_t)wrap_360(AP::ahrs().yaw_sensor * 0.01f),
+        yaw         : (uint16_t)wrap_360(AP::ahrs().get_yaw_deg()),
         target_pitch: (uint16_t)target_pitch,
         resist_chg  : resist_chg,
         margin      : margin,

libraries/AC_Avoidance/AP_OABendyRuler.cpp

@@ -114,7 +114,7 @@ bool AP_OABendyRuler::update(const Location& current_loc, const Location& destin
     float ground_course_deg;
     if (ground_speed_vec.length_squared() < OA_BENDYRULER_LOW_SPEED_SQUARED) {
         // with zero ground speed use vehicle's heading
-        ground_course_deg = AP::ahrs().yaw_sensor * 0.01f;
+        ground_course_deg = AP::ahrs().get_yaw_deg();
     } else {
         ground_course_deg = degrees(ground_speed_vec.angle());
     }

libraries/AP_AHRS/AP_AHRS.cpp

@@ -847,7 +847,7 @@ float AP_AHRS::wind_alignment(const float heading_deg) const
  */
 float AP_AHRS::head_wind(void) const
 {
-    const float alignment = wind_alignment(yaw_sensor*0.01f);
+    const float alignment = wind_alignment(get_yaw_deg());
     return alignment * wind_estimate().xy().length();
 }
 

libraries/AP_AHRS/AP_AHRS.h

@@ -561,9 +561,9 @@ class AP_AHRS {
     float get_yaw_rad() const { return yaw; }
 
     // roll/pitch/yaw euler angles, all in radians
-    float get_roll_deg() const { return roll_deg; }
-    float get_pitch_deg() const { return pitch_deg; }
-    float get_yaw_deg() const { return yaw_deg; }
+    float get_roll_deg() const { return rpy_deg[0]; }
+    float get_pitch_deg() const { return rpy_deg[1]; }
+    float get_yaw_deg() const { return rpy_deg[2]; }
 
     // helper trig value accessors
     float cos_roll() const  {
@@ -586,9 +586,7 @@ class AP_AHRS {
     }
 
     // floating point Euler angles (Degrees)
-    float roll_deg;
-    float pitch_deg;
-    float yaw_deg;
+    float rpy_deg[3];
 
     // integer Euler angles (Degrees * 100)
     int32_t roll_sensor;

libraries/AP_AHRS/AP_AHRS_Backend.cpp

@@ -151,16 +151,16 @@ void AP_AHRS::update_trig(void)
  */
 void AP_AHRS::update_cd_values(void)
 {
-    roll_deg = degrees(roll);
-    pitch_deg = degrees(pitch);
-    yaw_deg = degrees(yaw);
-    if (yaw_deg < 0) {
-        yaw_deg += 360;
+    rpy_deg[0] = degrees(roll);
+    rpy_deg[1] = degrees(pitch);
+    rpy_deg[2] = degrees(yaw);
+    if (rpy_deg[2] < 0) {
+        rpy_deg[2] += 360;
     }
 
-    roll_sensor  = roll_deg * 100;
-    pitch_sensor = pitch_deg * 100;
-    yaw_sensor   = yaw_deg * 100;
+    roll_sensor  = rpy_deg[0] * 100;
+    pitch_sensor = rpy_deg[1] * 100;
+    yaw_sensor   = rpy_deg[2] * 100;
 }
 
 /*

libraries/AP_Compass/Compass_learn.cpp

@@ -43,7 +43,7 @@ void CompassLearn::update(void)
         // no GSF available
         return;
     }
-    if (degrees(fabsf(ahrs.get_pitch_rad())) > 50) {
+    if (fabsf(ahrs.get_pitch_deg()) > 50) {
         // we don't want to be too close to nose up, or yaw gets
         // problematic. Tailsitters need to wait till they are in
         // forward flight

libraries/AP_DroneCAN/AP_DroneCAN.cpp

@@ -1047,7 +1047,7 @@ void AP_DroneCAN::notify_state_send()
 #endif // HAL_BUILD_AP_PERIPH
 
     msg.aux_data_type = ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_YAW_EARTH_CENTIDEGREES;
-    uint16_t yaw_cd = (uint16_t)(360.0f - degrees(AP::ahrs().get_yaw_rad()))*100.0f;
+    uint16_t yaw_cd = (uint16_t)(360.0f - AP::ahrs().get_yaw_deg())*100.0f;
     const uint8_t *data = (uint8_t *)&yaw_cd;
     for (uint8_t i=0; i<2; i++) {
         msg.aux_data.data[i] = data[i];

libraries/AP_LTM_Telem/AP_LTM_Telem.cpp

@@ -181,9 +181,9 @@ void AP_LTM_Telem::send_Aframe(void)
     {
         AP_AHRS &ahrs = AP::ahrs();
         WITH_SEMAPHORE(ahrs.get_semaphore());
-        pitch = roundf(ahrs.pitch_sensor * 0.01); // attitude pitch in degrees
-        roll = roundf(ahrs.roll_sensor * 0.01);   // attitude roll in degrees
-        heading = roundf(ahrs.yaw_sensor * 0.01); // heading in degrees
+        pitch = roundf(ahrs.get_pitch_deg()); // attitude pitch in degrees
+        roll = roundf(ahrs.get_roll_deg());   // attitude roll in degrees
+        heading = roundf(ahrs.get_yaw_deg()); // heading in degrees
     }
 #else
     pitch = 0;

libraries/AP_MSP/AP_MSP_Telem_Backend.cpp

@@ -905,9 +905,9 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_attitude(sbuf_t *dst)
         int16_t pitch;
         int16_t yaw;
     } attitude {
-        roll : int16_t(ahrs.roll_sensor * 0.1),     // centidegress to decidegrees
-        pitch : int16_t(ahrs.pitch_sensor * 0.1),   // centidegress to decidegrees
-        yaw : int16_t(ahrs.yaw_sensor * 0.01)       // centidegress to degrees
+        roll : int16_t(ahrs.get_roll_deg() * 10),     // degress to decidegrees
+        pitch : int16_t(ahrs.get_pitch_deg() * 10),   // degress to decidegrees
+        yaw : int16_t(ahrs.get_yaw_deg())
     };
 
     sbuf_write_data(dst, &attitude, sizeof(attitude));

libraries/AP_Mount/AP_Mount_Backend.cpp

@@ -554,7 +554,7 @@ void AP_Mount_Backend::calculate_poi()
         // iteratively move test_loc forward until its alt-above-sea-level is below terrain-alt-above-sea-level
         const float dist_increment_m = MAX(terrain->get_grid_spacing(), 10);
         const float mount_pitch_deg = degrees(quat.get_euler_pitch());
-        const float mount_yaw_ef_deg = wrap_180(degrees(quat.get_euler_yaw()) + degrees(ahrs.get_yaw_rad()));
+        const float mount_yaw_ef_deg = wrap_180(degrees(quat.get_euler_yaw()) + ahrs.get_yaw_deg());
         float total_dist_m = 0;
         bool get_terrain_alt_success = true;
         float prev_terrain_amsl_m = terrain_amsl_m;

libraries/AP_Mount/AP_Mount_Viewpro.cpp

@@ -681,16 +681,16 @@ bool AP_Mount_Viewpro::send_m_ahrs()
     uint16_t gps_vdop = AP::gps().get_vdop();
 
     // get vehicle yaw in the range 0 to 360
-    const uint16_t veh_yaw_deg = wrap_360(degrees(AP::ahrs().get_yaw_rad()));
+    const uint16_t veh_yaw_deg = wrap_360(AP::ahrs().get_yaw_deg());
 
     // fill in packet
     const M_AHRSPacket mahrs_packet {
         .content = {
             frame_id: FrameId::M_AHRS,
             data_type: 0x07,                        // Bit0: Attitude, Bit1: GPS, Bit2 Gyro
             unused2to8 : {0, 0, 0, 0, 0, 0, 0},
-            roll_be: htobe16(degrees(AP::ahrs().get_roll_rad()) * AP_MOUNT_VIEWPRO_DEG_TO_OUTPUT),      // vehicle roll angle.  1bit=360deg/65536
-            pitch_be: htobe16(-degrees(AP::ahrs().get_pitch_rad()) * AP_MOUNT_VIEWPRO_DEG_TO_OUTPUT),   // vehicle pitch angle.  1bit=360deg/65536
+            roll_be: htobe16(AP::ahrs().get_roll_deg() * AP_MOUNT_VIEWPRO_DEG_TO_OUTPUT),      // vehicle roll angle.  1bit=360deg/65536
+            pitch_be: htobe16(-AP::ahrs().get_pitch_deg() * AP_MOUNT_VIEWPRO_DEG_TO_OUTPUT),   // vehicle pitch angle.  1bit=360deg/65536
             yaw_be: htobe16(veh_yaw_deg * AP_MOUNT_VIEWPRO_DEG_TO_OUTPUT),                          // vehicle yaw angle.  1bit=360deg/65536
             date_be: htobe16(date),                 // bit0~6:year, bit7~10:month, bit11~15:day
             seconds_utc: {uint8_t((second_hundredths & 0xFF0000ULL) >> 16), // seconds * 100 MSB.  1bit = 0.01sec

libraries/AP_NMEA_Output/AP_NMEA_Output.cpp

@@ -251,9 +251,9 @@ void AP_NMEA_Output::update()
 #if AP_AHRS_ENABLED
     if ((_message_enable_bitmask.get() & static_cast<int16_t>(Enabled_Messages::PASHR)) != 0) {
         // get roll, pitch, yaw
-        const float roll_deg = wrap_180(degrees(ahrs.get_roll_rad()));
-        const float pitch_deg = wrap_180(degrees(ahrs.get_pitch_rad()));
-        const float yaw_deg = wrap_360(degrees(ahrs.get_yaw_rad()));
+        const float roll_deg = wrap_180(ahrs.get_roll_deg());
+        const float pitch_deg = wrap_180(ahrs.get_pitch_deg());
+        const float yaw_deg = wrap_360(ahrs.get_yaw_deg());
         const float heave_m = 0; // instantaneous heave in meters
         const float roll_deg_accuracy = 0; // stddev of roll_deg;
         const float pitch_deg_accuracy = 0; // stddev of pitch_deg;

libraries/AR_WPNav/AR_PivotTurn.cpp

@@ -94,7 +94,7 @@ void AR_PivotTurn::check_activation(float desired_heading_deg, bool force_active
     }
 
     // calc yaw error in degrees
-    const float yaw_error = fabsf(wrap_180(desired_heading_deg - (AP::ahrs().yaw_sensor * 0.01f)));
+    const float yaw_error = fabsf(wrap_180(desired_heading_deg - (AP::ahrs().get_yaw_deg())));
 
     // if error is larger than _pivot_angle start pivot steering
     if (yaw_error > _angle || force_active) {