diff --git a/src/MissionManager/PlanManager.cc b/src/MissionManager/PlanManager.cc index 6ec82d3dfc204855254a7d9e8dc3e6c0fedc3e41..f19f1b100c4d53225c30aaa29458f3f46ef961bc 100644 --- a/src/MissionManager/PlanManager.cc +++ b/src/MissionManager/PlanManager.cc @@ -392,8 +392,8 @@ void PlanManager::_handleMissionItem(const mavlink_message_t& message, bool miss mavlink_msg_mission_item_int_decode(&message, &missionItem); command = (MAV_CMD)missionItem.command, - frame = (MAV_FRAME)missionItem.frame, - param1 = missionItem.param1; + frame = (MAV_FRAME)missionItem.frame, + param1 = missionItem.param1; param2 = missionItem.param2; param3 = missionItem.param3; param4 = missionItem.param4; @@ -408,8 +408,8 @@ void PlanManager::_handleMissionItem(const mavlink_message_t& message, bool miss mavlink_msg_mission_item_decode(&message, &missionItem); command = (MAV_CMD)missionItem.command, - frame = (MAV_FRAME)missionItem.frame, - param1 = missionItem.param1; + frame = (MAV_FRAME)missionItem.frame, + param1 = missionItem.param1; param2 = missionItem.param2; param3 = missionItem.param3; param4 = missionItem.param4; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index eb08ed71883fa0b1ebb6f2fde349db3e38944a9b..e8ea30bf4f5501af1fc9abc3fd070eb921161739 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -1062,6 +1062,7 @@ void Vehicle::_setCapabilities(uint64_t capabilityBits) qCDebug(VehicleLog) << QString("Vehicle %1 Mavlink 2.0").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_MAVLINK2 ? supports : doesNotSupport); qCDebug(VehicleLog) << QString("Vehicle %1 MISSION_ITEM_INT").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_INT ? supports : doesNotSupport); + qCDebug(VehicleLog) << QString("Vehicle %1 MISSION_COMMAND_INT").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_COMMAND_INT ? supports : doesNotSupport); qCDebug(VehicleLog) << QString("Vehicle %1 GeoFence").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE ? supports : doesNotSupport); qCDebug(VehicleLog) << QString("Vehicle %1 RallyPoints").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_RALLY ? supports : doesNotSupport); } @@ -2767,14 +2768,26 @@ void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, alt = amslAltitude; } - sendMavCommand(defaultComponentId(), - MAV_CMD_DO_ORBIT, - true, // show error if fails - radius, - qQNaN(), // Use default velocity - 0, // Vehicle points to center - qQNaN(), // reserved - lat, lon, alt); + if (capabilityBits() && MAV_PROTOCOL_CAPABILITY_COMMAND_INT) { + sendMavCommandInt(defaultComponentId(), + MAV_CMD_DO_ORBIT, + MAV_FRAME_GLOBAL, + true, // show error if fails + radius, + qQNaN(), // Use default velocity + 0, // Vehicle points to center + qQNaN(), // reserved + lat, lon, alt); + } else { + sendMavCommand(defaultComponentId(), + MAV_CMD_DO_ORBIT, + true, // show error if fails + radius, + qQNaN(), // Use default velocity + 0, // Vehicle points to center + qQNaN(), // reserved + lat, lon, alt); + } } void Vehicle::pauseVehicle(void) @@ -2833,6 +2846,7 @@ void Vehicle::sendMavCommand(int component, MAV_CMD command, bool showError, flo { MavCommandQueueEntry_t entry; + entry.commandInt = false; entry.component = component; entry.command = command; entry.showError = showError; @@ -2852,6 +2866,31 @@ void Vehicle::sendMavCommand(int component, MAV_CMD command, bool showError, flo } } +void Vehicle::sendMavCommandInt(int component, MAV_CMD command, MAV_FRAME frame, bool showError, float param1, float param2, float param3, float param4, double param5, double param6, float param7) +{ + MavCommandQueueEntry_t entry; + + entry.commandInt = true; + entry.component = component; + entry.command = command; + entry.frame = frame; + entry.showError = showError; + entry.rgParam[0] = param1; + entry.rgParam[1] = param2; + entry.rgParam[2] = param3; + entry.rgParam[3] = param4; + entry.rgParam[4] = param5; + entry.rgParam[5] = param6; + entry.rgParam[6] = param7; + + _mavCommandQueue.append(entry); + + if (_mavCommandQueue.count() == 1) { + _mavCommandRetryCount = 0; + _sendMavCommandAgain(); + } +} + void Vehicle::_sendMavCommandAgain(void) { if(!_mavCommandQueue.size()) { @@ -2918,25 +2957,48 @@ void Vehicle::_sendMavCommandAgain(void) _mavCommandAckTimer.start(); mavlink_message_t msg; - mavlink_command_long_t cmd; - - memset(&cmd, 0, sizeof(cmd)); - cmd.command = queuedCommand.command; - cmd.confirmation = 0; - cmd.param1 = queuedCommand.rgParam[0]; - cmd.param2 = queuedCommand.rgParam[1]; - cmd.param3 = queuedCommand.rgParam[2]; - cmd.param4 = queuedCommand.rgParam[3]; - cmd.param5 = queuedCommand.rgParam[4]; - cmd.param6 = queuedCommand.rgParam[5]; - cmd.param7 = queuedCommand.rgParam[6]; - cmd.target_system = _id; - cmd.target_component = queuedCommand.component; - mavlink_msg_command_long_encode_chan(_mavlink->getSystemId(), - _mavlink->getComponentId(), - priorityLink()->mavlinkChannel(), - &msg, - &cmd); + if (queuedCommand.commandInt) { + mavlink_command_int_t cmd; + + memset(&cmd, 0, sizeof(cmd)); + cmd.target_system = _id; + cmd.target_component = queuedCommand.component; + cmd.command = queuedCommand.command; + cmd.frame = queuedCommand.frame; + cmd.param1 = queuedCommand.rgParam[0]; + cmd.param2 = queuedCommand.rgParam[1]; + cmd.param3 = queuedCommand.rgParam[2]; + cmd.param4 = queuedCommand.rgParam[3]; + cmd.x = queuedCommand.rgParam[4] * qPow(10.0, 7.0); + cmd.y = queuedCommand.rgParam[5] * qPow(10.0, 7.0); + cmd.z = queuedCommand.rgParam[6]; + mavlink_msg_command_int_encode_chan(_mavlink->getSystemId(), + _mavlink->getComponentId(), + priorityLink()->mavlinkChannel(), + &msg, + &cmd); + } else { + mavlink_message_t msg; + mavlink_command_long_t cmd; + + memset(&cmd, 0, sizeof(cmd)); + cmd.target_system = _id; + cmd.target_component = queuedCommand.component; + cmd.command = queuedCommand.command; + cmd.confirmation = 0; + cmd.param1 = queuedCommand.rgParam[0]; + cmd.param2 = queuedCommand.rgParam[1]; + cmd.param3 = queuedCommand.rgParam[2]; + cmd.param4 = queuedCommand.rgParam[3]; + cmd.param5 = queuedCommand.rgParam[4]; + cmd.param6 = queuedCommand.rgParam[5]; + cmd.param7 = queuedCommand.rgParam[6]; + mavlink_msg_command_long_encode_chan(_mavlink->getSystemId(), + _mavlink->getComponentId(), + priorityLink()->mavlinkChannel(), + &msg, + &cmd); + } sendMessageOnLink(priorityLink(), msg); } diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 03a8c520f033180b0c51e60d1478b210655a94e1..252e230366a632f7b9c9570091ee89f8a584b6f2 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -840,6 +840,7 @@ public: /// @param showError true: Display error to user if command failed, false: no error shown /// Signals: mavCommandResult on success or failure void sendMavCommand(int component, MAV_CMD command, bool showError, float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f); + void sendMavCommandInt(int component, MAV_CMD command, MAV_FRAME frame, bool showError, float param1, float param2, float param3, float param4, double param5, double param6, float param7); /// Same as sendMavCommand but available from Qml. Q_INVOKABLE void sendCommand(int component, int command, bool showError, double param1 = 0.0f, double param2 = 0.0f, double param3 = 0.0f, double param4 = 0.0f, double param5 = 0.0f, double param6 = 0.0f, double param7 = 0.0f) @@ -1179,10 +1180,12 @@ private: QGCCameraManager* _cameras; typedef struct { - int component; - MAV_CMD command; - float rgParam[7]; - bool showError; + int component; + bool commandInt; // true: use COMMAND_INT, false: use COMMAND_LONG + MAV_CMD command; + MAV_FRAME frame; + double rgParam[7]; + bool showError; } MavCommandQueueEntry_t; QList _mavCommandQueue;