From 549dd3875d58c0e74c975cdf21c337437b805802 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 12 Feb 2023 13:33:27 +0000 Subject: [PATCH] Plane: add mode ahrs convenience refence --- ArduPlane/mode.cpp | 7 ++++--- ArduPlane/mode.h | 2 ++ ArduPlane/mode_acro.cpp | 6 +++--- ArduPlane/mode_auto.cpp | 4 ++-- ArduPlane/mode_guided.cpp | 2 +- ArduPlane/mode_loiter.cpp | 2 +- ArduPlane/mode_manual.cpp | 4 ++-- ArduPlane/mode_qacro.cpp | 2 +- ArduPlane/mode_qloiter.cpp | 2 +- ArduPlane/mode_takeoff.cpp | 4 ++-- ArduPlane/mode_training.cpp | 8 ++++---- 11 files changed, 23 insertions(+), 20 deletions(-) diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index fcaafa1399..63559a50dd 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -1,8 +1,9 @@ #include "Plane.h" -Mode::Mode() +Mode::Mode() : + ahrs(plane.ahrs) #if HAL_QUADPLANE_ENABLED - : quadplane(plane.quadplane), + , quadplane(plane.quadplane), pos_control(plane.quadplane.pos_control), attitude_control(plane.quadplane.attitude_control), loiter_nav(plane.quadplane.loiter_nav), @@ -64,7 +65,7 @@ bool Mode::enter() // zero initial pitch and highest airspeed on mode change plane.auto_state.highest_airspeed = 0; - plane.auto_state.initial_pitch_cd = plane.ahrs.pitch_sensor; + plane.auto_state.initial_pitch_cd = ahrs.pitch_sensor; // disable taildrag takeoff on mode change plane.auto_state.fbwa_tdrag_takeoff_mode = false; diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 527bb1fb13..c6a0681ec0 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -7,6 +7,7 @@ #include #include #include "quadplane.h" +#include class AC_PosControl; class AC_AttitudeControl_Multi; @@ -143,6 +144,7 @@ protected: QuadPlane& quadplane; QuadPlane::PosControlState &poscontrol; #endif + AP_AHRS& ahrs; }; diff --git a/ArduPlane/mode_acro.cpp b/ArduPlane/mode_acro.cpp index 71f10dee1e..0f7e67b4e4 100644 --- a/ArduPlane/mode_acro.cpp +++ b/ArduPlane/mode_acro.cpp @@ -5,7 +5,7 @@ bool ModeAcro::_enter() { plane.acro_state.locked_roll = false; plane.acro_state.locked_pitch = false; - IGNORE_RETURN(plane.ahrs.get_quaternion(plane.acro_state.q)); + IGNORE_RETURN(ahrs.get_quaternion(plane.acro_state.q)); return true; } @@ -15,12 +15,12 @@ void ModeAcro::update() if (plane.acro_state.locked_roll) { plane.nav_roll_cd = plane.acro_state.locked_roll_err; } else { - plane.nav_roll_cd = plane.ahrs.roll_sensor; + plane.nav_roll_cd = ahrs.roll_sensor; } if (plane.acro_state.locked_pitch) { plane.nav_pitch_cd = plane.acro_state.locked_pitch_cd; } else { - plane.nav_pitch_cd = plane.ahrs.pitch_sensor; + plane.nav_pitch_cd = ahrs.pitch_sensor; } } diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp index 706ed76f67..548dd0b261 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -100,8 +100,8 @@ void ModeAuto::update() #if AP_SCRIPTING_ENABLED } else if (nav_cmd_id == MAV_CMD_NAV_SCRIPT_TIME) { // NAV_SCRIPTING has a desired roll and pitch rate and desired throttle - plane.nav_roll_cd = plane.ahrs.roll_sensor; - plane.nav_pitch_cd = plane.ahrs.pitch_sensor; + plane.nav_roll_cd = ahrs.roll_sensor; + plane.nav_pitch_cd = ahrs.pitch_sensor; SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.nav_scripting.throttle_pct); #endif } else { diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index 91aa272fc1..900b084f8c 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -15,7 +15,7 @@ bool ModeGuided::_enter() /* if using Q_GUIDED_MODE then project forward by the stopping distance */ - loc.offset_bearing(degrees(plane.ahrs.groundspeed_vector().angle()), + loc.offset_bearing(degrees(ahrs.groundspeed_vector().angle()), plane.quadplane.stopping_distance()); } #endif diff --git a/ArduPlane/mode_loiter.cpp b/ArduPlane/mode_loiter.cpp index 8896dd98f3..e7b07c9db1 100644 --- a/ArduPlane/mode_loiter.cpp +++ b/ArduPlane/mode_loiter.cpp @@ -63,7 +63,7 @@ bool ModeLoiter::isHeadingLinedUp_cd(const int32_t bearing_cd) // Tolerance is initially 10 degrees and grows at 10 degrees for each loiter circle completed. // get current heading. - const int32_t heading_cd = (wrap_360(degrees(plane.ahrs.groundspeed_vector().angle())))*100; + const int32_t heading_cd = (wrap_360(degrees(ahrs.groundspeed_vector().angle())))*100; const int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd); diff --git a/ArduPlane/mode_manual.cpp b/ArduPlane/mode_manual.cpp index 854b1fd4cd..68535f9618 100644 --- a/ArduPlane/mode_manual.cpp +++ b/ArduPlane/mode_manual.cpp @@ -7,7 +7,7 @@ void ModeManual::update() SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitch_in_expo(false)); plane.steering_control.steering = plane.steering_control.rudder = plane.rudder_in_expo(false); - plane.nav_roll_cd = plane.ahrs.roll_sensor; - plane.nav_pitch_cd = plane.ahrs.pitch_sensor; + plane.nav_roll_cd = ahrs.roll_sensor; + plane.nav_pitch_cd = ahrs.pitch_sensor; } diff --git a/ArduPlane/mode_qacro.cpp b/ArduPlane/mode_qacro.cpp index e008537f34..c4f5f6a934 100644 --- a/ArduPlane/mode_qacro.cpp +++ b/ArduPlane/mode_qacro.cpp @@ -12,7 +12,7 @@ bool ModeQAcro::_enter() // disable yaw rate time contant to mantain old behaviour quadplane.disable_yaw_rate_time_constant(); - IGNORE_RETURN(plane.ahrs.get_quaternion(plane.acro_state.q)); + IGNORE_RETURN(ahrs.get_quaternion(plane.acro_state.q)); return true; } diff --git a/ArduPlane/mode_qloiter.cpp b/ArduPlane/mode_qloiter.cpp index 0b3aa6dfa6..26ef2bf214 100644 --- a/ArduPlane/mode_qloiter.cpp +++ b/ArduPlane/mode_qloiter.cpp @@ -100,7 +100,7 @@ void ModeQLoiter::run() float descent_rate_cms = quadplane.landing_descent_rate_cms(height_above_ground); if (poscontrol.get_state() == QuadPlane::QPOS_LAND_FINAL && !quadplane.option_is_set(QuadPlane::OPTION::DISABLE_GROUND_EFFECT_COMP)) { - quadplane.ahrs.set_touchdown_expected(true); + ahrs.set_touchdown_expected(true); } pos_control->land_at_climb_rate_cm(-descent_rate_cms, descent_rate_cms>0); diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index f65394c4e3..caeb0a73da 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -61,7 +61,7 @@ void ModeTakeoff::update() { if (!takeoff_started) { // see if we will skip takeoff as already flying - if (plane.is_flying() && (millis() - plane.started_flying_ms > 10000U) && plane.ahrs.groundspeed() > 3) { + if (plane.is_flying() && (millis() - plane.started_flying_ms > 10000U) && ahrs.groundspeed() > 3) { gcs().send_text(MAV_SEVERITY_INFO, "Takeoff skipped - circling"); plane.prev_WP_loc = plane.current_loc; plane.next_WP_loc = plane.current_loc; @@ -75,7 +75,7 @@ void ModeTakeoff::update() // takeoff point, at a height of TKOFF_ALT const float dist = target_dist; const float alt = target_alt; - const float direction = degrees(plane.ahrs.yaw); + const float direction = degrees(ahrs.yaw); start_loc = plane.current_loc; plane.prev_WP_loc = plane.current_loc; diff --git a/ArduPlane/mode_training.cpp b/ArduPlane/mode_training.cpp index 6209aae1ec..2c73c92d19 100644 --- a/ArduPlane/mode_training.cpp +++ b/ArduPlane/mode_training.cpp @@ -9,9 +9,9 @@ void ModeTraining::update() // if the roll is past the set roll limit, then // we set target roll to the limit - if (plane.ahrs.roll_sensor >= plane.roll_limit_cd) { + if (ahrs.roll_sensor >= plane.roll_limit_cd) { plane.nav_roll_cd = plane.roll_limit_cd; - } else if (plane.ahrs.roll_sensor <= -plane.roll_limit_cd) { + } else if (ahrs.roll_sensor <= -plane.roll_limit_cd) { plane.nav_roll_cd = -plane.roll_limit_cd; } else { plane.training_manual_roll = true; @@ -20,9 +20,9 @@ void ModeTraining::update() // if the pitch is past the set pitch limits, then // we set target pitch to the limit - if (plane.ahrs.pitch_sensor >= plane.aparm.pitch_limit_max_cd) { + if (ahrs.pitch_sensor >= plane.aparm.pitch_limit_max_cd) { plane.nav_pitch_cd = plane.aparm.pitch_limit_max_cd; - } else if (plane.ahrs.pitch_sensor <= plane.pitch_limit_min_cd) { + } else if (ahrs.pitch_sensor <= plane.pitch_limit_min_cd) { plane.nav_pitch_cd = plane.pitch_limit_min_cd; } else { plane.training_manual_pitch = true;