Commit d7bb6d85 authored by DonLakeFlyer's avatar DonLakeFlyer

Better retries on arming, flight mode change and APM guided

parent 4423b2bf
......@@ -162,35 +162,42 @@ bool ArduCopterFirmwarePlugin::isCapable(const Vehicle* vehicle, FirmwareCapabil
void ArduCopterFirmwarePlugin::guidedModeRTL(Vehicle* vehicle)
{
vehicle->setFlightMode("RTL");
_setFlightModeAndValidate(vehicle, "RTL");
}
void ArduCopterFirmwarePlugin::guidedModeLand(Vehicle* vehicle)
{
vehicle->setFlightMode("Land");
_setFlightModeAndValidate(vehicle, "Land");
}
void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
{
_guidedModeTakeoff(vehicle);
}
bool ArduCopterFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle)
{
QString takeoffAltParam("PILOT_TKOFF_ALT");
if (!vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, takeoffAltParam)) {
qgcApp()->showMessage(tr("Unable to takeoff, %1 parameter missing.").arg(takeoffAltParam));
return;
float takeoffAlt = 0;
if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, takeoffAltParam)) {
Fact* takeoffAltFact = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam);
takeoffAlt = takeoffAltFact->rawValue().toDouble();
}
Fact* takeoffAltFact = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam);
float takeoffAlt = takeoffAltFact->rawValue().toDouble();
if (takeoffAlt <= 0) {
takeoffAlt = 2.5;
} else {
takeoffAlt /= 100; // centimeters -> meters
}
setGuidedMode(vehicle, true);
if (!_setFlightModeAndValidate(vehicle, "Guided")) {
qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to change to Guided mode."));
return false;
}
if (!_armVehicleAndValidate(vehicle)) {
qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to arm."));
return;
return false;
}
vehicle->sendMavCommand(vehicle->defaultComponentId(),
......@@ -198,6 +205,8 @@ void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
true, // show error
0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
takeoffAlt);
return true;
}
void ArduCopterFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
......@@ -246,13 +255,13 @@ void ArduCopterFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double
void ArduCopterFirmwarePlugin::pauseVehicle(Vehicle* vehicle)
{
vehicle->setFlightMode("Brake");
_setFlightModeAndValidate(vehicle, "Brake");
}
void ArduCopterFirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
{
if (guidedMode) {
vehicle->setFlightMode("Guided");
_setFlightModeAndValidate(vehicle, "Guided");
} else {
pauseVehicle(vehicle);
}
......@@ -293,25 +302,29 @@ void ArduCopterFirmwarePlugin::startMission(Vehicle* vehicle)
double currentAlt = vehicle->altitudeRelative()->rawValue().toDouble();
if (!vehicle->flying()) {
guidedModeTakeoff(vehicle);
// Wait for vehicle to get off ground before switching to auto
bool didTakeoff = false;
for (int i=0; i<50; i++) {
if (vehicle->altitudeRelative()->rawValue().toDouble() >= currentAlt + 1.0) {
didTakeoff = true;
break;
if (_guidedModeTakeoff(vehicle)) {
// Wait for vehicle to get off ground before switching to auto (10 seconds)
bool didTakeoff = false;
for (int i=0; i<100; i++) {
if (vehicle->altitudeRelative()->rawValue().toDouble() >= currentAlt + 1.0) {
didTakeoff = true;
break;
}
QGC::SLEEP::msleep(100);
qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
}
QGC::SLEEP::msleep(100);
qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
}
if (!didTakeoff) {
qgcApp()->showMessage(QStringLiteral("Unable to switch to Auto. Vehicle takeoff failed."));
return;
if (!didTakeoff) {
qgcApp()->showMessage(QStringLiteral("Unable to start mission. Vehicle takeoff failed."));
return;
}
}
}
vehicle->setFlightMode(missionFlightMode());
if (!_setFlightModeAndValidate(vehicle, missionFlightMode())) {
qgcApp()->showMessage(QStringLiteral("Unable to start mission. Vehicle failed to change to auto."));
return;
}
}
......@@ -81,6 +81,8 @@ public:
private:
static bool _remapParamNameIntialized;
static FirmwarePlugin::remapParamNameMajorVersionMap_t _remapParamName;
bool _guidedModeTakeoff(Vehicle* vehicle);
};
#endif
......@@ -437,21 +437,63 @@ bool FirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle)
bool FirmwarePlugin::_armVehicleAndValidate(Vehicle* vehicle)
{
if (!vehicle->armed()) {
if (vehicle->armed()) {
return true;
}
bool armedChanged = false;
// We try arming 3 times
for (int retries=0; retries<3; retries++) {
vehicle->setArmed(true);
// 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) {
break;
}
}
// Wait for vehicle to return armed state for 2 seconds
for (int i=0; i<20; i++) {
if (vehicle->armed()) {
return armedChanged;
}
bool FirmwarePlugin::_setFlightModeAndValidate(Vehicle* vehicle, const QString& flightMode)
{
if (vehicle->flightMode() == flightMode) {
return true;
}
bool flightModeChanged = false;
// We try 3 times
for (int retries=0; retries<3; retries++) {
vehicle->setFlightMode(flightMode);
// Wait for vehicle to return flight mode
for (int i=0; i<30; i++) {
if (vehicle->flightMode() == flightMode) {
flightModeChanged = true;
break;
}
QGC::SLEEP::msleep(100);
qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
}
if (flightModeChanged) {
break;
}
QGC::SLEEP::msleep(100);
qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
}
return vehicle->armed();
return flightModeChanged;
}
void FirmwarePlugin::batteryConsumptionData(Vehicle* vehicle, int& mAhBattery, double& hoverAmps, double& cruiseAmps) const
{
Q_UNUSED(vehicle);
......
......@@ -296,10 +296,14 @@ public:
static const char* px4FollowMeFlightMode;
protected:
// Arms the vehicle, waiting for the arm state to change.
// Arms the vehicle with validation and retries
// @return: true - vehicle armed, false - vehicle failed to arm
bool _armVehicleAndValidate(Vehicle* vehicle);
// Sets the vehicle to the specified flight mode with validation and retries
// @return: true - vehicle in specified flight mode, false - flight mode change failed
bool _setFlightModeAndValidate(Vehicle* vehicle, const QString& flightMode);
private:
QVariantList _toolBarIndicatorList;
static QVariantList _cameraList; ///< Standard QGC camera list
......
......@@ -331,12 +331,12 @@ void PX4FirmwarePlugin::pauseVehicle(Vehicle* vehicle)
void PX4FirmwarePlugin::guidedModeRTL(Vehicle* vehicle)
{
vehicle->setFlightMode(_rtlFlightMode);
_setFlightModeAndValidate(vehicle, _rtlFlightMode);
}
void PX4FirmwarePlugin::guidedModeLand(Vehicle* vehicle)
{
vehicle->setFlightMode(_landingFlightMode);
_setFlightModeAndValidate(vehicle, _landingFlightMode);
}
void PX4FirmwarePlugin::guidedModeOrbit(Vehicle* vehicle, const QGeoCoordinate& centerCoord, double radius, double velocity, double altitude)
......@@ -457,13 +457,13 @@ void PX4FirmwarePlugin::startMission(Vehicle* vehicle)
return;
}
vehicle->setFlightMode(missionFlightMode());
_setFlightModeAndValidate(vehicle, missionFlightMode());
}
void PX4FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
{
if (guidedMode) {
vehicle->setFlightMode(_holdFlightMode);
_setFlightModeAndValidate(vehicle, _holdFlightMode);
} else {
pauseVehicle(vehicle);
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment