diff --git a/ArduPlane/Plane.cpp b/ArduPlane/Plane.cpp index b0a284a5b14..bf90ff8a891 100644 --- a/ArduPlane/Plane.cpp +++ b/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; } diff --git a/ArduPlane/VTOL_Assist.cpp b/ArduPlane/VTOL_Assist.cpp index 1d0a9c32165..9c3f7faff57 100644 --- a/ArduPlane/VTOL_Assist.cpp +++ b/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; } diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index e9c17535a0b..c7fd0157335 100644 --- a/ArduPlane/commands_logic.cpp +++ b/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 } diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index 7118f352fe2..49a815b34d3 100644 --- a/ArduPlane/is_flying.cpp +++ b/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 diff --git a/ArduPlane/mode_autoland.cpp b/ArduPlane/mode_autoland.cpp index b8bb429211b..c79114c3c11 100644 --- a/ArduPlane/mode_autoland.cpp +++ b/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()); } } diff --git a/ArduPlane/pullup.cpp b/ArduPlane/pullup.cpp index b8cb1a7727b..70f5e7ddeed 100644 --- a/ArduPlane/pullup.cpp +++ b/ArduPlane/pullup.cpp @@ -115,7 +115,7 @@ 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; } @@ -123,10 +123,10 @@ bool GliderPullup::verify_pullup(void) } 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; } @@ -134,7 +134,7 @@ bool GliderPullup::verify_pullup(void) } 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; diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index cf1b8d1b514..fc435ba1f83 100644 --- a/ArduPlane/servos.cpp +++ b/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