diff --git a/src/Vehicle/Actuators/MotorAssignment.cc b/src/Vehicle/Actuators/MotorAssignment.cc index 7682ef8..b7e087a 100644 --- a/src/Vehicle/Actuators/MotorAssignment.cc +++ b/src/Vehicle/Actuators/MotorAssignment.cc @@ -17,7 +17,7 @@ MotorAssignment::MotorAssignment(QObject* parent, Vehicle* vehicle, QmlObjectListModel* actuators) : QObject(parent), _vehicle(vehicle), _actuators(actuators) { - _spinTimer.setInterval(1000); + _spinTimer.setInterval(_spinTimeoutDefaultSec); _spinTimer.setSingleShot(true); connect(&_spinTimer, &QTimer::timeout, this, &MotorAssignment::spinTimeout); } @@ -141,6 +141,7 @@ void MotorAssignment::start() } _state = State::Running; emit activeChanged(); + _spinTimer.setInterval(_assignMotors ? _spinTimeoutHighSec : _spinTimeoutDefaultSec); _spinTimer.start(); } @@ -169,6 +170,7 @@ void MotorAssignment::selectMotor(int motorIndex) emit activeChanged(); } else { // spin the next motor after some time + _spinTimer.setInterval(_spinTimeoutDefaultSec); _spinTimer.start(); } } diff --git a/src/Vehicle/Actuators/MotorAssignment.h b/src/Vehicle/Actuators/MotorAssignment.h index 6613889..1fea827 100644 --- a/src/Vehicle/Actuators/MotorAssignment.h +++ b/src/Vehicle/Actuators/MotorAssignment.h @@ -59,6 +59,9 @@ private slots: void spinTimeout(); private: + static constexpr int _spinTimeoutDefaultSec = 1000; + static constexpr int _spinTimeoutHighSec = 3000; ///< wait a bit longer after assigning motors, so ESCs can initialize + static void ackHandlerEntry(void* resultHandlerData, int compId, MAV_RESULT commandResult, uint8_t progress, Vehicle::MavCmdResultFailureCode_t failureCode); void ackHandler(MAV_RESULT commandResult, Vehicle::MavCmdResultFailureCode_t failureCode);