Skip to content

Q
qgroundcontrol
Commit 9315d65f authored by DonLakeFlyer's avatar DonLakeFlyer
Browse files
Options

Add support for MISSION_ITEM_INT

parent 8e95ba8f
Show whitespace changes
Inline Side-by-side
Showing with 217 additions and 84 deletions
src/MissionManager/MissionManager.cc
View file @ 9315d65f
...@@ -343,6 +343,19 @@ void MissionManager::_requestNextMissionItem(void) ...@@ -343,6 +343,19 @@ void MissionManager::_requestNextMissionItem(void)
qCDebug(MissionManagerLog) << "_requestNextMissionItem sequenceNumber:retry" << _itemIndicesToRead[0] << _retryCount; qCDebug(MissionManagerLog) << "_requestNextMissionItem sequenceNumber:retry" << _itemIndicesToRead[0] << _retryCount;
mavlink_message_t message; mavlink_message_t message;
if (_vehicle->supportsMissionItemInt()) {
mavlink_mission_request_int_t missionRequest;
missionRequest.target_system = _vehicle->id();
missionRequest.target_component = MAV_COMP_ID_MISSIONPLANNER;
missionRequest.seq = _itemIndicesToRead[0];
mavlink_msg_mission_request_int_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_dedicatedLink->mavlinkChannel(),
&message,
&missionRequest);
} else {
mavlink_mission_request_t missionRequest; mavlink_mission_request_t missionRequest;
missionRequest.target_system = _vehicle->id(); missionRequest.target_system = _vehicle->id();
...@@ -354,38 +367,83 @@ void MissionManager::_requestNextMissionItem(void) ...@@ -354,38 +367,83 @@ void MissionManager::_requestNextMissionItem(void)
_dedicatedLink->mavlinkChannel(), _dedicatedLink->mavlinkChannel(),
&message, &message,
&missionRequest); &missionRequest);
}
_vehicle->sendMessageOnLink(_dedicatedLink, message); _vehicle->sendMessageOnLink(_dedicatedLink, message);
_startAckTimeout(AckMissionItem); _startAckTimeout(AckMissionItem);
} }
void MissionManager::_handleMissionItem(const mavlink_message_t& message) void MissionManager::_handleMissionItem(const mavlink_message_t& message, bool missionItemInt)
{ {
mavlink_mission_item_t missionItem;
if (!_checkForExpectedAck(AckMissionItem)) { if (!_checkForExpectedAck(AckMissionItem)) {
return; return;
} }
MAV_CMD command;
MAV_FRAME frame;
double param1;
double param2;
double param3;
double param4;
double param5;
double param6;
double param7;
bool autoContinue;
bool isCurrentItem;
int seq;
if (missionItemInt) {
mavlink_mission_item_int_t missionItem;
mavlink_msg_mission_item_int_decode(&message, &missionItem);
command = (MAV_CMD)missionItem.command,
frame = (MAV_FRAME)missionItem.frame,
param1 = missionItem.param1;
param2 = missionItem.param2;
param3 = missionItem.param3;
param4 = missionItem.param4;
param5 = (double)missionItem.x / qPow(10.0, 7.0);
param6 = (double)missionItem.y / qPow(10.0, 7.0);
param7 = (double)missionItem.z / qPow(10.0, 7.0);
autoContinue = missionItem.autocontinue;
isCurrentItem = missionItem.current;
seq = missionItem.seq;
} else {
mavlink_mission_item_t missionItem;
mavlink_msg_mission_item_decode(&message, &missionItem); mavlink_msg_mission_item_decode(&message, &missionItem);
qCDebug(MissionManagerLog) << "_handleMissionItem sequenceNumber:" << missionItem.seq; command = (MAV_CMD)missionItem.command,
frame = (MAV_FRAME)missionItem.frame,
if (_itemIndicesToRead.contains(missionItem.seq)) { param1 = missionItem.param1;
_itemIndicesToRead.removeOne(missionItem.seq); param2 = missionItem.param2;
param3 = missionItem.param3;
MissionItem* item = new MissionItem(missionItem.seq, param4 = missionItem.param4;
(MAV_CMD)missionItem.command, param5 = missionItem.x;
(MAV_FRAME)missionItem.frame, param6 = missionItem.y;
missionItem.param1, param7 = missionItem.z;
missionItem.param2, autoContinue = missionItem.autocontinue;
missionItem.param3, isCurrentItem = missionItem.current;
missionItem.param4, seq = missionItem.seq;
missionItem.x, }
missionItem.y,
missionItem.z, qCDebug(MissionManagerLog) << "_handleMissionItem sequenceNumber:" << seq;
missionItem.autocontinue,
missionItem.current, if (_itemIndicesToRead.contains(seq)) {
_itemIndicesToRead.removeOne(seq);
MissionItem* item = new MissionItem(seq,
command,
frame,
param1,
param2,
param3,
param4,
param5,
param6,
param7,
autoContinue,
isCurrentItem,
this); this);
if (item->command() == MAV_CMD_DO_JUMP && !_vehicle->firmwarePlugin()->sendHomePositionToVehicle()) { if (item->command() == MAV_CMD_DO_JUMP && !_vehicle->firmwarePlugin()->sendHomePositionToVehicle()) {
...@@ -395,7 +453,7 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message) ...@@ -395,7 +453,7 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message)
_missionItems.append(item); _missionItems.append(item);
} else { } else {
qCDebug(MissionManagerLog) << "_handleMissionItem mission item received item index which was not requested, disregrarding:" << missionItem.seq; qCDebug(MissionManagerLog) << "_handleMissionItem mission item received item index which was not requested, disregrarding:" << seq;
// We have to put the ack timeout back since it was removed above // We have to put the ack timeout back since it was removed above
_startAckTimeout(AckMissionItem); _startAckTimeout(AckMissionItem);
return; return;
...@@ -415,7 +473,7 @@ void MissionManager::_clearMissionItems(void) ...@@ -415,7 +473,7 @@ void MissionManager::_clearMissionItems(void)
_missionItems.clear(); _missionItems.clear();
} }
void MissionManager::_handleMissionRequest(const mavlink_message_t& message) void MissionManager::_handleMissionRequest(const mavlink_message_t& message, bool missionItemInt)
{ {
mavlink_mission_request_t missionRequest; mavlink_mission_request_t missionRequest;
...@@ -440,6 +498,32 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message) ...@@ -440,6 +498,32 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message)
} }
mavlink_message_t messageOut; mavlink_message_t messageOut;
if (missionItemInt) {
mavlink_mission_item_int_t missionItem;
MissionItem* item = _missionItems[missionRequest.seq];
missionItem.target_system = _vehicle->id();
missionItem.target_component = MAV_COMP_ID_MISSIONPLANNER;
missionItem.seq = missionRequest.seq;
missionItem.command = item->command();
missionItem.param1 = item->param1();
missionItem.param2 = item->param2();
missionItem.param3 = item->param3();
missionItem.param4 = item->param4();
missionItem.x = item->param5() * qPow(10.0, 7.0);
missionItem.y = item->param6() * qPow(10.0, 7.0);
missionItem.z = item->param7() * qPow(10.0, 7.0);
missionItem.frame = item->frame();
missionItem.current = missionRequest.seq == 0;
missionItem.autocontinue = item->autoContinue();
mavlink_msg_mission_item_int_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_dedicatedLink->mavlinkChannel(),
&messageOut,
&missionItem);
} else {
mavlink_mission_item_t missionItem; mavlink_mission_item_t missionItem;
MissionItem* item = _missionItems[missionRequest.seq]; MissionItem* item = _missionItems[missionRequest.seq];
...@@ -464,6 +548,7 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message) ...@@ -464,6 +548,7 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message)
_dedicatedLink->mavlinkChannel(), _dedicatedLink->mavlinkChannel(),
&messageOut, &messageOut,
&missionItem); &missionItem);
}
_vehicle->sendMessageOnLink(_dedicatedLink, messageOut); _vehicle->sendMessageOnLink(_dedicatedLink, messageOut);
_startAckTimeout(AckMissionRequest); _startAckTimeout(AckMissionRequest);
...@@ -540,11 +625,19 @@ void MissionManager::_mavlinkMessageReceived(const mavlink_message_t& message) ...@@ -540,11 +625,19 @@ void MissionManager::_mavlinkMessageReceived(const mavlink_message_t& message)
break; break;
case MAVLINK_MSG_ID_MISSION_ITEM: case MAVLINK_MSG_ID_MISSION_ITEM:
_handleMissionItem(message); _handleMissionItem(message, false /* missionItemInt */);
break;
case MAVLINK_MSG_ID_MISSION_ITEM_INT:
_handleMissionItem(message, true /* missionItemInt */);
break; break;
case MAVLINK_MSG_ID_MISSION_REQUEST: case MAVLINK_MSG_ID_MISSION_REQUEST:
_handleMissionRequest(message); _handleMissionRequest(message, false /* missionItemInt */);
break;
case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
_handleMissionRequest(message, true /* missionItemInt */);
break; break;
case MAVLINK_MSG_ID_MISSION_ACK: case MAVLINK_MSG_ID_MISSION_ACK:
......
src/MissionManager/MissionManager.h
View file @ 9315d65f
...@@ -88,8 +88,8 @@ private: ...@@ -88,8 +88,8 @@ private:
bool _checkForExpectedAck(AckType_t receivedAck); bool _checkForExpectedAck(AckType_t receivedAck);
void _readTransactionComplete(void); void _readTransactionComplete(void);
void _handleMissionCount(const mavlink_message_t& message); void _handleMissionCount(const mavlink_message_t& message);
void _handleMissionItem(const mavlink_message_t& message); void _handleMissionItem(const mavlink_message_t& message, bool missionItemInt);
void _handleMissionRequest(const mavlink_message_t& message); void _handleMissionRequest(const mavlink_message_t& message, bool missionItemInt);
void _handleMissionAck(const mavlink_message_t& message); void _handleMissionAck(const mavlink_message_t& message);
void _handleMissionCurrent(const mavlink_message_t& message); void _handleMissionCurrent(const mavlink_message_t& message);
void _requestNextMissionItem(void); void _requestNextMissionItem(void);
......
src/Vehicle/Vehicle.cc
View file @ 9315d65f
...@@ -111,6 +111,8 @@ Vehicle::Vehicle(LinkInterface* link, ...@@ -111,6 +111,8 @@ Vehicle::Vehicle(LinkInterface* link,
, _telemetryTXBuffer(0) , _telemetryTXBuffer(0)
, _telemetryLNoise(0) , _telemetryLNoise(0)
, _telemetryRNoise(0) , _telemetryRNoise(0)
, _vehicleCapabilitiesKnown(false)
, _supportsMissionItemInt(false)
, _connectionLost(false) , _connectionLost(false)
, _connectionLostEnabled(true) , _connectionLostEnabled(true)
, _missionManager(NULL) , _missionManager(NULL)
...@@ -264,6 +266,8 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, ...@@ -264,6 +266,8 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType,
, _globalPositionIntMessageAvailable(false) , _globalPositionIntMessageAvailable(false)
, _cruiseSpeed(_settingsManager->appSettings()->offlineEditingCruiseSpeed()->rawValue().toDouble()) , _cruiseSpeed(_settingsManager->appSettings()->offlineEditingCruiseSpeed()->rawValue().toDouble())
, _hoverSpeed(_settingsManager->appSettings()->offlineEditingHoverSpeed()->rawValue().toDouble()) , _hoverSpeed(_settingsManager->appSettings()->offlineEditingHoverSpeed()->rawValue().toDouble())
, _vehicleCapabilitiesKnown(true)
, _supportsMissionItemInt(false)
, _connectionLost(false) , _connectionLost(false)
, _connectionLostEnabled(true) , _connectionLostEnabled(true)
, _missionManager(NULL) , _missionManager(NULL)
...@@ -692,6 +696,13 @@ void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& me ...@@ -692,6 +696,13 @@ void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& me
} }
emit gitHashChanged(_gitHash); 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) void Vehicle::_handleHilActuatorControls(mavlink_message_t &message)
...@@ -725,6 +736,12 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message) ...@@ -725,6 +736,12 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message)
mavlink_command_ack_t ack; mavlink_command_ack_t ack;
mavlink_msg_command_ack_decode(&message, &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) { if (_mavCommandQueue.count() && ack.command == _mavCommandQueue[0].command) {
_mavCommandAckTimer.stop(); _mavCommandAckTimer.stop();
showError = _mavCommandQueue[0].showError; showError = _mavCommandQueue[0].showError;
...@@ -1634,9 +1651,10 @@ void Vehicle::_mapTrajectoryStop() ...@@ -1634,9 +1651,10 @@ void Vehicle::_mapTrajectoryStop()
_mapTrajectoryTimer.stop(); _mapTrajectoryTimer.stop();
} }
void Vehicle::_parametersReady(bool parametersReady) void Vehicle::_startMissionRequest(void)
{ {
if (parametersReady && !_missionManagerInitialRequestSent) { if (!_missionManagerInitialRequestSent && _parameterManager->parametersReady() && _vehicleCapabilitiesKnown) {
qCDebug(VehicleLog) << "_startMissionRequest";
_missionManagerInitialRequestSent = true; _missionManagerInitialRequestSent = true;
QString missionAutoLoadDirPath = _settingsManager->appSettings()->missionAutoLoadDir()->rawValue().toString(); QString missionAutoLoadDirPath = _settingsManager->appSettings()->missionAutoLoadDir()->rawValue().toString();
if (missionAutoLoadDirPath.isEmpty()) { if (missionAutoLoadDirPath.isEmpty()) {
...@@ -1650,6 +1668,19 @@ void Vehicle::_parametersReady(bool parametersReady) ...@@ -1650,6 +1668,19 @@ void Vehicle::_parametersReady(bool parametersReady)
MissionController::sendItemsToVehicle(this, visualItems); 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) { if (parametersReady) {
...@@ -2043,6 +2074,11 @@ void Vehicle::_sendMavCommandAgain(void) ...@@ -2043,6 +2074,11 @@ void Vehicle::_sendMavCommandAgain(void)
MavCommandQueueEntry_t& queuedCommand = _mavCommandQueue[0]; MavCommandQueueEntry_t& queuedCommand = _mavCommandQueue[0];
if (_mavCommandRetryCount++ > _mavCommandMaxRetryCount) { 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 */); emit mavCommandResult(_id, queuedCommand.component, queuedCommand.command, MAV_RESULT_FAILED, true /* noResponsefromVehicle */);
if (queuedCommand.showError) { if (queuedCommand.showError) {
qgcApp()->showMessage(tr("Vehicle did not respond to command: %1").arg(qgcApp()->toolbox()->missionCommandTree()->friendlyName(queuedCommand.command))); qgcApp()->showMessage(tr("Vehicle did not respond to command: %1").arg(qgcApp()->toolbox()->missionCommandTree()->friendlyName(queuedCommand.command)));
......
src/Vehicle/Vehicle.h
View file @ 9315d65f
...@@ -658,6 +658,7 @@ public: ...@@ -658,6 +658,7 @@ public:
const QVariantList& toolBarIndicators (); const QVariantList& toolBarIndicators ();
const QVariantList& cameraList (void) const; const QVariantList& cameraList (void) const;
bool supportsMissionItemInt(void) const { return _supportsMissionItemInt; }
public slots: public slots:
/// Sets the firmware plugin instance data associated with this Vehicle. This object will be parented to the Vehicle /// Sets the firmware plugin instance data associated with this Vehicle. This object will be parented to the Vehicle
...@@ -822,6 +823,7 @@ private: ...@@ -822,6 +823,7 @@ private:
void _sendNextQueuedMavCommand(void); void _sendNextQueuedMavCommand(void);
void _updatePriorityLink(void); void _updatePriorityLink(void);
void _commonInit(void); void _commonInit(void);
void _startMissionRequest(void);
int _id; ///< Mavlink system id int _id; ///< Mavlink system id
int _defaultComponentId; int _defaultComponentId;
...@@ -879,6 +881,8 @@ private: ...@@ -879,6 +881,8 @@ private:
uint32_t _telemetryTXBuffer; uint32_t _telemetryTXBuffer;
uint32_t _telemetryLNoise; uint32_t _telemetryLNoise;
uint32_t _telemetryRNoise; uint32_t _telemetryRNoise;
bool _vehicleCapabilitiesKnown;
bool _supportsMissionItemInt;
typedef struct { typedef struct {
int component; 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