diff --git a/src/MissionManager/GeoFenceController.cc b/src/MissionManager/GeoFenceController.cc index 3b8f6df847178e5770586dc0ce6a59a35115cf40..d3f4b284cd8d63fed535022d374295206e1d364f 100644 --- a/src/MissionManager/GeoFenceController.cc +++ b/src/MissionManager/GeoFenceController.cc @@ -186,6 +186,7 @@ void GeoFenceController::sendToVehicle(void) } else if (syncInProgress()) { qCWarning(GeoFenceControllerLog) << "GeoFenceController::sendToVehicle called while syncInProgress"; } else { + qCDebug(GeoFenceControllerLog) << "GeoFenceController::sendToVehicle"; _geoFenceManager->sendToVehicle(_breachReturnPoint, _mapPolygon.pathModel()); _mapPolygon.setDirty(false); setDirty(false); @@ -316,14 +317,14 @@ void GeoFenceController::_updateContainsItems(void) bool GeoFenceController::showPlanFromManagerVehicle(void) { - qCDebug(GeoFenceControllerLog) << "showPlanFromManagerVehicle"; + qCDebug(GeoFenceControllerLog) << "showPlanFromManagerVehicle" << _editMode; if (_masterController->offline()) { qCWarning(GeoFenceControllerLog) << "GeoFenceController::showPlanFromManagerVehicle called while offline"; return true; // stops further propogation of showPlanFromManagerVehicle due to error } else { _itemsRequested = true; if (!_managerVehicle->initialPlanRequestComplete()) { - // The vehicle hasn't completed initial load, we can just wait for newMissionItemsAvailable to be signalled automatically + // The vehicle hasn't completed initial load, we can just wait for loadComplete to be signalled automatically qCDebug(GeoFenceControllerLog) << "showPlanFromManagerVehicle: !initialPlanRequestComplete, wait for signal"; return true; } else if (syncInProgress()) { diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 7f98d0daab2d269f1e82e434d90e73e2cc68e8e6..a8bf784866f041a6c53cc82446a8f948e19f8fff 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -205,6 +205,7 @@ void MissionController::sendToVehicle(void) } else if (syncInProgress()) { qCWarning(MissionControllerLog) << "MissionControllerLog::sendToVehicle called while syncInProgress"; } else { + qCDebug(MissionControllerLog) << "MissionControllerLog::sendToVehicle"; if (_visualItems->count() == 1) { // This prevents us from sending a possibly bogus home position to the vehicle QmlObjectListModel emptyModel; @@ -1678,7 +1679,7 @@ void MissionController::_visualItemsDirtyChanged(bool dirty) bool MissionController::showPlanFromManagerVehicle (void) { - qCDebug(MissionControllerLog) << "showPlanFromManagerVehicle"; + qCDebug(MissionControllerLog) << "showPlanFromManagerVehicle" << _editMode; if (_masterController->offline()) { qCWarning(MissionControllerLog) << "MissionController::showPlanFromManagerVehicle called while offline"; return true; // stops further propogation of showPlanFromManagerVehicle due to error diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index db35e2fca8bb0797774e246ed2691504eb29a581..8a88bada062df9658350f8e3c8fba2d768711585 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -409,11 +409,6 @@ void MissionManager::_requestNextMissionItem(void) void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool missionItemInt) { - - if (!_checkForExpectedAck(AckMissionItem)) { - return; - } - MAV_CMD command; MAV_FRAME frame; double param1; @@ -468,7 +463,24 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool m frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; } - qCDebug(MissionManagerLog) << "_handleMissionItem seq:command:current" << seq << command << isCurrentItem; + + bool ardupilotHomePositionUpdate = false; + if (!_checkForExpectedAck(AckMissionItem)) { + if (_vehicle->apmFirmware() && seq == 0) { + ardupilotHomePositionUpdate = true; + } else { + qCDebug(MissionManagerLog) << "_handleMissionItem dropping spurious item seq:command:current" << seq << command << isCurrentItem; + return; + } + } + + qCDebug(MissionManagerLog) << "_handleMissionItem seq:command:current:ardupilotHomePositionUpdate" << seq << command << isCurrentItem << ardupilotHomePositionUpdate; + + if (ardupilotHomePositionUpdate) { + QGeoCoordinate newHomePosition(param5, param6, param7); + _vehicle->_setHomePosition(newHomePosition); + return; + } if (_itemIndicesToRead.contains(seq)) { _itemIndicesToRead.removeOne(seq); diff --git a/src/MissionManager/RallyPointController.cc b/src/MissionManager/RallyPointController.cc index 8b48dd9d4f8126af2c766139ac22f4e8778ba88c..a4f3967d75285ebcf065eb5edbeb1fbe670a4b7a 100644 --- a/src/MissionManager/RallyPointController.cc +++ b/src/MissionManager/RallyPointController.cc @@ -157,6 +157,7 @@ void RallyPointController::sendToVehicle(void) } else if (syncInProgress()) { qCWarning(RallyPointControllerLog) << "RallyPointController::sendToVehicle called while syncInProgress"; } else { + qCDebug(RallyPointControllerLog) << "RallyPointController::sendToVehicle"; setDirty(false); QList rgPoints; for (int i=0; i<_points.count(); i++) { @@ -280,13 +281,13 @@ void RallyPointController::_updateContainsItems(void) bool RallyPointController::showPlanFromManagerVehicle (void) { - qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle"; + qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle _editMode" << _editMode; if (_masterController->offline()) { qCWarning(RallyPointControllerLog) << "RallyPointController::showPlanFromManagerVehicle called while offline"; return true; // stops further propogation of showPlanFromManagerVehicle due to error } else { if (!_managerVehicle->initialPlanRequestComplete()) { - // The vehicle hasn't completed initial load, we can just wait for newMissionItemsAvailable to be signalled automatically + // The vehicle hasn't completed initial load, we can just wait for loadComplete to be signalled automatically qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle: !initialPlanRequestComplete, wait for signal"; return true; } else if (syncInProgress()) { diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 57aa7bc8ef3ef3cfddfaf1ba3d38ad153ddc52bc..4e47d692f72de93637af6afd0fa948cdc7775e99 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -669,6 +669,16 @@ void Vehicle::_handleAltitude(mavlink_message_t& message) } } +void Vehicle::_setCapabilities(uint64_t capabilityBits) +{ + if (capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_INT) { + _supportsMissionItemInt = true; + } + _vehicleCapabilitiesKnown = true; + + qCDebug(VehicleLog) << QString("Vehicle %1 MISSION_ITEM_INT").arg(_supportsMissionItemInt ? QStringLiteral("supports") : QStringLiteral("does not support")); +} + void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& message) { Q_UNUSED(link); @@ -703,12 +713,8 @@ void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& me emit gitHashChanged(_gitHash); } - if (autopilotVersion.capabilities & MAV_PROTOCOL_CAPABILITY_MISSION_INT) { - qCDebug(VehicleLog) << "Vehicle supports MISSION_ITEM_INT"; - _supportsMissionItemInt = true; - _vehicleCapabilitiesKnown = true; - _startPlanRequest(); - } + _setCapabilities(autopilotVersion.capabilities); + _startPlanRequest(); } void Vehicle::_handleHilActuatorControls(mavlink_message_t &message) @@ -745,6 +751,7 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message) if (ack.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES && ack.result != MAV_RESULT_ACCEPTED) { // We aren't going to get a response back for capabilities, so stop waiting for it before we ask for mission items qCDebug(VehicleLog) << "Vehicle failed to responded to MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES with error. Starting mission request."; + _setCapabilities(0); _startPlanRequest(); } @@ -899,6 +906,14 @@ void Vehicle::_handleBatteryStatus(mavlink_message_t& message) _batteryFactGroup.cellCount()->setRawValue(cellCount); } +void Vehicle::_setHomePosition(QGeoCoordinate& homeCoord) +{ + if (homeCoord != _homePosition) { + _homePosition = homeCoord; + emit homePositionChanged(_homePosition); + } +} + void Vehicle::_handleHomePosition(mavlink_message_t& message) { mavlink_home_position_t homePos; @@ -908,10 +923,7 @@ void Vehicle::_handleHomePosition(mavlink_message_t& message) QGeoCoordinate newHomePosition (homePos.latitude / 10000000.0, homePos.longitude / 10000000.0, homePos.altitude / 1000.0); - if (newHomePosition != _homePosition) { - _homePosition = newHomePosition; - emit homePositionChanged(_homePosition); - } + _setHomePosition(newHomePosition); } void Vehicle::_handleHeartbeat(mavlink_message_t& message) @@ -1625,7 +1637,11 @@ void Vehicle::_mapTrajectoryStop() void Vehicle::_startPlanRequest(void) { - if (!_missionManagerInitialRequestSent && _parameterManager->parametersReady() && _vehicleCapabilitiesKnown) { + if (_missionManagerInitialRequestSent) { + return; + } + + if (_parameterManager->parametersReady() && _vehicleCapabilitiesKnown) { qCDebug(VehicleLog) << "_startPlanRequest"; _missionManagerInitialRequestSent = true; if (_settingsManager->appSettings()->autoLoadMissions()->rawValue().toBool()) { @@ -1644,7 +1660,7 @@ void Vehicle::_startPlanRequest(void) if (!_parameterManager->parametersReady()) { qCDebug(VehicleLog) << "Delaying _startPlanRequest due to parameters not ready"; } else if (!_vehicleCapabilitiesKnown) { - qCDebug(VehicleLog) << "Delaying _startPlanRequest due to vehicle capabilities not know"; + qCDebug(VehicleLog) << "Delaying _startPlanRequest due to vehicle capabilities not known"; } } } @@ -2067,6 +2083,7 @@ void Vehicle::_sendMavCommandAgain(void) if (_mavCommandRetryCount++ > _mavCommandMaxRetryCount) { if (queuedCommand.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) { // We aren't going to get a response back for capabilities, so stop waiting for it before we ask for mission items + _setCapabilities(0); _startPlanRequest(); } @@ -2100,7 +2117,7 @@ void Vehicle::_sendMavCommandAgain(void) } } } - qDebug() << "Vehicle::_sendMavCommandAgain retrying command:_mavCommandRetryCount" << queuedCommand.command << _mavCommandRetryCount; + qCDebug(VehicleLog) << "Vehicle::_sendMavCommandAgain retrying command:_mavCommandRetryCount" << queuedCommand.command << _mavCommandRetryCount; } _mavCommandAckTimer.start(); diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 555f9f2b61e38508ad19155ec4eef60db7caaff8..8cd5e3c7670c407ecf2dfb14368124026fefd5af 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -673,6 +673,8 @@ public: /// @return: true: initial request is complete, false: initial request is still in progress; bool initialPlanRequestComplete(void) const { return _initialPlanRequestComplete; } + void _setHomePosition(QGeoCoordinate& homeCoord); + signals: void allLinksInactive(Vehicle* vehicle); void coordinateChanged(QGeoCoordinate coordinate); @@ -834,6 +836,7 @@ private: void _commonInit(void); void _startPlanRequest(void); void _setupAutoDisarmSignalling(void); + void _setCapabilities(uint64_t capabilityBits); int _id; ///< Mavlink system id int _defaultComponentId; diff --git a/src/ui/toolbar/ArmedIndicator.qml b/src/ui/toolbar/ArmedIndicator.qml index 9428825e5c9d6256c097f83dae3110b98fcdb88c..3b549dd7334c5d0f585817632b1b5fde11b8002b 100644 --- a/src/ui/toolbar/ArmedIndicator.qml +++ b/src/ui/toolbar/ArmedIndicator.qml @@ -26,11 +26,8 @@ QGCLabel { text: _armed ? qsTr("Armed") : qsTr("Disarmed") font.pointSize: ScreenTools.mediumFontPointSize color: qgcPal.buttonText - visible: !_autoDisarm || _fixedWing property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle - property bool _autoDisarm: _activeVehicle ? _activeVehicle.autoDisarm : false - property bool _fixedWing: _activeVehicle ? _activeVehicle.fixedWing : false property bool _armed: _activeVehicle ? _activeVehicle.armed : false QGCPalette { id: qgcPal }