zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Add support for MISSION_ITEM_INT

QGC4.4
DonLakeFlyer 8 years ago
parent
8e95ba8f35
commit
9315d65fb4
4 changed files with 217 additions and 84 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 253
      src/MissionManager/MissionManager.cc
  2. 4
      src/MissionManager/MissionManager.h
  3. 40
      src/Vehicle/Vehicle.cc
  4. 4
      src/Vehicle/Vehicle.h

253
src/MissionManager/MissionManager.cc
Unescape View File

@ -342,50 +342,108 @@ void MissionManager::_requestNextMissionItem(void) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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;
}
}

4
src/MissionManager/MissionManager.h
Unescape View File

@ -88,8 +88,8 @@ private: @@ -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);

40
src/Vehicle/Vehicle.cc
Unescape View File

@ -111,6 +111,8 @@ Vehicle::Vehicle(LinkInterface* link, @@ -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, @@ -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 @@ -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) @@ -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() @@ -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) @@ -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) @@ -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)));

4
src/Vehicle/Vehicle.h
Unescape View File

@ -658,6 +658,7 @@ public: @@ -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: @@ -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: @@ -879,6 +881,8 @@ private:
uint32_t _telemetryTXBuffer;
uint32_t _telemetryLNoise;
uint32_t _telemetryRNoise;
bool _vehicleCapabilitiesKnown;
bool _supportsMissionItemInt;
typedef struct {
int component;

Write Preview
Loading…
Cancel
Save