From ab0755d0d8ec55d23816b14b09d875c33ecc5774 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 16 Dec 2023 17:23:56 +0000 Subject: [PATCH] Plane: rework forward throttle votlage compensation into sub class and split min/max from throttle --- ArduPlane/Parameters.cpp | 6 ++--- ArduPlane/Parameters.h | 23 ++++++++++++++--- ArduPlane/Plane.h | 1 - ArduPlane/servos.cpp | 54 ++++++++++++++++++++++++++++++---------- 4 files changed, 64 insertions(+), 20 deletions(-) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index bb4a2a86a5..fd5288223d 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1175,7 +1175,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Units: V // @Increment: 0.1 // @User: Advanced - AP_GROUPINFO("FWD_BAT_VOLT_MAX", 23, ParametersG2, fwd_thr_batt_voltage_max, 0.0f), + AP_GROUPINFO("FWD_BAT_VOLT_MAX", 23, ParametersG2, fwd_batt_cmp.batt_voltage_max, 0.0f), // @Param: FWD_BAT_VOLT_MIN // @DisplayName: Forward throttle battery voltage compensation minimum voltage @@ -1184,14 +1184,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Units: V // @Increment: 0.1 // @User: Advanced - AP_GROUPINFO("FWD_BAT_VOLT_MIN", 24, ParametersG2, fwd_thr_batt_voltage_min, 0.0f), + AP_GROUPINFO("FWD_BAT_VOLT_MIN", 24, ParametersG2, fwd_batt_cmp.batt_voltage_min, 0.0f), // @Param: FWD_BAT_IDX // @DisplayName: Forward throttle battery compensation index // @Description: Which battery monitor should be used for doing compensation for the forward throttle // @Values: 0:First battery, 1:Second battery // @User: Advanced - AP_GROUPINFO("FWD_BAT_IDX", 25, ParametersG2, fwd_thr_batt_idx, 0), + AP_GROUPINFO("FWD_BAT_IDX", 25, ParametersG2, fwd_batt_cmp.batt_idx, 0), // @Param: FS_EKF_THRESH // @DisplayName: EKF failsafe variance threshold diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 2a9a5ef071..ef126900df 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -542,9 +542,26 @@ public: AP_Int8 crow_flap_aileron_matching; // Forward throttle battery voltage compensation - AP_Float fwd_thr_batt_voltage_max; - AP_Float fwd_thr_batt_voltage_min; - AP_Int8 fwd_thr_batt_idx; + class FWD_BATT_CMP { + public: + // Calculate the throttle scale to compensate for battery voltage drop + void update(); + + // Apply throttle scale to min and max limits + void apply_min_max(int8_t &min_throttle, int8_t &max_throttle) const; + + // Apply throttle scale to throttle demand + float apply_throttle(float throttle) const; + + AP_Float batt_voltage_max; + AP_Float batt_voltage_min; + AP_Int8 batt_idx; + + private: + bool enabled; + float ratio; + } fwd_batt_cmp; + #if OFFBOARD_GUIDED == ENABLED // guided yaw heading PID diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index b0baf16706..8dcb90e47d 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1107,7 +1107,6 @@ private: void servos_auto_trim(void); void servos_twin_engine_mix(); void force_flare(); - void throttle_voltage_comp(int8_t &min_throttle, int8_t &max_throttle) const; void throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle); void throttle_slew_limit(SRV_Channel::Aux_servo_function_t func); bool suppress_throttle(void); diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 35dc8b0801..c8f03bb896 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -384,23 +384,26 @@ void Plane::set_servos_idle(void) /* - Scale the throttle to conpensate for battery voltage drop + Calculate the throttle scale to compensate for battery voltage drop */ -void Plane::throttle_voltage_comp(int8_t &min_throttle, int8_t &max_throttle) const +void ParametersG2::FWD_BATT_CMP::update() { + // Assume disabled + enabled = false; + // return if not enabled, or setup incorrectly - if (!is_positive(g2.fwd_thr_batt_voltage_min) || g2.fwd_thr_batt_voltage_min >= g2.fwd_thr_batt_voltage_max) { + if (!is_positive(batt_voltage_min) || batt_voltage_min >= batt_voltage_max) { return; } - float batt_voltage_resting_estimate = AP::battery().voltage_resting_estimate(g2.fwd_thr_batt_idx); + float batt_voltage_resting_estimate = AP::battery().voltage_resting_estimate(batt_idx); // Return for a very low battery - if (batt_voltage_resting_estimate < 0.25f * g2.fwd_thr_batt_voltage_min) { + if (batt_voltage_resting_estimate < 0.25f * batt_voltage_min) { return; } // constrain read voltage to min and max params - batt_voltage_resting_estimate = constrain_float(batt_voltage_resting_estimate,g2.fwd_thr_batt_voltage_min,g2.fwd_thr_batt_voltage_max); + batt_voltage_resting_estimate = constrain_float(batt_voltage_resting_estimate, batt_voltage_min, batt_voltage_max); // don't apply compensation if the voltage is excessively low if (batt_voltage_resting_estimate < 1) { @@ -409,14 +412,38 @@ void Plane::throttle_voltage_comp(int8_t &min_throttle, int8_t &max_throttle) co // Scale the throttle up to compensate for voltage drop // Ratio = 1 when voltage = voltage max, ratio increases as voltage drops - const float ratio = g2.fwd_thr_batt_voltage_max / batt_voltage_resting_estimate; + ratio = batt_voltage_max / batt_voltage_resting_estimate; + + // Got this far then ratio is valid + enabled = true; +} + +// Apply throttle scale to min and max limits +void ParametersG2::FWD_BATT_CMP::apply_min_max(int8_t &min_throttle, int8_t &max_throttle) const +{ + // return if not enabled + if (!enabled) { + return; + } // Scale the throttle limits to prevent subsequent clipping + // Ratio will always be >= 1, ensure still within max limits min_throttle = int8_t(MAX((ratio * (float)min_throttle), -100)); max_throttle = int8_t(MIN((ratio * (float)max_throttle), 100)); - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, - constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * ratio, -100, 100)); +} + +// Apply throttle scale to throttle demand +float ParametersG2::FWD_BATT_CMP::apply_throttle(float throttle) const +{ + // return if not enabled + if (!enabled) { + return throttle; + } + + // Ratio will always be >= 1, ensure still within max limits + return constrain_float(throttle * ratio, -100, 100); + } /* @@ -504,7 +531,8 @@ void Plane::set_throttle(void) } // compensate for battery voltage drop - throttle_voltage_comp(min_throttle, max_throttle); + g2.fwd_batt_cmp.update(); + g2.fwd_batt_cmp.apply_min_max(min_throttle, max_throttle); #if AP_BATTERY_WATT_MAX_ENABLED // apply watt limiter @@ -573,9 +601,9 @@ void Plane::set_throttle(void) #endif // HAL_QUADPLANE_ENABLED } else { - // Apply min/max limits to throttle output from flight mode - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, - constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), min_throttle, max_throttle)); + // Apply min/max limits and voltage compensation to throttle output from flight mode + const float throttle = g2.fwd_batt_cmp.apply_throttle(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_float(throttle, min_throttle, max_throttle)); } }