Skip to content

Q
qgroundcontrol
Unverified Commit 4dc7b9d6 authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files
Options

Merge pull request #6611 from DonLakeFlyer/CommandInt 

Vehicle send COMMAND_INT support
parents 5dcaf18b 3e7254aa
Hide whitespace changes
Inline Side-by-side
Showing with 100 additions and 35 deletions
src/MissionManager/PlanManager.cc
View file @ 4dc7b9d6
...@@ -392,8 +392,8 @@ void PlanManager::_handleMissionItem(const mavlink_message_t& message, bool miss ...@@ -392,8 +392,8 @@ void PlanManager::_handleMissionItem(const mavlink_message_t& message, bool miss
mavlink_msg_mission_item_int_decode(&message, &missionItem); mavlink_msg_mission_item_int_decode(&message, &missionItem);
command = (MAV_CMD)missionItem.command, command = (MAV_CMD)missionItem.command,
frame = (MAV_FRAME)missionItem.frame, frame = (MAV_FRAME)missionItem.frame,
param1 = missionItem.param1; param1 = missionItem.param1;
param2 = missionItem.param2; param2 = missionItem.param2;
param3 = missionItem.param3; param3 = missionItem.param3;
param4 = missionItem.param4; param4 = missionItem.param4;
...@@ -408,8 +408,8 @@ void PlanManager::_handleMissionItem(const mavlink_message_t& message, bool miss ...@@ -408,8 +408,8 @@ void PlanManager::_handleMissionItem(const mavlink_message_t& message, bool miss
mavlink_msg_mission_item_decode(&message, &missionItem); mavlink_msg_mission_item_decode(&message, &missionItem);
command = (MAV_CMD)missionItem.command, command = (MAV_CMD)missionItem.command,
frame = (MAV_FRAME)missionItem.frame, frame = (MAV_FRAME)missionItem.frame,
param1 = missionItem.param1; param1 = missionItem.param1;
param2 = missionItem.param2; param2 = missionItem.param2;
param3 = missionItem.param3; param3 = missionItem.param3;
param4 = missionItem.param4; param4 = missionItem.param4;
......
src/Vehicle/Vehicle.cc
View file @ 4dc7b9d6
...@@ -1062,6 +1062,7 @@ void Vehicle::_setCapabilities(uint64_t capabilityBits) ...@@ -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 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_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 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); 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, ...@@ -2767,14 +2768,26 @@ void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius,
alt = amslAltitude; alt = amslAltitude;
} }
sendMavCommand(defaultComponentId(), if (capabilityBits() && MAV_PROTOCOL_CAPABILITY_COMMAND_INT) {
MAV_CMD_DO_ORBIT, sendMavCommandInt(defaultComponentId(),
true, // show error if fails MAV_CMD_DO_ORBIT,
radius, MAV_FRAME_GLOBAL,
qQNaN(), // Use default velocity true, // show error if fails
0, // Vehicle points to center radius,
qQNaN(), // reserved qQNaN(), // Use default velocity
lat, lon, alt); 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) void Vehicle::pauseVehicle(void)
...@@ -2833,6 +2846,7 @@ void Vehicle::sendMavCommand(int component, MAV_CMD command, bool showError, flo ...@@ -2833,6 +2846,7 @@ void Vehicle::sendMavCommand(int component, MAV_CMD command, bool showError, flo
{ {
MavCommandQueueEntry_t entry; MavCommandQueueEntry_t entry;
entry.commandInt = false;
entry.component = component; entry.component = component;
entry.command = command; entry.command = command;
entry.showError = showError; entry.showError = showError;
...@@ -2852,6 +2866,31 @@ void Vehicle::sendMavCommand(int component, MAV_CMD command, bool showError, flo ...@@ -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) void Vehicle::_sendMavCommandAgain(void)
{ {
if(!_mavCommandQueue.size()) { if(!_mavCommandQueue.size()) {
...@@ -2918,25 +2957,48 @@ void Vehicle::_sendMavCommandAgain(void) ...@@ -2918,25 +2957,48 @@ void Vehicle::_sendMavCommandAgain(void)
_mavCommandAckTimer.start(); _mavCommandAckTimer.start();
mavlink_message_t msg; mavlink_message_t msg;
mavlink_command_long_t cmd; if (queuedCommand.commandInt) {
mavlink_command_int_t cmd;
memset(&cmd, 0, sizeof(cmd));
cmd.command = queuedCommand.command; memset(&cmd, 0, sizeof(cmd));
cmd.confirmation = 0; cmd.target_system = _id;
cmd.param1 = queuedCommand.rgParam[0]; cmd.target_component = queuedCommand.component;
cmd.param2 = queuedCommand.rgParam[1]; cmd.command = queuedCommand.command;
cmd.param3 = queuedCommand.rgParam[2]; cmd.frame = queuedCommand.frame;
cmd.param4 = queuedCommand.rgParam[3]; cmd.param1 = queuedCommand.rgParam[0];
cmd.param5 = queuedCommand.rgParam[4]; cmd.param2 = queuedCommand.rgParam[1];
cmd.param6 = queuedCommand.rgParam[5]; cmd.param3 = queuedCommand.rgParam[2];
cmd.param7 = queuedCommand.rgParam[6]; cmd.param4 = queuedCommand.rgParam[3];
cmd.target_system = _id; cmd.x = queuedCommand.rgParam[4] * qPow(10.0, 7.0);
cmd.target_component = queuedCommand.component; cmd.y = queuedCommand.rgParam[5] * qPow(10.0, 7.0);
mavlink_msg_command_long_encode_chan(_mavlink->getSystemId(), cmd.z = queuedCommand.rgParam[6];
_mavlink->getComponentId(), mavlink_msg_command_int_encode_chan(_mavlink->getSystemId(),
priorityLink()->mavlinkChannel(), _mavlink->getComponentId(),
&msg, priorityLink()->mavlinkChannel(),
&cmd); &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); sendMessageOnLink(priorityLink(), msg);
} }
......
src/Vehicle/Vehicle.h
View file @ 4dc7b9d6
...@@ -840,6 +840,7 @@ public: ...@@ -840,6 +840,7 @@ public:
/// @param showError true: Display error to user if command failed, false: no error shown /// @param showError true: Display error to user if command failed, false: no error shown
/// Signals: mavCommandResult on success or failure /// 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 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. /// 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) 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: ...@@ -1179,10 +1180,12 @@ private:
QGCCameraManager* _cameras; QGCCameraManager* _cameras;
typedef struct { typedef struct {
int component; int component;
MAV_CMD command; bool commandInt; // true: use COMMAND_INT, false: use COMMAND_LONG
float rgParam[7]; MAV_CMD command;
bool showError; MAV_FRAME frame;
double rgParam[7];
bool showError;
} MavCommandQueueEntry_t; } MavCommandQueueEntry_t;
QList<MavCommandQueueEntry_t> _mavCommandQueue; QList<MavCommandQueueEntry_t> _mavCommandQueue;
......
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