Skip to content

Q
qgroundcontrol
Commit 3e7254aa authored by DonLakeFlyer's avatar DonLakeFlyer
Browse files
Options

Vehicle send COMMAND_INT support

parent beff8561
Hide whitespace changes
Inline Side-by-side
Showing with 100 additions and 35 deletions
src/MissionManager/PlanManager.cc
View file @ 3e7254aa
......@@ -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;
......
src/Vehicle/Vehicle.cc
View file @ 3e7254aa
......@@ -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);
}
......
src/Vehicle/Vehicle.h
View file @ 3e7254aa
......@@ -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<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