Plane: ask mode if battery voltage compensation should be done

This commit is contained in:
Iampete1 2024-01-17 20:18:18 +00:00 committed by Andrew Tridgell
parent 2bc5078c7a
commit 7e79a13053
3 changed files with 37 additions and 1 deletions

View File

@ -290,3 +290,35 @@ bool Mode::use_throttle_limits() const
return true;
}
// true if voltage correction should be applied to throttle
bool Mode::use_battery_compensation() const
{
#if AP_SCRIPTING_ENABLED
if (plane.nav_scripting_active()) {
return false;
}
#endif
if (this == &plane.mode_stabilize ||
this == &plane.mode_training ||
this == &plane.mode_acro ||
this == &plane.mode_fbwa ||
this == &plane.mode_autotune) {
// a manual throttle mode
return false;
}
if (is_guided_mode() && plane.guided_throttle_passthru) {
// manual pass through of throttle while in GUIDED
return false;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode()) {
return false;
}
#endif
return true;
}

View File

@ -138,6 +138,9 @@ public:
// true if throttle min/max limits should be applied
bool use_throttle_limits() const;
// true if voltage correction should be applied to throttle
bool use_battery_compensation() const;
protected:
// subclasses override this to perform checks before entering the mode

View File

@ -614,8 +614,9 @@ void Plane::set_throttle(void)
}
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, fwd_thr);
#endif // HAL_QUADPLANE_ENABLED
}
} else {
if (control_mode->use_battery_compensation()) {
// Apply 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, throttle);