ArduPlane: use get_roll_deg in place of roll_sensor (etc.)

Author: peterbarker

ArduPlane/Plane.cpp

@@ -431,8 +431,8 @@ void Plane::airspeed_ratio_update(void)
         return;
     }
     if (labs(ahrs.roll_sensor) > roll_limit_cd ||
-        ahrs.pitch_sensor > aparm.pitch_limit_max*100 ||
-        ahrs.pitch_sensor < pitch_limit_min*100) {
+        ahrs.get_pitch_deg() > aparm.pitch_limit_max ||
+        ahrs.get_pitch_deg() < pitch_limit_min) {
         // don't calibrate when going beyond normal flight envelope
         return;
     }

ArduPlane/VTOL_Assist.cpp

@@ -132,8 +132,8 @@ bool VTOL_Assist::should_assist(float aspeed, bool have_airspeed)
 
         if (angle_error.update(!inside_envelope && !inside_angle_error, now_ms, tigger_delay_ms, clear_delay_ms)) {
             gcs().send_text(MAV_SEVERITY_WARNING, "Angle assist r=%d p=%d",
-                                         (int)(plane.ahrs.roll_sensor/100),
-                                         (int)(plane.ahrs.pitch_sensor/100));
+                            (int)plane.ahrs.get_roll_deg(),
+                            (int)plane.ahrs.get_pitch_deg());
         }
     }
 
@@ -194,7 +194,7 @@ bool VTOL_Assist::check_VTOL_recovery(void)
             fabsf(gyro.x) > radians(30) &&
             fabsf(gyro.y) > radians(30) &&
             gyro.x * gyro.z < 0 &&
-            plane.ahrs.pitch_sensor < -4500;
+            plane.ahrs.get_pitch_deg() < -45;
     } else {
         quadplane.in_spin_recovery = false;
     }

ArduPlane/commands_logic.cpp

@@ -500,7 +500,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/is_flying.cpp

@@ -232,8 +232,7 @@ void Plane::crash_detection_update(void)
             // Declare a crash if we are oriented more that 60deg in pitch or roll
             if (!crash_state.checkedHardLanding && // only check once
                 been_auto_flying &&
-                (labs(ahrs.roll_sensor) > 6000 || labs(ahrs.pitch_sensor) > 6000)) {
-                
+                (fabsf(ahrs.get_roll_deg()) > 60 || fabsf(ahrs.get_pitch_deg()) > 60)) {
                 crashed = true;
 
                 // did we "crash" within 75m of the landing location? Probably just a hard landing

ArduPlane/mode_autoland.cpp

@@ -301,7 +301,7 @@ bool ModeAutoLand::landing_lined_up(void)
 void ModeAutoLand::arm_check(void)
 {
     if (plane.ahrs.use_compass() && autoland_option_is_set(ModeAutoLand::AutoLandOption::AUTOLAND_DIR_ON_ARM)) {
-        set_autoland_direction(plane.ahrs.yaw_sensor * 0.01);
+        set_autoland_direction(plane.ahrs.get_yaw_deg());
     }
 }
 

ArduPlane/pullup.cpp

@@ -115,26 +115,26 @@ bool GliderPullup::verify_pullup(void)
     switch (stage) {
     case Stage::WAIT_AIRSPEED: {
         float aspeed;
-        if (ahrs.airspeed_estimate(aspeed) && (aspeed > airspeed_start || ahrs.pitch_sensor*0.01 > pitch_start)) {
+        if (ahrs.airspeed_estimate(aspeed) && (aspeed > airspeed_start || ahrs.get_pitch_deg() > pitch_start)) {
             gcs().send_text(MAV_SEVERITY_INFO, "Pullup airspeed %.1fm/s alt %.1fm AMSL", aspeed, current_loc.alt*0.01);
             stage = Stage::WAIT_PITCH;
         }
         return false;
     }
 
     case Stage::WAIT_PITCH: {
-        if (ahrs.pitch_sensor*0.01 > pitch_start && fabsf(ahrs.roll_sensor*0.01) < 90) {
+        if (ahrs.get_pitch_deg() > pitch_start && fabsf(ahrs.get_roll_deg()) < 90) {
             gcs().send_text(MAV_SEVERITY_INFO, "Pullup pitch p=%.1f r=%.1f alt %.1fm AMSL",
-                            ahrs.pitch_sensor*0.01,
-                            ahrs.roll_sensor*0.01,
+                            ahrs.get_pitch_deg(),
+                            ahrs.get_roll_deg(),
                             current_loc.alt*0.01);
             stage = Stage::WAIT_LEVEL;
         }
         return false;
     }
 
     case Stage::PUSH_NOSE_DOWN: {
-        if (fabsf(ahrs.roll_sensor*0.01) < aparm.roll_limit) {
+        if (fabsf(ahrs.get_roll_deg()) < aparm.roll_limit) {
             stage = Stage::WAIT_LEVEL;
         }
         return false;
@@ -143,21 +143,21 @@ bool GliderPullup::verify_pullup(void)
     case Stage::WAIT_LEVEL: {
         // When pitch has raised past lower limit used by speed controller, wait for airspeed to approach
         // target value before handing over control of pitch demand to speed controller
-        bool pitchup_complete = ahrs.pitch_sensor*0.01 > MIN(0, aparm.pitch_limit_min);
+        bool pitchup_complete = ahrs.get_pitch_deg() > MIN(0, aparm.pitch_limit_min);
         const float pitch_lag_time = 1.0f * sqrtf(ahrs.get_EAS2TAS());
         float aspeed;
         const float aspeed_derivative = (ahrs.get_accel().x + GRAVITY_MSS * ahrs.get_DCM_rotation_body_to_ned().c.x) / ahrs.get_EAS2TAS();
         bool airspeed_low = ahrs.airspeed_estimate(aspeed) ? (aspeed + aspeed_derivative * pitch_lag_time) < 0.01f * (float)plane.target_airspeed_cm : true;
-        bool roll_control_lost = fabsf(ahrs.roll_sensor*0.01) > aparm.roll_limit;
+        bool roll_control_lost = fabsf(ahrs.get_roll_deg()) > aparm.roll_limit;
         if (pitchup_complete && airspeed_low && !roll_control_lost) {
                 gcs().send_text(MAV_SEVERITY_INFO, "Pullup level r=%.1f p=%.1f alt %.1fm AMSL",
-                                ahrs.roll_sensor*0.01, ahrs.pitch_sensor*0.01, current_loc.alt*0.01);
+                                ahrs.get_roll_deg(), ahrs.get_pitch_deg(), current_loc.alt*0.01);
                 break;
         } else if (pitchup_complete && roll_control_lost) {
                 // push nose down and wait to get roll control back
                 gcs().send_text(MAV_SEVERITY_ALERT, "Pullup level roll bad r=%.1f p=%.1f",
-                                ahrs.roll_sensor*0.01,
-                                ahrs.pitch_sensor*0.01);
+                                ahrs.get_roll_deg(),
+                                ahrs.get_pitch_deg());
                 stage = Stage::PUSH_NOSE_DOWN;
         }
         return false;

ArduPlane/servos.cpp

@@ -114,7 +114,7 @@ bool Plane::suppress_throttle(void)
         if (is_flying() &&
             millis() - started_flying_ms > MAX(launch_duration_ms, 5000U) && // been flying >5s in any mode
             adjusted_relative_altitude_cm() > 500 && // are >5m above AGL/home
-            labs(ahrs.pitch_sensor) < 3000 && // not high pitch, which happens when held before launch
+            fabsf(ahrs.get_pitch_deg()) < 30 && // not high pitch, which happens when held before launch
             gps_movement) { // definite gps movement
             // we're already flying, do not suppress the throttle. We can get
             // stuck in this condition if we reset a mission and cmd 1 is takeoff