From 045cde5fcf24afa79a65130386b7cdd590af782f Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 6 Jul 2023 00:01:16 +0100 Subject: [PATCH] Plane: output rudder and steering directly removing steering_control struct --- ArduPlane/Attitude.cpp | 71 ++++++++++++++++++++++++------------- ArduPlane/Plane.h | 16 ++------- ArduPlane/mode.cpp | 7 ++++ ArduPlane/mode.h | 3 ++ ArduPlane/mode_acro.cpp | 14 ++++---- ArduPlane/mode_manual.cpp | 4 +-- ArduPlane/mode_training.cpp | 5 +-- ArduPlane/servos.cpp | 22 ------------ 8 files changed, 69 insertions(+), 73 deletions(-) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 77e41ba2fe..90484121cb 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -307,38 +307,57 @@ void Plane::stabilize_stick_mixing_fbw() */ void Plane::stabilize_yaw() { + bool ground_steering = false; if (landing.is_flaring()) { // in flaring then enable ground steering - steering_control.ground_steering = true; + ground_steering = true; } else { // otherwise use ground steering when no input control and we // are below the GROUND_STEER_ALT - steering_control.ground_steering = (channel_roll->get_control_in() == 0 && + ground_steering = (channel_roll->get_control_in() == 0 && fabsf(relative_altitude) < g.ground_steer_alt); if (!landing.is_ground_steering_allowed()) { // don't use ground steering on landing approach - steering_control.ground_steering = false; + ground_steering = false; } } /* - first calculate steering_control.steering for a nose or tail + first calculate steering for a nose or tail wheel. We use "course hold" mode for the rudder when either performing a flare (when the wings are held level) or when in course hold in FBWA mode (when we are below GROUND_STEER_ALT) */ + float steering_output = 0.0; if (landing.is_flaring() || - (steer_state.hold_course_cd != -1 && steering_control.ground_steering)) { - calc_nav_yaw_course(); - } else if (steering_control.ground_steering) { - calc_nav_yaw_ground(); + (steer_state.hold_course_cd != -1 && ground_steering)) { + steering_output = calc_nav_yaw_course(); + } else if (ground_steering) { + steering_output = calc_nav_yaw_ground(); } /* - now calculate steering_control.rudder for the rudder + now calculate rudder for the rudder */ - calc_nav_yaw_coordinated(); + const float rudder_output = calc_nav_yaw_coordinated(); + + if (!ground_steering) { + // Not doing ground steering, output rudder on steering channel + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder_output); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, rudder_output); + + } else if (!SRV_Channels::function_assigned(SRV_Channel::k_steering)) { + // Ground steering active but no steering output configured, output steering on rudder channel + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, steering_output); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering_output); + + } else { + // Ground steering with both steering and rudder channels + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder_output); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering_output); + } + } /* @@ -370,15 +389,17 @@ void Plane::stabilize() const float elevator = pitchController.get_rate_out(nav_scripting.pitch_rate_dps, speed_scaler); SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator); + float rudder = 0; if (yawController.rate_control_enabled()) { - float rudder = nav_scripting.rudder_offset_pct * 45; + rudder = nav_scripting.rudder_offset_pct * 45; if (nav_scripting.run_yaw_rate_controller) { rudder += yawController.get_rate_out(nav_scripting.yaw_rate_dps, speed_scaler, false); } else { yawController.reset_I(); } - steering_control.rudder = rudder; } + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, rudder); #endif } else { plane.control_mode->run(); @@ -427,7 +448,7 @@ void Plane::calc_throttle() /* calculate yaw control for coordinated flight */ -void Plane::calc_nav_yaw_coordinated() +int16_t Plane::calc_nav_yaw_coordinated() { const float speed_scaler = get_speed_scaler(); bool disable_integrator = false; @@ -461,35 +482,35 @@ void Plane::calc_nav_yaw_coordinated() commanded_rudder += rudder_in; } - steering_control.rudder = constrain_int16(commanded_rudder, -4500, 4500); - if (!using_rate_controller) { /* When not running the yaw rate controller, we need to reset the rate */ yawController.reset_rate_PID(); } + + return constrain_int16(commanded_rudder, -4500, 4500); } /* calculate yaw control for ground steering with specific course */ -void Plane::calc_nav_yaw_course(void) +int16_t Plane::calc_nav_yaw_course(void) { // holding a specific navigation course on the ground. Used in // auto-takeoff and landing int32_t bearing_error_cd = nav_controller->bearing_error_cd(); - steering_control.steering = steerController.get_steering_out_angle_error(bearing_error_cd); + int16_t steering = steerController.get_steering_out_angle_error(bearing_error_cd); if (stick_mixing_enabled()) { - steering_control.steering = channel_rudder->stick_mixing(steering_control.steering); + steering = channel_rudder->stick_mixing(steering); } - steering_control.steering = constrain_int16(steering_control.steering, -4500, 4500); + return constrain_int16(steering, -4500, 4500); } /* calculate yaw control for ground steering */ -void Plane::calc_nav_yaw_ground(void) +int16_t Plane::calc_nav_yaw_ground(void) { if (gps.ground_speed() < 1 && is_zero(get_throttle_input()) && @@ -498,8 +519,7 @@ void Plane::calc_nav_yaw_ground(void) // manual rudder control while still steer_state.locked_course = false; steer_state.locked_course_err = 0; - steering_control.steering = rudder_input(); - return; + return rudder_input(); } // if we haven't been steering for 1s then clear locked course @@ -526,15 +546,16 @@ void Plane::calc_nav_yaw_ground(void) } } + int16_t steering; if (!steer_state.locked_course) { // use a rate controller at the pilot specified rate - steering_control.steering = steerController.get_steering_out_rate(steer_rate); + steering = steerController.get_steering_out_rate(steer_rate); } else { // use a error controller on the summed error int32_t yaw_error_cd = -ToDeg(steer_state.locked_course_err)*100; - steering_control.steering = steerController.get_steering_out_angle_error(yaw_error_cd); + steering = steerController.get_steering_out_angle_error(yaw_error_cd); } - steering_control.steering = constrain_int16(steering_control.steering, -4500, 4500); + return constrain_int16(steering, -4500, 4500); } diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 24c7d22c99..fe9e9e892b 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -211,16 +211,6 @@ private: bool training_manual_roll; // user has manual roll control bool training_manual_pitch; // user has manual pitch control - /* - keep steering and rudder control separated until we update servos, - to allow for a separate wheel servo from rudder servo - */ - struct { - bool ground_steering; // are we doing ground steering? - int16_t steering; // value for nose/tail wheel - int16_t rudder; // value for rudder - } steering_control; - // should throttle be pass-thru in guided? bool guided_throttle_passthru; @@ -871,9 +861,9 @@ private: float stabilize_pitch_get_pitch_out(); void stabilize_stick_mixing_fbw(); void stabilize_yaw(); - void calc_nav_yaw_coordinated(); - void calc_nav_yaw_course(void); - void calc_nav_yaw_ground(void); + int16_t calc_nav_yaw_coordinated(); + int16_t calc_nav_yaw_course(void); + int16_t calc_nav_yaw_ground(void); // Log.cpp uint32_t last_log_fast_ms; diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index a1fdb5ac2d..0042b3da0b 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -230,3 +230,10 @@ bool Mode::is_taking_off() const { return (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF); } + +// Helper to output to both k_rudder and k_steering servo functions +void Mode::output_rudder_and_steering(float val) +{ + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, val); + SRV_Channels::set_output_scaled(SRV_Channel::k_steering, val); +} diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index ec94ed4457..4da472cd06 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -146,6 +146,9 @@ protected: // mode specific pre-arm checks virtual bool _pre_arm_checks(size_t buflen, char *buffer) const; + // Helper to output to both k_rudder and k_steering servo functions + void output_rudder_and_steering(float val); + #if HAL_QUADPLANE_ENABLED // References for convenience, used by QModes AC_PosControl*& pos_control; diff --git a/ArduPlane/mode_acro.cpp b/ArduPlane/mode_acro.cpp index e30fc50483..acf0f49752 100644 --- a/ArduPlane/mode_acro.cpp +++ b/ArduPlane/mode_acro.cpp @@ -104,22 +104,24 @@ void ModeAcro::stabilize() SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitchController.get_rate_out(pitch_rate, speed_scaler)); } - plane.steering_control.steering = plane.rudder_input(); - + float rudder_output; if (plane.g.acro_yaw_rate > 0 && plane.yawController.rate_control_enabled()) { // user has asked for yaw rate control with yaw rate scaled by ACRO_YAW_RATE 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); + rudder_output = plane.yawController.get_rate_out(yaw_rate, speed_scaler, false); } else if (plane.flight_option_enabled(FlightOptions::ACRO_YAW_DAMPER)) { // use yaw controller - plane.calc_nav_yaw_coordinated(); + rudder_output = plane.calc_nav_yaw_coordinated(); } else { /* manual rudder */ - plane.steering_control.rudder = plane.steering_control.steering; + rudder_output = plane.rudder_input(); } + + output_rudder_and_steering(rudder_output); + } /* @@ -215,7 +217,7 @@ void ModeAcro::stabilize_quaternion() // call to rate controllers SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.rollController.get_rate_out(desired_rates.x, speed_scaler)); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitchController.get_rate_out(desired_rates.y, speed_scaler)); - plane.steering_control.steering = plane.steering_control.rudder = plane.yawController.get_rate_out(desired_rates.z, speed_scaler, false); + output_rudder_and_steering(plane.yawController.get_rate_out(desired_rates.z, speed_scaler, false)); acro_state.roll_active_last = roll_active; acro_state.pitch_active_last = pitch_active; diff --git a/ArduPlane/mode_manual.cpp b/ArduPlane/mode_manual.cpp index 4dc3d067d8..1f8fcd702c 100644 --- a/ArduPlane/mode_manual.cpp +++ b/ArduPlane/mode_manual.cpp @@ -5,9 +5,7 @@ void ModeManual::update() { SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.roll_in_expo(false)); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitch_in_expo(false)); - //rudder in sets rudder, but is also assigned to steering values used later in servos.cpp for steering - plane.steering_control.steering = plane.steering_control.rudder = plane.rudder_in_expo(false); - SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, plane.steering_control.steering); + output_rudder_and_steering(plane.rudder_in_expo(false)); float throttle = plane.get_throttle_input(true); diff --git a/ArduPlane/mode_training.cpp b/ArduPlane/mode_training.cpp index 2ee8b67272..23c81fbcc2 100644 --- a/ArduPlane/mode_training.cpp +++ b/ArduPlane/mode_training.cpp @@ -64,9 +64,6 @@ void ModeTraining::run() } // Always manual rudder control - const float pilot_rudder = plane.rudder_in_expo(false); - plane.steering_control.rudder = pilot_rudder; - plane.steering_control.steering = pilot_rudder; - SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, pilot_rudder); + output_rudder_and_steering(plane.rudder_in_expo(false)); } diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 0c01c15393..ae6f3c69c5 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -819,28 +819,6 @@ void Plane::set_servos(void) return; } - /* - see if we are doing ground steering. - */ - if (!steering_control.ground_steering) { - // we are not at an altitude for ground steering. Set the nose - // wheel to the rudder just in case the barometer has drifted - // a lot - steering_control.steering = steering_control.rudder; - } else if (!SRV_Channels::function_assigned(SRV_Channel::k_steering)) { - // we are within the ground steering altitude but don't have a - // dedicated steering channel. Set the rudder to the ground - // steering output - steering_control.rudder = steering_control.steering; - } - - // clear ground_steering to ensure manual control if the yaw stabilizer doesn't run - steering_control.ground_steering = false; - - - SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, steering_control.rudder); - SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering_control.steering); - if (control_mode != &mode_manual) { set_servos_controlled(); }