From b4c79c91056dc67c096647eecb7c6e13e2bfb6df Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Tue, 25 Apr 2023 22:27:13 -0500 Subject: [PATCH] Plane:add FlightOptions helper --- ArduPlane/AP_Arming.cpp | 2 +- ArduPlane/ArduPlane.cpp | 12 +++++++++--- ArduPlane/Attitude.cpp | 8 ++++---- ArduPlane/GCS_Mavlink.cpp | 2 +- ArduPlane/Plane.h | 1 + ArduPlane/mode_acro.cpp | 2 +- ArduPlane/mode_loiter.cpp | 8 ++++---- ArduPlane/mode_rtl.cpp | 2 +- ArduPlane/navigation.cpp | 4 ++-- ArduPlane/radio.cpp | 6 +++--- ArduPlane/reverse_thrust.cpp | 2 +- ArduPlane/servos.cpp | 2 +- ArduPlane/takeoff.cpp | 2 +- 13 files changed, 30 insertions(+), 23 deletions(-) diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index e9e0005f90..3848fbe814 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -101,7 +101,7 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure) ret = false; } - if (plane.g2.flight_options & FlightOptions::CENTER_THROTTLE_TRIM){ + if (plane.flight_option_enabled(FlightOptions::CENTER_THROTTLE_TRIM)){ int16_t trim = plane.channel_throttle->get_radio_trim(); if (trim < 1250 || trim > 1750) { check_failed(display_failure, "Throttle trim not near center stick(%u)",trim ); diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 16b7fcfb94..4a70c50c22 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -303,7 +303,7 @@ void Plane::one_second_loop() adsb.set_max_speed(aparm.airspeed_max); #endif - if (g2.flight_options & FlightOptions::ENABLE_DEFAULT_AIRSPEED) { + if (flight_option_enabled(FlightOptions::ENABLE_DEFAULT_AIRSPEED)) { // use average of min and max airspeed as default airspeed fusion with high variance ahrs.writeDefaultAirSpeed((float)((aparm.airspeed_min + aparm.airspeed_max)/2), (float)((aparm.airspeed_max - aparm.airspeed_min)/2)); @@ -547,7 +547,7 @@ void Plane::update_alt() tecs_target_alt_cm = relative_target_altitude_cm(); - if (control_mode == &mode_rtl && !rtl.done_climb && (g2.rtl_climb_min > 0 || (plane.g2.flight_options & FlightOptions::CLIMB_BEFORE_TURN))) { + if (control_mode == &mode_rtl && !rtl.done_climb && (g2.rtl_climb_min > 0 || (plane.flight_option_enabled(FlightOptions::CLIMB_BEFORE_TURN)))) { // ensure we do the initial climb in RTL. We add an extra // 10m in the demanded height to push TECS to climb // quickly @@ -864,7 +864,7 @@ void Plane::get_osd_roll_pitch_rad(float &roll, float &pitch) const #endif pitch = ahrs.pitch; roll = ahrs.roll; - if (!(g2.flight_options & FlightOptions::OSD_REMOVE_TRIM_PITCH_CD)) { // correct for TRIM_PITCH_CD + if (!(flight_option_enabled(FlightOptions::OSD_REMOVE_TRIM_PITCH_CD))) { // correct for TRIM_PITCH_CD pitch -= g.pitch_trim_cd * 0.01 * DEG_TO_RAD; } } @@ -881,4 +881,10 @@ void Plane::update_current_loc(void) relative_altitude *= -1.0f; } +// check if FLIGHT_OPTION is enabled +bool Plane::flight_option_enabled(FlightOptions flight_option) const +{ + return g2.flight_options & flight_option; +} + AP_HAL_MAIN_CALLBACKS(&plane); diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index ec1d47ce26..0134a187bf 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -50,7 +50,7 @@ float Plane::calc_speed_scaler(void) speed_scaler = 1; } if (!plane.ahrs.airspeed_sensor_enabled() && - (plane.g2.flight_options & FlightOptions::SURPRESS_TKOFF_SCALING) && + (plane.flight_option_enabled(FlightOptions::SURPRESS_TKOFF_SCALING)) && (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF)) { //scaling is surpressed during climb phase of automatic takeoffs with no airspeed sensor being used due to problems with inaccurate airspeed estimates return MIN(speed_scaler, 1.0f) ; } @@ -145,7 +145,7 @@ float Plane::stabilize_roll_get_roll_out() disable_integrator = true; } return rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor, speed_scaler, disable_integrator, - ground_mode && !(plane.g2.flight_options & FlightOptions::DISABLE_GROUND_PID_SUPPRESSION)); + ground_mode && !(plane.flight_option_enabled(FlightOptions::DISABLE_GROUND_PID_SUPPRESSION))); } /* @@ -208,7 +208,7 @@ float Plane::stabilize_pitch_get_pitch_out() } return pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, speed_scaler, disable_integrator, - ground_mode && !(plane.g2.flight_options & FlightOptions::DISABLE_GROUND_PID_SUPPRESSION)); + ground_mode && !(plane.flight_option_enabled(FlightOptions::DISABLE_GROUND_PID_SUPPRESSION))); } /* @@ -274,7 +274,7 @@ void Plane::stabilize_stick_mixing_fbw() nav_roll_cd += roll_input * roll_limit_cd; nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); - if ((control_mode == &mode_loiter) && (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL)) { + if ((control_mode == &mode_loiter) && (plane.flight_option_enabled(FlightOptions::ENABLE_LOITER_ALT_CONTROL))) { // loiter is using altitude control based on the pitch stick, don't use it again here return; } diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 19bc5f2ab2..1a32d712d4 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -133,7 +133,7 @@ void GCS_MAVLINK_Plane::send_attitude() const float p = ahrs.pitch; float y = ahrs.yaw; - if (!(plane.g2.flight_options & FlightOptions::GCS_REMOVE_TRIM_PITCH_CD)) { + if (!(plane.flight_option_enabled(FlightOptions::GCS_REMOVE_TRIM_PITCH_CD))) { p -= radians(plane.g.pitch_trim_cd * 0.01f); } diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 48f6ca5f24..54e2c05072 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1027,6 +1027,7 @@ private: void update_fly_forward(void); void update_flight_stage(); void set_flight_stage(AP_FixedWing::FlightStage fs); + bool flight_option_enabled(FlightOptions flight_option) const; // navigation.cpp void loiter_angle_reset(void); diff --git a/ArduPlane/mode_acro.cpp b/ArduPlane/mode_acro.cpp index 5bfeea2f81..e30fc50483 100644 --- a/ArduPlane/mode_acro.cpp +++ b/ArduPlane/mode_acro.cpp @@ -111,7 +111,7 @@ void ModeAcro::stabilize() const float rudd_expo = plane.rudder_in_expo(true); const float yaw_rate = (rudd_expo/SERVO_MAX) * plane.g.acro_yaw_rate; plane.steering_control.steering = plane.steering_control.rudder = plane.yawController.get_rate_out(yaw_rate, speed_scaler, false); - } else if (plane.g2.flight_options & FlightOptions::ACRO_YAW_DAMPER) { + } else if (plane.flight_option_enabled(FlightOptions::ACRO_YAW_DAMPER)) { // use yaw controller plane.calc_nav_yaw_coordinated(); } else { diff --git a/ArduPlane/mode_loiter.cpp b/ArduPlane/mode_loiter.cpp index e7b07c9db1..2ee6ba24b4 100644 --- a/ArduPlane/mode_loiter.cpp +++ b/ArduPlane/mode_loiter.cpp @@ -9,7 +9,7 @@ bool ModeLoiter::_enter() // make sure the local target altitude is the same as the nav target used for loiter nav // this allows us to do FBWB style stick control /*IGNORE_RETURN(plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, plane.target_altitude.amsl_cm));*/ - if (plane.stick_mixing_enabled() && (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL)) { + if (plane.stick_mixing_enabled() && (plane.flight_option_enabled(FlightOptions::ENABLE_LOITER_ALT_CONTROL))) { plane.set_target_altitude_current(); } @@ -21,7 +21,7 @@ bool ModeLoiter::_enter() void ModeLoiter::update() { plane.calc_nav_roll(); - if (plane.stick_mixing_enabled() && (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL)) { + if (plane.stick_mixing_enabled() && plane.flight_option_enabled(FlightOptions::ENABLE_LOITER_ALT_CONTROL)) { plane.update_fbwb_speed_height(); } else { plane.calc_nav_pitch(); @@ -94,7 +94,7 @@ bool ModeLoiter::isHeadingLinedUp_cd(const int32_t bearing_cd) void ModeLoiter::navigate() { - if (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL) { + if (plane.flight_option_enabled(FlightOptions::ENABLE_LOITER_ALT_CONTROL)) { // update the WP alt from the global target adjusted by update_fbwb_speed_height plane.next_WP_loc.set_alt_cm(plane.target_altitude.amsl_cm, Location::AltFrame::ABSOLUTE); } @@ -112,7 +112,7 @@ void ModeLoiter::navigate() void ModeLoiter::update_target_altitude() { - if (plane.stick_mixing_enabled() && (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL)) { + if (plane.stick_mixing_enabled() && (plane.flight_option_enabled(FlightOptions::ENABLE_LOITER_ALT_CONTROL))) { return; } Mode::update_target_altitude(); diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp index fbe368b2bc..417eb6170f 100644 --- a/ArduPlane/mode_rtl.cpp +++ b/ArduPlane/mode_rtl.cpp @@ -47,7 +47,7 @@ void ModeRTL::update() plane.calc_throttle(); bool alt_threshold_reached = false; - if (plane.g2.flight_options & FlightOptions::CLIMB_BEFORE_TURN) { + if (plane.flight_option_enabled(FlightOptions::CLIMB_BEFORE_TURN)) { // Climb to ALT_HOLD_RTL before turning. This overrides RTL_CLIMB_MIN. alt_threshold_reached = plane.current_loc.alt > plane.next_WP_loc.alt; } else if (plane.g2.rtl_climb_min > 0) { diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 16ceb26035..b5e2e3ff2a 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -157,9 +157,9 @@ void Plane::calc_airspeed_errors() // FBW_B/cruise airspeed target if (!failsafe.rc_failsafe && (control_mode == &mode_fbwb || control_mode == &mode_cruise)) { - if (g2.flight_options & FlightOptions::CRUISE_TRIM_AIRSPEED) { + if (flight_option_enabled(FlightOptions::CRUISE_TRIM_AIRSPEED)) { target_airspeed_cm = aparm.airspeed_cruise_cm; - } else if (g2.flight_options & FlightOptions::CRUISE_TRIM_THROTTLE) { + } else if (flight_option_enabled(FlightOptions::CRUISE_TRIM_THROTTLE)) { float control_min = 0.0f; float control_mid = 0.0f; const float control_max = channel_throttle->get_range(); diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 8e48b0ea4c..e4802bfe83 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -222,7 +222,7 @@ int16_t Plane::rudder_input(void) return 0; } - if ((g2.flight_options & FlightOptions::DIRECT_RUDDER_ONLY) && + if ((flight_option_enabled(FlightOptions::DIRECT_RUDDER_ONLY)) && !(control_mode == &mode_manual || control_mode == &mode_stabilize || control_mode == &mode_acro)) { // the user does not want any input except in these modes return 0; @@ -436,8 +436,8 @@ bool Plane::throttle_at_zero(void) const /* true if throttle stick is at idle position...if throttle trim has been moved to center stick area in conjunction with sprung throttle, cannot use in_trim, must use rc_min */ - if (((!(g2.flight_options & FlightOptions::CENTER_THROTTLE_TRIM) && channel_throttle->in_trim_dz()) || - (g2.flight_options & FlightOptions::CENTER_THROTTLE_TRIM && channel_throttle->in_min_dz()))) { + if (((!(flight_option_enabled(FlightOptions::CENTER_THROTTLE_TRIM) && channel_throttle->in_trim_dz())) || + (flight_option_enabled(FlightOptions::CENTER_THROTTLE_TRIM)&& channel_throttle->in_min_dz()))) { return true; } return false; diff --git a/ArduPlane/reverse_thrust.cpp b/ArduPlane/reverse_thrust.cpp index 04033d00f6..f64f37b276 100644 --- a/ArduPlane/reverse_thrust.cpp +++ b/ArduPlane/reverse_thrust.cpp @@ -144,7 +144,7 @@ float Plane::get_throttle_input(bool no_deadzone) const float Plane::get_adjusted_throttle_input(bool no_deadzone) const { if ((plane.channel_throttle->get_type() != RC_Channel::ControlType::RANGE) || - (g2.flight_options & FlightOptions::CENTER_THROTTLE_TRIM) == 0) { + (flight_option_enabled(FlightOptions::CENTER_THROTTLE_TRIM)) == 0) { return get_throttle_input(no_deadzone); } float ret = channel_throttle->get_range() * throttle_curve(aparm.throttle_cruise * 0.01, 0, 0.5 + 0.5*channel_throttle->norm_input()); diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index b924febd23..1b3c7f6a30 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -1017,7 +1017,7 @@ void Plane::servos_output(void) landing_neutral_control_surface_servos(); // set rudder arm waiting for neutral control throws (rudder neutral, aileron/rt vtail/rt elevon to full right) - if (g2.flight_options & FlightOptions::INDICATE_WAITING_FOR_RUDDER_NEUTRAL) { + if (flight_option_enabled(FlightOptions::INDICATE_WAITING_FOR_RUDDER_NEUTRAL)) { indicate_waiting_for_rud_neutral_to_takeoff(); } diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index a298b870e1..2b316fe8e0 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -54,7 +54,7 @@ bool Plane::auto_takeoff_check(void) return false; } - bool do_takeoff_attitude_check = !(g2.flight_options & FlightOptions::DISABLE_TOFF_ATTITUDE_CHK); + bool do_takeoff_attitude_check = !(flight_option_enabled(FlightOptions::DISABLE_TOFF_ATTITUDE_CHK)); #if HAL_QUADPLANE_ENABLED // disable attitude check on tailsitters do_takeoff_attitude_check = !quadplane.tailsitter.enabled();