mirror of https://github.com/ArduPilot/ardupilot
AP_Motors: add spoolup block
This commit is contained in:
parent
b9280630f5
commit
ff91bbd762
|
@ -77,6 +77,11 @@ void AP_Motors::armed(bool arm)
|
|||
|
||||
void AP_Motors::set_desired_spool_state(DesiredSpoolState spool)
|
||||
{
|
||||
// check if spoolup has been blocked
|
||||
if (_spoolup_block && (spool == DesiredSpoolState::THROTTLE_UNLIMITED)) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (_armed || (spool == DesiredSpoolState::SHUT_DOWN)) {
|
||||
_spool_desired = spool;
|
||||
}
|
||||
|
|
|
@ -123,6 +123,10 @@ public:
|
|||
// get motor interlock status. true means motors run, false motors don't run
|
||||
bool get_interlock() const { return _interlock; }
|
||||
|
||||
// get/set spoolup block
|
||||
bool get_spoolup_block() const { return _spoolup_block; }
|
||||
void set_spoolup_block(bool set) { _spoolup_block = set; }
|
||||
|
||||
// set_roll, set_pitch, set_yaw, set_throttle
|
||||
void set_roll(float roll_in) { _roll_in = roll_in; }; // range -1 ~ +1
|
||||
void set_roll_ff(float roll_in) { _roll_in_ff = roll_in; }; // range -1 ~ +1
|
||||
|
@ -361,6 +365,7 @@ private:
|
|||
bool _armed; // 0 if disarmed, 1 if armed
|
||||
bool _interlock; // 1 if the motor interlock is enabled (i.e. motors run), 0 if disabled (motors don't run)
|
||||
bool _initialised_ok; // 1 if initialisation was successful
|
||||
bool _spoolup_block; // true if spoolup is blocked
|
||||
|
||||
static AP_Motors *_singleton;
|
||||
};
|
||||
|
|
Loading…
Reference in New Issue