Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

mavlink: mavlink main vehicle_commands only handled in iridium mode

Browse Source
This commit is contained in:
Daniel Agar
2021-03-11 14:17:12 -05:00
parent eef304110d
commit b0a4f35024
1 changed files with 39 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+39 -36
View File
@@ -2247,45 +2247,48 @@ Mavlink::task_main(int argc, char *argv[])
// vehicle_command
while (_vehicle_command_sub.updated()) {
const unsigned last_generation = _vehicle_command_sub.get_last_generation();
vehicle_command_s vehicle_cmd;
if (_mode == MAVLINK_MODE_IRIDIUM) {
while (_vehicle_command_sub.updated()) {
const unsigned last_generation = _vehicle_command_sub.get_last_generation();
vehicle_command_s vehicle_cmd;
if (_vehicle_command_sub.update(&vehicle_cmd)) {
if (_vehicle_command_sub.get_last_generation() != last_generation + 1) {
PX4_ERR("vehicle_command lost, generation %d -> %d", last_generation, _vehicle_command_sub.get_last_generation());
}
if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) &&
(_mode == MAVLINK_MODE_IRIDIUM)) {
if (vehicle_cmd.param1 > 0.5f) {
if (!_transmitting_enabled) {
mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s by command",
_device_name);
}
_transmitting_enabled = true;
_transmitting_enabled_commanded = true;
} else {
if (_transmitting_enabled) {
mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s by command",
_device_name);
}
_transmitting_enabled = false;
_transmitting_enabled_commanded = false;
if (_vehicle_command_sub.update(&vehicle_cmd)) {
if (_vehicle_command_sub.get_last_generation() != last_generation + 1) {
PX4_ERR("vehicle_command lost, generation %d -> %d", last_generation, _vehicle_command_sub.get_last_generation());
}
// send positive command ack
vehicle_command_ack_s command_ack{};
command_ack.command = vehicle_cmd.command;
command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
command_ack.from_external = !vehicle_cmd.from_external;
command_ack.target_system = vehicle_cmd.source_system;
command_ack.target_component = vehicle_cmd.source_component;
command_ack.timestamp = vehicle_cmd.timestamp;
_vehicle_command_ack_pub.publish(command_ack);
if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) &&
_mode == MAVLINK_MODE_IRIDIUM) {
if (vehicle_cmd.param1 > 0.5f) {
if (!_transmitting_enabled) {
mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s by command",
_device_name);
}
_transmitting_enabled = true;
_transmitting_enabled_commanded = true;
} else {
if (_transmitting_enabled) {
mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s by command",
_device_name);
}
_transmitting_enabled = false;
_transmitting_enabled_commanded = false;
}
// send positive command ack
vehicle_command_ack_s command_ack{};
command_ack.command = vehicle_cmd.command;
command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
command_ack.from_external = !vehicle_cmd.from_external;
command_ack.target_system = vehicle_cmd.source_system;
command_ack.target_component = vehicle_cmd.source_component;
command_ack.timestamp = vehicle_cmd.timestamp;
_vehicle_command_ack_pub.publish(command_ack);
}
}
}
}