rename AHRS get_roll etc accessors to include _rad

Author: peterbarker

ArduCopter/land_detector.cpp

@@ -82,7 +82,7 @@ void Copter::update_land_detector()
         // check if landing
         const bool landing = flightmode->is_landing();
         SET_LOG_FLAG(landing, LandDetectorLoggingFlag::LANDING);
-        bool motor_at_lower_limit = (flightmode->has_manual_throttle() && (motors->get_below_land_min_coll() || heli_flags.coll_stk_low) && fabsf(ahrs.get_roll()) < M_PI/2.0f)
+        bool motor_at_lower_limit = (flightmode->has_manual_throttle() && (motors->get_below_land_min_coll() || heli_flags.coll_stk_low) && fabsf(ahrs.get_roll_rad()) < M_PI/2.0f)
 #if MODE_AUTOROTATE_ENABLED == ENABLED
                                     || (flightmode->mode_number() == Mode::Number::AUTOROTATE && motors->get_below_land_min_coll())
 #endif

ArduPlane/ArduPlane.cpp

@@ -952,8 +952,8 @@ void Plane::get_osd_roll_pitch_rad(float &roll, float &pitch) const
         return;
     }
 #endif
-    pitch = ahrs.get_pitch();
-    roll = ahrs.get_roll();
+    pitch = ahrs.get_pitch_rad();
+    roll = ahrs.get_roll_rad();
     if (!(flight_option_enabled(FlightOptions::OSD_REMOVE_TRIM_PITCH))) {  // correct for PTCH_TRIM_DEG
         pitch -= g.pitch_trim * DEG_TO_RAD;
     }

ArduPlane/GCS_Mavlink.cpp

@@ -129,9 +129,9 @@ void GCS_MAVLINK_Plane::send_attitude() const
 {
     const AP_AHRS &ahrs = AP::ahrs();
 
-    float r = ahrs.get_roll();
-    float p = ahrs.get_pitch();
-    float y = ahrs.get_yaw();
+    float r = ahrs.get_roll_rad();
+    float p = ahrs.get_pitch_rad();
+    float y = ahrs.get_yaw_rad();
 
     if (!(plane.flight_option_enabled(FlightOptions::GCS_REMOVE_TRIM_PITCH))) {
         p -= radians(plane.g.pitch_trim);

ArduPlane/commands_logic.cpp

@@ -1060,7 +1060,7 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
                 const float breakout_direction_rad = radians(vtol_approach_s.approach_direction_deg + (direction > 0 ? 270 : 90));
 
                 // breakout when within 5 degrees of the opposite direction
-                if (fabsF(wrap_PI(ahrs.get_yaw() - breakout_direction_rad)) < radians(5.0f)) {
+                if (fabsF(wrap_PI(ahrs.get_yaw_rad() - breakout_direction_rad)) < radians(5.0f)) {
                     gcs().send_text(MAV_SEVERITY_INFO, "Starting VTOL land approach path");
                     vtol_approach_s.approach_stage = VTOLApproach::Stage::APPROACH_LINE;
                     set_next_WP(cmd.content.location);

ArduPlane/mode_cruise.cpp

@@ -61,7 +61,7 @@ void ModeCruise::navigate()
 
     // check if we are moving in the direction of the front of the vehicle
     const int32_t ground_course_cd = plane.gps.ground_course_cd();
-    const bool moving_forwards = fabsf(wrap_PI(radians(ground_course_cd * 0.01) - plane.ahrs.get_yaw())) < M_PI_2;
+    const bool moving_forwards = fabsf(wrap_PI(radians(ground_course_cd * 0.01) - plane.ahrs.get_yaw_rad())) < M_PI_2;
 
     if (!locked_heading &&
         plane.channel_roll->get_control_in() == 0 &&

ArduPlane/mode_guided.cpp

@@ -52,7 +52,7 @@ void ModeGuided::update()
 
         float error = 0.0f;
         if (plane.guided_state.target_heading_type == GUIDED_HEADING_HEADING) {
-            error = wrap_PI(plane.guided_state.target_heading - AP::ahrs().get_yaw());
+            error = wrap_PI(plane.guided_state.target_heading - AP::ahrs().get_yaw_rad());
         } else {
             Vector2f groundspeed = AP::ahrs().groundspeed_vector();
             error = wrap_PI(plane.guided_state.target_heading - atan2f(-groundspeed.y, -groundspeed.x) + M_PI);

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());
+        const float direction = degrees(ahrs.get_yaw_rad());
         // 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()));
+            const float yaw_err_deg = wrap_180(target_yaw_deg - degrees(plane.ahrs.get_yaw_rad()));
             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()) - target_bearing_deg));
+    const float yaw_difference = fabsf(wrap_180(degrees(plane.ahrs.get_yaw_rad()) - 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

ArduSub/mode_guided.cpp

@@ -385,7 +385,7 @@ void ModeGuided::guided_set_angle(const Quaternion &q, float climb_rate_cms)
 // helper function to set yaw state and targets
 void ModeGuided::guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle)
 {    
-    float current_yaw = wrap_2PI(AP::ahrs().get_yaw());
+    float current_yaw = wrap_2PI(AP::ahrs().get_yaw_rad());
     float euler_yaw_angle;
     float yaw_error;
 

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() >= 0.0f && ahrs.get_yaw() < radians(90)) {
+    if (ahrs.get_yaw_rad() >= 0.0f && ahrs.get_yaw_rad() < radians(90)) {
         turn_state = 0;
-    } else if (ahrs.get_yaw() > radians(90)) {
+    } else if (ahrs.get_yaw_rad() > radians(90)) {
         turn_state = 1;
-    } else if (ahrs.get_yaw() < -radians(90)) {
+    } else if (ahrs.get_yaw_rad() < -radians(90)) {
         turn_state = 2;
     } else {
         turn_state = 3;

Blimp/Blimp.cpp

@@ -244,7 +244,7 @@ void Blimp::read_AHRS(void)
                                 pos_ned.x,
                                 pos_ned.y,
                                 pos_ned.z,
-                                blimp.ahrs.get_yaw());
+                                blimp.ahrs.get_yaw_rad());
 #endif
 }
 

Blimp/Loiter.cpp

@@ -34,7 +34,7 @@ void Loiter::run(Vector3f& target_pos, float& target_yaw, Vector4b axes_disabled
                                 scaler_xz, scaler_yyaw, scaler_xz_n, scaler_yyaw_n);
 #endif
 
-    float yaw_ef = blimp.ahrs.get_yaw();
+    float yaw_ef = blimp.ahrs.get_yaw_rad();
     Vector3f err_xyz = target_pos - blimp.pos_ned;
     float err_yaw = wrap_PI(target_yaw - yaw_ef);
 
@@ -96,7 +96,7 @@ void Loiter::run(Vector3f& target_pos, float& target_yaw, Vector4b axes_disabled
         blimp.pid_vel_z.set_integrator(0);
         blimp.pid_vel_yaw.set_integrator(0);
         target_pos = blimp.pos_ned;
-        target_yaw = blimp.ahrs.get_yaw();
+        target_yaw = blimp.ahrs.get_yaw_rad();
     }
 
     if (zero.x) {

Blimp/mode_loiter.cpp

@@ -9,7 +9,7 @@
 bool ModeLoiter::init(bool ignore_checks)
 {
     target_pos = blimp.pos_ned;
-    target_yaw = blimp.ahrs.get_yaw();
+    target_yaw = blimp.ahrs.get_yaw_rad();
 
     return true;
 }
@@ -41,7 +41,7 @@ void ModeLoiter::run()
     if (fabsf(target_pos.z-blimp.pos_ned.z) < (g.max_pos_z*POS_LAG)) {
         target_pos.z += pilot.z;
     }
-    if (fabsf(wrap_PI(target_yaw-ahrs.get_yaw())) < (g.max_pos_yaw*POS_LAG)) {
+    if (fabsf(wrap_PI(target_yaw-ahrs.get_yaw_rad())) < (g.max_pos_yaw*POS_LAG)) {
         target_yaw = wrap_PI(target_yaw + pilot_yaw);
     }
 

Rover/crash_check.cpp

@@ -20,7 +20,7 @@ void Rover::crash_check()
     }
 
     // Crashed if pitch/roll > crash_angle
-    if ((g2.crash_angle != 0) && ((fabsf(ahrs.get_pitch()) > radians(g2.crash_angle)) || (fabsf(ahrs.get_roll()) > radians(g2.crash_angle)))) {
+    if ((g2.crash_angle != 0) && ((fabsf(ahrs.get_pitch_rad()) > radians(g2.crash_angle)) || (fabsf(ahrs.get_roll_rad()) > radians(g2.crash_angle)))) {
         crashed = true;
     }
 

Rover/mode.cpp

@@ -292,7 +292,7 @@ void Mode::calc_throttle(float target_speed, bool avoidance_enabled)
 #if AP_AVOIDANCE_ENABLED
     // apply object avoidance to desired speed using half vehicle's maximum deceleration
     if (avoidance_enabled) {
-        g2.avoid.adjust_speed(0.0f, 0.5f * attitude_control.get_decel_max(), ahrs.get_yaw(), target_speed, rover.G_Dt);
+        g2.avoid.adjust_speed(0.0f, 0.5f * attitude_control.get_decel_max(), ahrs.get_yaw_rad(), target_speed, rover.G_Dt);
         if (g2.sailboat.tack_enabled() && g2.avoid.limits_active()) {
             // we are a sailboat trying to avoid fence, try a tack
             if (rover.control_mode != &rover.mode_acro) {

Rover/mode_circle.cpp

@@ -82,7 +82,7 @@ bool ModeCircle::_enter()
 
     config.dir = (direction == 1) ? Direction::CCW : Direction::CW;
     config.speed = is_positive(speed) ? speed : g2.wp_nav.get_default_speed();
-    target.yaw_rad = AP::ahrs().get_yaw();
+    target.yaw_rad = AP::ahrs().get_yaw_rad();
     target.speed = 0;
 
     // check speed around circle does not lead to excessive lateral acceleration
@@ -116,7 +116,7 @@ void ModeCircle::init_target_yaw_rad()
     // if no position estimate use vehicle yaw
     Vector2f curr_pos_NE;
     if (!AP::ahrs().get_relative_position_NE_origin(curr_pos_NE)) {
-        target.yaw_rad = AP::ahrs().get_yaw();
+        target.yaw_rad = AP::ahrs().get_yaw_rad();
         return;
     }
 
@@ -126,7 +126,7 @@ void ModeCircle::init_target_yaw_rad()
 
     // if current position is exactly at the center of the circle return vehicle yaw
     if (is_zero(dist_m)) {
-        target.yaw_rad = AP::ahrs().get_yaw();
+        target.yaw_rad = AP::ahrs().get_yaw_rad();
     } else {
         target.yaw_rad = center_to_veh.angle();
     }

Rover/sailboat.cpp

@@ -317,7 +317,7 @@ float Sailboat::get_VMG() const
         return speed;
     }
 
-    return (speed * cosf(wrap_PI(radians(rover.g2.wp_nav.wp_bearing_cd() * 0.01f) - rover.ahrs.get_yaw())));
+    return (speed * cosf(wrap_PI(radians(rover.g2.wp_nav.wp_bearing_cd() * 0.01f) - rover.ahrs.get_yaw_rad())));
 }
 
 // handle user initiated tack while in acro mode
@@ -328,15 +328,15 @@ void Sailboat::handle_tack_request_acro()
     }
     // set tacking heading target to the current angle relative to the true wind but on the new tack
     currently_tacking = true;
-    tack_heading_rad = wrap_2PI(rover.ahrs.get_yaw() + 2.0f * wrap_PI((rover.g2.windvane.get_true_wind_direction_rad() - rover.ahrs.get_yaw())));
+    tack_heading_rad = wrap_2PI(rover.ahrs.get_yaw_rad() + 2.0f * wrap_PI((rover.g2.windvane.get_true_wind_direction_rad() - rover.ahrs.get_yaw_rad())));
 
     tack_request_ms = AP_HAL::millis();
 }
 
 // return target heading in radians when tacking (only used in acro)
 float Sailboat::get_tack_heading_rad()
 {
-    if (fabsf(wrap_PI(tack_heading_rad - rover.ahrs.get_yaw())) < radians(SAILBOAT_TACKING_ACCURACY_DEG) ||
+    if (fabsf(wrap_PI(tack_heading_rad - rover.ahrs.get_yaw_rad())) < radians(SAILBOAT_TACKING_ACCURACY_DEG) ||
        ((AP_HAL::millis() - tack_request_ms) > SAILBOAT_AUTO_TACKING_TIMEOUT_MS)) {
         clear_tack();
     }
@@ -482,7 +482,7 @@ float Sailboat::calc_heading(float desired_heading_cd)
     // if we are tacking we maintain the target heading until the tack completes or times out
     if (currently_tacking) {
         // check if we have reached target
-        if (fabsf(wrap_PI(tack_heading_rad - rover.ahrs.get_yaw())) <= radians(SAILBOAT_TACKING_ACCURACY_DEG)) {
+        if (fabsf(wrap_PI(tack_heading_rad - rover.ahrs.get_yaw_rad())) <= radians(SAILBOAT_TACKING_ACCURACY_DEG)) {
             clear_tack();
         } else if ((now - auto_tack_start_ms) > SAILBOAT_AUTO_TACKING_TIMEOUT_MS) {
             // tack has taken too long

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());
+        roll_deg = degrees(AP::ahrs().get_roll_rad());
     }
     if (forward_enable) {
-        pitch_deg = degrees(AP::ahrs().get_pitch());
+        pitch_deg = degrees(AP::ahrs().get_pitch_rad());
     }
     _motors.set_roll_pitch(roll_deg,pitch_deg);
 

libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp

@@ -460,7 +460,7 @@ void AC_AttitudeControl_Sub::input_euler_angle_roll_pitch_slew_yaw(float euler_r
     // Convert from centidegrees on public interface to radians
     const float euler_yaw_angle = wrap_PI(radians(euler_yaw_angle_cd * 0.01f));
 
-    const float current_yaw = AP::ahrs().get_yaw();
+    const float current_yaw = AP::ahrs().get_yaw_rad();
 
     // Compute angle error
     const float yaw_error = wrap_PI(euler_yaw_angle - current_yaw);
@@ -491,4 +491,4 @@ void AC_AttitudeControl_Sub::set_notch_sample_rate(float sample_rate)
     _pid_rate_pitch.set_notch_sample_rate(sample_rate);
     _pid_rate_yaw.set_notch_sample_rate(sample_rate);
 #endif
-}
+}

libraries/APM_Control/AP_PitchController.cpp

@@ -273,7 +273,7 @@ float AP_PitchController::get_rate_out(float desired_rate, float scaler)
 float AP_PitchController::_get_coordination_rate_offset(float &aspeed, bool &inverted) const
 {
     float rate_offset;
-    float bank_angle = AP::ahrs().get_roll();
+    float bank_angle = AP::ahrs().get_roll_rad();
 
     // limit bank angle between +- 80 deg if right way up
     if (fabsf(bank_angle) < radians(90))	{
@@ -296,7 +296,7 @@ float AP_PitchController::_get_coordination_rate_offset(float &aspeed, bool &inv
         // don't do turn coordination handling when at very high pitch angles
         rate_offset = 0;
     } else {
-        rate_offset = cosf(_ahrs.get_pitch())*fabsf(ToDeg((GRAVITY_MSS / MAX((aspeed * _ahrs.get_EAS2TAS()), MAX(aparm.airspeed_min, 1))) * tanf(bank_angle) * sinf(bank_angle))) * _roll_ff;
+        rate_offset = cosf(_ahrs.get_pitch_rad())*fabsf(ToDeg((GRAVITY_MSS / MAX((aspeed * _ahrs.get_EAS2TAS()), MAX(aparm.airspeed_min, 1))) * tanf(bank_angle) * sinf(bank_angle))) * _roll_ff;
     }
     if (inverted) {
         rate_offset = -rate_offset;

libraries/APM_Control/AP_YawController.cpp

@@ -206,7 +206,7 @@ int32_t AP_YawController::get_servo_out(float scaler, bool disable_integrator)
     // Calculate yaw rate required to keep up with a constant height coordinated turn
     float aspeed;
     float rate_offset;
-    float bank_angle = AP::ahrs().get_roll();
+    float bank_angle = AP::ahrs().get_roll_rad();
     // limit bank angle between +- 80 deg if right way up
     if (fabsf(bank_angle) < 1.5707964f)	{
         bank_angle = constrain_float(bank_angle,-1.3962634f,1.3962634f);

libraries/APM_Control/AR_AttitudeControl.cpp

@@ -613,7 +613,7 @@ float AR_AttitudeControl::get_steering_out_heading(float heading_rad, float rate
 // return a desired turn-rate given a desired heading in radians
 float AR_AttitudeControl::get_turn_rate_from_heading(float heading_rad, float rate_max_rads) const
 {
-    const float yaw_error = wrap_PI(heading_rad - AP::ahrs().get_yaw());
+    const float yaw_error = wrap_PI(heading_rad - AP::ahrs().get_yaw_rad());
 
     // Calculate the desired turn rate (in radians) from the angle error (also in radians)
     float desired_rate = _steer_angle_p.get_p(yaw_error);
@@ -884,7 +884,7 @@ float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, float
     }
 
     // initialise output to feed forward from current pitch angle
-    const float pitch_rad = AP::ahrs().get_pitch();
+    const float pitch_rad = AP::ahrs().get_pitch_rad();
     float output = sinf(pitch_rad) * _pitch_to_throttle_ff;
 
     // add regular PID control
@@ -940,7 +940,7 @@ float AR_AttitudeControl::get_sail_out_from_heel(float desired_heel, float dt)
     }
     _heel_controller_last_ms = now;
 
-    _sailboat_heel_pid.update_all(desired_heel, fabsf(AP::ahrs().get_roll()), dt);
+    _sailboat_heel_pid.update_all(desired_heel, fabsf(AP::ahrs().get_roll_rad()), dt);
 
     // get feed-forward
     const float ff = _sailboat_heel_pid.get_ff();

libraries/AP_AHRS/AP_AHRS.h

@@ -556,9 +556,14 @@ class AP_AHRS {
      */
 
     // roll/pitch/yaw euler angles, all in radians
-    float get_roll() const { return roll; }
-    float get_pitch() const { return pitch; }
-    float get_yaw() const { return yaw; }
+    float get_roll_rad() const { return roll; }
+    float get_pitch_rad() const { return pitch; }
+    float get_yaw_rad() const { return yaw; }
+
+    // roll/pitch/yaw euler angles, all in radians
+    float get_roll_deg() const { return roll_sensor * 0.01; }
+    float get_pitch_deg() const { return pitch_sensor * 0.01; }
+    float get_yaw_deg() const { return yaw_sensor * 0.01; }
 
     // helper trig value accessors
     float cos_roll() const  {
@@ -580,6 +585,16 @@ class AP_AHRS {
         return _sin_yaw;
     }
 
+    float roll_deg() const {
+        return roll_sensor * 0.01;
+    }
+    float pitch_deg() const {
+        return pitch_sensor * 0.01;
+    }
+    float yaw_deg() const {
+        return yaw_sensor * 0.01;
+    }
+
     // integer Euler angles (Degrees * 100)
     int32_t roll_sensor;
     int32_t pitch_sensor;

libraries/AP_Compass/AP_Compass_Calibration.cpp

@@ -538,7 +538,7 @@ bool Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask,
     field = R * field;
 
     Matrix3f dcm;
-    dcm.from_euler(AP::ahrs().get_roll(), AP::ahrs().get_pitch(), radians(yaw_deg));
+    dcm.from_euler(AP::ahrs().get_roll_rad(), AP::ahrs().get_pitch_rad(), radians(yaw_deg));
 
     // Rotate into body frame using provided yaw
     field = dcm.transposed() * field;

libraries/AP_Compass/Compass_learn.cpp

@@ -43,7 +43,7 @@ void CompassLearn::update(void)
         // no GSF available
         return;
     }
-    if (degrees(fabsf(ahrs.get_pitch())) > 50) {
+    if (degrees(fabsf(ahrs.get_pitch_rad())) > 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()))*100.0f;
+    uint16_t yaw_cd = (uint16_t)(360.0f - degrees(AP::ahrs().get_yaw_rad()))*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_InertialSensor/AP_InertialSensor.cpp

@@ -1361,7 +1361,7 @@ bool AP_InertialSensor::_calculate_trim(const Vector3f &accel_sample, Vector3f &
     if (view != nullptr) {
         // Use pitch to guess which axis the user is trying to trim
         // 5 deg buffer to favor normal AHRS and avoid floating point funny business
-        if (fabsf(view->pitch) < (fabsf(AP::ahrs().get_pitch())+radians(5)) ) {
+        if (fabsf(view->pitch) < (fabsf(AP::ahrs().get_pitch_rad())+radians(5)) ) {
             // user is trying to calibrate view
             rotation = view->get_rotation();
             if (!is_zero(view->get_pitch_trim())) {