mirror of https://github.com/ArduPilot/ardupilot
Plane: Suppress throttle when parachute release initiated, not after release.
This commit is contained in:
parent
2a1985d0f9
commit
df922dacfa
|
@ -578,12 +578,12 @@ void Plane::flap_slew_limit(int8_t &last_value, int8_t &new_value)
|
|||
*/
|
||||
bool Plane::suppress_throttle(void)
|
||||
{
|
||||
if (auto_throttle_mode && parachute.released()) {
|
||||
// throttle always suppressed in auto-throttle modes after parachute release
|
||||
if (auto_throttle_mode && parachute.release_initiated()) {
|
||||
// throttle always suppressed in auto-throttle modes after parachute release initiated
|
||||
throttle_suppressed = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
if (!throttle_suppressed) {
|
||||
// we've previously met a condition for unsupressing the throttle
|
||||
return false;
|
||||
|
|
|
@ -86,6 +86,8 @@ void AP_Parachute::release()
|
|||
_release_time = AP_HAL::millis();
|
||||
}
|
||||
|
||||
_release_initiated = true;
|
||||
|
||||
// update AP_Notify
|
||||
AP_Notify::flags.parachute_release = 1;
|
||||
}
|
||||
|
|
|
@ -32,7 +32,8 @@ public:
|
|||
AP_Parachute(AP_Relay& relay) :
|
||||
_relay(relay),
|
||||
_release_time(0),
|
||||
_released(false)
|
||||
_released(false),
|
||||
_release_initiated(false)
|
||||
{
|
||||
// setup parameter defaults
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
|
@ -50,6 +51,9 @@ public:
|
|||
/// released - true if the parachute has been released (or release is in progress)
|
||||
bool released() const { return _released; }
|
||||
|
||||
/// release_initiated - true if the parachute release sequence has been initiated (may wait before actual release)
|
||||
bool release_initiated() const { return _release_initiated; }
|
||||
|
||||
/// update - shuts off the trigger should be called at about 10hz
|
||||
void update();
|
||||
|
||||
|
@ -71,6 +75,7 @@ private:
|
|||
// internal variables
|
||||
AP_Relay &_relay; // pointer to relay object from the base class Relay.
|
||||
uint32_t _release_time; // system time that parachute is ordered to be released (actual release will happen 0.5 seconds later)
|
||||
bool _release_initiated:1; // true if the parachute release initiated (may still be waiting for engine to be suppressed etc.)
|
||||
bool _release_in_progress:1; // true if the parachute release is in progress
|
||||
bool _released:1; // true if the parachute has been released
|
||||
bool _released:1; // true if the parachute has been released
|
||||
};
|
||||
|
|
Loading…
Reference in New Issue