mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Plane: ask mode if battery voltage compensation should be done
This commit is contained in:
parent
2bc5078c7a
commit
7e79a13053
@ -290,3 +290,35 @@ bool Mode::use_throttle_limits() const
|
|||||||
|
|
||||||
return true;
|
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;
|
||||||
|
}
|
||||||
|
@ -138,6 +138,9 @@ public:
|
|||||||
// true if throttle min/max limits should be applied
|
// true if throttle min/max limits should be applied
|
||||||
bool use_throttle_limits() const;
|
bool use_throttle_limits() const;
|
||||||
|
|
||||||
|
// true if voltage correction should be applied to throttle
|
||||||
|
bool use_battery_compensation() const;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|
||||||
// subclasses override this to perform checks before entering the mode
|
// subclasses override this to perform checks before entering the mode
|
||||||
|
@ -614,8 +614,9 @@ void Plane::set_throttle(void)
|
|||||||
}
|
}
|
||||||
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, fwd_thr);
|
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, fwd_thr);
|
||||||
#endif // HAL_QUADPLANE_ENABLED
|
#endif // HAL_QUADPLANE_ENABLED
|
||||||
|
}
|
||||||
|
|
||||||
} else {
|
if (control_mode->use_battery_compensation()) {
|
||||||
// Apply voltage compensation to throttle output from flight mode
|
// 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));
|
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);
|
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle);
|
||||||
|
Loading…
Reference in New Issue
Block a user