Skip to content

Q
qgroundcontrol
Commit 0dcb10c7 authored by Gus Grubba's avatar Gus Grubba Committed by GitHub
Browse files
Options

Merge pull request #4782 from DonLakeFlyer/MissionInt 

Add support for MISSION_ITEM_INT
parents a3be26dd 9315d65f
Hide whitespace changes
Inline Side-by-side
Showing with 217 additions and 84 deletions
src/MissionManager/MissionManager.cc
View file @ 0dcb10c7
......@@ -342,50 +342,108 @@ void MissionManager::_requestNextMissionItem(void)
qCDebug(MissionManagerLog) << "_requestNextMissionItem sequenceNumber:retry" << _itemIndicesToRead[0] << _retryCount;
mavlink_message_t message;
mavlink_mission_request_t missionRequest;
missionRequest.target_system = _vehicle->id();
missionRequest.target_component = MAV_COMP_ID_MISSIONPLANNER;
missionRequest.seq = _itemIndicesToRead[0];
mavlink_msg_mission_request_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_dedicatedLink->mavlinkChannel(),
&message,
&missionRequest);
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;
missionRequest.target_system = _vehicle->id();
missionRequest.target_component = MAV_COMP_ID_MISSIONPLANNER;
missionRequest.seq = _itemIndicesToRead[0];
mavlink_msg_mission_request_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_dedicatedLink->mavlinkChannel(),
&message,
&missionRequest);
}
_vehicle->sendMessageOnLink(_dedicatedLink, message);
_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)) {
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);
command = (MAV_CMD)missionItem.command,
frame = (MAV_FRAME)missionItem.frame,
param1 = missionItem.param1;
param2 = missionItem.param2;
param3 = missionItem.param3;
param4 = missionItem.param4;
param5 = missionItem.x;
param6 = missionItem.y;
param7 = missionItem.z;
autoContinue = missionItem.autocontinue;
isCurrentItem = missionItem.current;
seq = missionItem.seq;
}
mavlink_msg_mission_item_decode(&message, &missionItem);
qCDebug(MissionManagerLog) << "_handleMissionItem sequenceNumber:" << missionItem.seq;
qCDebug(MissionManagerLog) << "_handleMissionItem sequenceNumber:" << seq;
if (_itemIndicesToRead.contains(missionItem.seq)) {
_itemIndicesToRead.removeOne(missionItem.seq);
MissionItem* item = new MissionItem(missionItem.seq,
(MAV_CMD)missionItem.command,
(MAV_FRAME)missionItem.frame,
missionItem.param1,
missionItem.param2,
missionItem.param3,
missionItem.param4,
missionItem.x,
missionItem.y,
missionItem.z,
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);
if (item->command() == MAV_CMD_DO_JUMP && !_vehicle->firmwarePlugin()->sendHomePositionToVehicle()) {
......@@ -395,7 +453,7 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message)
_missionItems.append(item);
} 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
_startAckTimeout(AckMissionItem);
return;
......@@ -415,7 +473,7 @@ void MissionManager::_clearMissionItems(void)
_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;
......@@ -440,30 +498,57 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message)
}
mavlink_message_t messageOut;
mavlink_mission_item_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();
missionItem.y = item->param6();
missionItem.z = item->param7();
missionItem.frame = item->frame();
missionItem.current = missionRequest.seq == 0;
missionItem.autocontinue = item->autoContinue();
mavlink_msg_mission_item_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_dedicatedLink->mavlinkChannel(),
&messageOut,
&missionItem);
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;
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();
missionItem.y = item->param6();
missionItem.z = item->param7();
missionItem.frame = item->frame();
missionItem.current = missionRequest.seq == 0;
missionItem.autocontinue = item->autoContinue();
mavlink_msg_mission_item_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(),
qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
_dedicatedLink->mavlinkChannel(),
&messageOut,
&missionItem);
}
_vehicle->sendMessageOnLink(_dedicatedLink, messageOut);
_startAckTimeout(AckMissionRequest);
......@@ -535,29 +620,37 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message)
void MissionManager::_mavlinkMessageReceived(const mavlink_message_t& message)
{
switch (message.msgid) {
case MAVLINK_MSG_ID_MISSION_COUNT:
_handleMissionCount(message);
break;
case MAVLINK_MSG_ID_MISSION_COUNT:
_handleMissionCount(message);
break;
case MAVLINK_MSG_ID_MISSION_ITEM:
_handleMissionItem(message);
break;
case MAVLINK_MSG_ID_MISSION_REQUEST:
_handleMissionRequest(message);
break;
case MAVLINK_MSG_ID_MISSION_ACK:
_handleMissionAck(message);
break;
case MAVLINK_MSG_ID_MISSION_ITEM_REACHED:
// FIXME: NYI
break;
case MAVLINK_MSG_ID_MISSION_CURRENT:
_handleMissionCurrent(message);
break;
case MAVLINK_MSG_ID_MISSION_ITEM:
_handleMissionItem(message, false /* missionItemInt */);
break;
case MAVLINK_MSG_ID_MISSION_ITEM_INT:
_handleMissionItem(message, true /* missionItemInt */);
break;
case MAVLINK_MSG_ID_MISSION_REQUEST:
_handleMissionRequest(message, false /* missionItemInt */);
break;
case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
_handleMissionRequest(message, true /* missionItemInt */);
break;
case MAVLINK_MSG_ID_MISSION_ACK:
_handleMissionAck(message);
break;
case MAVLINK_MSG_ID_MISSION_ITEM_REACHED:
// FIXME: NYI
break;
case MAVLINK_MSG_ID_MISSION_CURRENT:
_handleMissionCurrent(message);
break;
}
}
......
src/MissionManager/MissionManager.h
View file @ 0dcb10c7
......@@ -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);
......
src/Vehicle/Vehicle.cc
View file @ 0dcb10c7
......@@ -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)));
......
src/Vehicle/Vehicle.h
View file @ 0dcb10c7
......@@ -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