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

Vehicle send COMMAND_INT support

QGC4.4
DonLakeFlyer 7 years ago
parent
beff8561d9
commit
3e7254aa2c
3 changed files with 100 additions and 35 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. 8
      src/MissionManager/PlanManager.cc
  2. 116
      src/Vehicle/Vehicle.cc
  3. 11
      src/Vehicle/Vehicle.h

8
src/MissionManager/PlanManager.cc
Unescape View File

@ -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);
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 @@ -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;

116
src/Vehicle/Vehicle.cc
Unescape View File

@ -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 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, @@ -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 @@ -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 @@ -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) @@ -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);
}

11
src/Vehicle/Vehicle.h
Unescape View File

@ -840,6 +840,7 @@ public: @@ -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: @@ -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;

Write Preview
Loading…
Cancel
Save