Browse Source

Don't retry arming. Remove double error popups for arming errors. (#9198)

QGC4.4
Don Gagne 5 years ago committed by GitHub
parent
commit
e34bed37ab
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
  1. 25
      src/FirmwarePlugin/FirmwarePlugin.cc
  2. 2
      src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
  3. 6
      src/Joystick/Joystick.cc
  4. 4
      src/Vehicle/Vehicle.cc
  5. 7
      src/Vehicle/Vehicle.h
  6. 2
      src/qgcunittest/MavlinkLogTest.cc

25
src/FirmwarePlugin/FirmwarePlugin.cc

@ -841,27 +841,22 @@ bool FirmwarePlugin::_armVehicleAndValidate(Vehicle* vehicle) @@ -841,27 +841,22 @@ bool FirmwarePlugin::_armVehicleAndValidate(Vehicle* vehicle)
return true;
}
bool armedChanged = false;
bool vehicleArmed = false;
// We try arming 3 times
for (int retries=0; retries<3; retries++) {
vehicle->setArmed(true);
// Only try arming the vehicle a single time. Doing retries on arming with a delay can lead to safety issues.
vehicle->setArmed(true, false /* showError */);
// Wait for vehicle to return armed state for 3 seconds
for (int i=0; i<30; i++) {
if (vehicle->armed()) {
armedChanged = true;
break;
}
QGC::SLEEP::msleep(100);
qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
}
if (armedChanged) {
// Wait 1000 msecs for vehicle to arm
for (int i=0; i<10; i++) {
if (vehicle->armed()) {
vehicleArmed = true;
break;
}
QGC::SLEEP::msleep(100);
qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
}
return armedChanged;
return vehicleArmed;
}
bool FirmwarePlugin::_setFlightModeAndValidate(Vehicle* vehicle, const QString& flightMode)

2
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

@ -389,7 +389,7 @@ void PX4FirmwarePlugin::_mavCommandResult(int vehicleId, int component, int comm @@ -389,7 +389,7 @@ void PX4FirmwarePlugin::_mavCommandResult(int vehicleId, int component, int comm
// They are trying to disarm.
disconnect(vehicle, &Vehicle::mavCommandResult, this, &PX4FirmwarePlugin::_mavCommandResult);
if (!vehicle->armed()) {
vehicle->setArmed(true);
vehicle->setArmedShowError(true);
}
}
}

6
src/Joystick/Joystick.cc

@ -683,7 +683,7 @@ void Joystick::startPolling(Vehicle* vehicle) @@ -683,7 +683,7 @@ void Joystick::startPolling(Vehicle* vehicle)
if (vehicle) {
// If a vehicle is connected, disconnect it
if (_activeVehicle) {
disconnect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmed);
disconnect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmedShowError);
disconnect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight);
disconnect(this, &Joystick::setFlightMode, _activeVehicle, &Vehicle::setFlightMode);
disconnect(this, &Joystick::gimbalPitchStep, _activeVehicle, &Vehicle::gimbalPitchStep);
@ -705,7 +705,7 @@ void Joystick::startPolling(Vehicle* vehicle) @@ -705,7 +705,7 @@ void Joystick::startPolling(Vehicle* vehicle)
// Only connect the new vehicle if it wants joystick data
if (vehicle->joystickEnabled()) {
_pollingStartedForCalibration = false;
connect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmed);
connect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmedShowError);
connect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight);
connect(this, &Joystick::setFlightMode, _activeVehicle, &Vehicle::setFlightMode);
connect(this, &Joystick::gimbalPitchStep, _activeVehicle, &Vehicle::gimbalPitchStep);
@ -725,7 +725,7 @@ void Joystick::stopPolling(void) @@ -725,7 +725,7 @@ void Joystick::stopPolling(void)
{
if (isRunning()) {
if (_activeVehicle && _activeVehicle->joystickEnabled()) {
disconnect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmed);
disconnect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmedShowError);
disconnect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight);
disconnect(this, &Joystick::setFlightMode, _activeVehicle, &Vehicle::setFlightMode);
disconnect(this, &Joystick::gimbalPitchStep, _activeVehicle, &Vehicle::gimbalPitchStep);

4
src/Vehicle/Vehicle.cc

@ -1851,12 +1851,12 @@ QGeoCoordinate Vehicle::homePosition() @@ -1851,12 +1851,12 @@ QGeoCoordinate Vehicle::homePosition()
return _homePosition;
}
void Vehicle::setArmed(bool armed)
void Vehicle::setArmed(bool armed, bool showError)
{
// We specifically use COMMAND_LONG:MAV_CMD_COMPONENT_ARM_DISARM since it is supported by more flight stacks.
sendMavCommand(_defaultComponentId,
MAV_CMD_COMPONENT_ARM_DISARM,
true, // show error if fails
showError,
armed ? 1.0f : 0.0f);
}

7
src/Vehicle/Vehicle.h

@ -145,7 +145,7 @@ public: @@ -145,7 +145,7 @@ public:
Q_PROPERTY(QGeoCoordinate coordinate READ coordinate NOTIFY coordinateChanged)
Q_PROPERTY(QGeoCoordinate homePosition READ homePosition NOTIFY homePositionChanged)
Q_PROPERTY(QGeoCoordinate armedPosition READ armedPosition NOTIFY armedPositionChanged)
Q_PROPERTY(bool armed READ armed WRITE setArmed NOTIFY armedChanged)
Q_PROPERTY(bool armed READ armed WRITE setArmedShowError NOTIFY armedChanged)
Q_PROPERTY(bool autoDisarm READ autoDisarm NOTIFY autoDisarmChanged)
Q_PROPERTY(bool flightModeSetAvailable READ flightModeSetAvailable CONSTANT)
Q_PROPERTY(QStringList flightModes READ flightModes NOTIFY flightModesChanged)
@ -439,8 +439,9 @@ public: @@ -439,8 +439,9 @@ public:
QGeoCoordinate homePosition();
bool armed () { return _armed; }
void setArmed (bool armed);
bool armed () { return _armed; }
void setArmed (bool armed, bool showError);
void setArmedShowError (bool armed) { setArmed(armed, true); }
bool flightModeSetAvailable ();
QStringList flightModes ();

2
src/qgcunittest/MavlinkLogTest.cc

@ -126,7 +126,7 @@ void MavlinkLogTest::_connectLogWorker(bool arm) @@ -126,7 +126,7 @@ void MavlinkLogTest::_connectLogWorker(bool arm)
QDir logSaveDir;
if (arm) {
qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->setArmed(true);
qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->setArmedShowError(true);
QTest::qWait(500); // Wait long enough for heartbeat to come through
// On Disconnect: We should get a getSaveFileName dialog.

Loading…
Cancel
Save