Commit 0dcb10c7 authored by Gus Grubba's avatar Gus Grubba Committed by GitHub

Merge pull request #4782 from DonLakeFlyer/MissionInt

Add support for MISSION_ITEM_INT
parents a3be26dd 9315d65f
This diff is collapsed.
......@@ -88,8 +88,8 @@ private:
bool _checkForExpectedAck(AckType_t receivedAck);
void _readTransactionComplete(void);
void _handleMissionCount(const mavlink_message_t& message);
void _handleMissionItem(const mavlink_message_t& message);
void _handleMissionRequest(const mavlink_message_t& message);
void _handleMissionItem(const mavlink_message_t& message, bool missionItemInt);
void _handleMissionRequest(const mavlink_message_t& message, bool missionItemInt);
void _handleMissionAck(const mavlink_message_t& message);
void _handleMissionCurrent(const mavlink_message_t& message);
void _requestNextMissionItem(void);
......
......@@ -111,6 +111,8 @@ Vehicle::Vehicle(LinkInterface* link,
, _telemetryTXBuffer(0)
, _telemetryLNoise(0)
, _telemetryRNoise(0)
, _vehicleCapabilitiesKnown(false)
, _supportsMissionItemInt(false)
, _connectionLost(false)
, _connectionLostEnabled(true)
, _missionManager(NULL)
......@@ -264,6 +266,8 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType,
, _globalPositionIntMessageAvailable(false)
, _cruiseSpeed(_settingsManager->appSettings()->offlineEditingCruiseSpeed()->rawValue().toDouble())
, _hoverSpeed(_settingsManager->appSettings()->offlineEditingHoverSpeed()->rawValue().toDouble())
, _vehicleCapabilitiesKnown(true)
, _supportsMissionItemInt(false)
, _connectionLost(false)
, _connectionLostEnabled(true)
, _missionManager(NULL)
......@@ -692,6 +696,13 @@ 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;
_startMissionRequest();
}
}
void Vehicle::_handleHilActuatorControls(mavlink_message_t &message)
......@@ -725,6 +736,12 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message)
mavlink_command_ack_t ack;
mavlink_msg_command_ack_decode(&message, &ack);
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.";
_startMissionRequest();
}
if (_mavCommandQueue.count() && ack.command == _mavCommandQueue[0].command) {
_mavCommandAckTimer.stop();
showError = _mavCommandQueue[0].showError;
......@@ -1634,9 +1651,10 @@ void Vehicle::_mapTrajectoryStop()
_mapTrajectoryTimer.stop();
}
void Vehicle::_parametersReady(bool parametersReady)
void Vehicle::_startMissionRequest(void)
{
if (parametersReady && !_missionManagerInitialRequestSent) {
if (!_missionManagerInitialRequestSent && _parameterManager->parametersReady() && _vehicleCapabilitiesKnown) {
qCDebug(VehicleLog) << "_startMissionRequest";
_missionManagerInitialRequestSent = true;
QString missionAutoLoadDirPath = _settingsManager->appSettings()->missionAutoLoadDir()->rawValue().toString();
if (missionAutoLoadDirPath.isEmpty()) {
......@@ -1650,6 +1668,19 @@ void Vehicle::_parametersReady(bool parametersReady)
MissionController::sendItemsToVehicle(this, visualItems);
}
}
} else {
if (!_parameterManager->parametersReady()) {
qCDebug(VehicleLog) << "Delaying _startMissionRequest due to parameters not ready";
} else if (!_vehicleCapabilitiesKnown) {
qCDebug(VehicleLog) << "Delaying _startMissionRequest due to vehicle capabilities not know";
}
}
}
void Vehicle::_parametersReady(bool parametersReady)
{
if (parametersReady) {
_startMissionRequest();
}
if (parametersReady) {
......@@ -2043,6 +2074,11 @@ void Vehicle::_sendMavCommandAgain(void)
MavCommandQueueEntry_t& queuedCommand = _mavCommandQueue[0];
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
_startMissionRequest();
}
emit mavCommandResult(_id, queuedCommand.component, queuedCommand.command, MAV_RESULT_FAILED, true /* noResponsefromVehicle */);
if (queuedCommand.showError) {
qgcApp()->showMessage(tr("Vehicle did not respond to command: %1").arg(qgcApp()->toolbox()->missionCommandTree()->friendlyName(queuedCommand.command)));
......
......@@ -658,6 +658,7 @@ public:
const QVariantList& toolBarIndicators ();
const QVariantList& cameraList (void) const;
bool supportsMissionItemInt(void) const { return _supportsMissionItemInt; }
public slots:
/// Sets the firmware plugin instance data associated with this Vehicle. This object will be parented to the Vehicle
......@@ -822,6 +823,7 @@ private:
void _sendNextQueuedMavCommand(void);
void _updatePriorityLink(void);
void _commonInit(void);
void _startMissionRequest(void);
int _id; ///< Mavlink system id
int _defaultComponentId;
......@@ -879,6 +881,8 @@ private:
uint32_t _telemetryTXBuffer;
uint32_t _telemetryLNoise;
uint32_t _telemetryRNoise;
bool _vehicleCapabilitiesKnown;
bool _supportsMissionItemInt;
typedef struct {
int component;
......
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