fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 05:03:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Move VEHICLE_CMD_CONTROL_HIGH_LATENCY ack to commander

Browse Source
This commit is contained in:
acfloria
2018-04-11 15:15:28 +02:00
committed by Beat Küng
parent 975d7ddcb2
commit ae807fc189
2 changed files with 18 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+5 -1
View File
@@ -1049,6 +1049,11 @@ Commander::handle_command(vehicle_status_s *status_local,
} }
} }
break; break;
case vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY: {
// only send the acknowledge from the commander, the command actually is handled by each mavlink instance
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
}
break;
case vehicle_command_s::VEHICLE_CMD_CUSTOM_0: case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
case vehicle_command_s::VEHICLE_CMD_CUSTOM_1: case vehicle_command_s::VEHICLE_CMD_CUSTOM_1:
case vehicle_command_s::VEHICLE_CMD_CUSTOM_2: case vehicle_command_s::VEHICLE_CMD_CUSTOM_2:
@@ -1080,7 +1085,6 @@ Commander::handle_command(vehicle_status_s *status_local,
case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION: case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION:
case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET: case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET:
case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE: case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE:
case vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY:
/* ignore commands that are handled by other parts of the system */ /* ignore commands that are handled by other parts of the system */
break; break;
src/modules/mavlink/mavlink_main.cpp
+13 -48
View File
@@ -1976,9 +1976,6 @@ Mavlink::task_main(int argc, char *argv[])
struct vehicle_status_s status; struct vehicle_status_s status;
status_sub->update(&status_time, &status); status_sub->update(&status_time, &status);
/* command ack */
orb_advert_t command_ack_pub = nullptr;
/* Activate sending the data by default (for the IRIDIUM mode it will be disabled after the first round of packages is sent)*/ /* Activate sending the data by default (for the IRIDIUM mode it will be disabled after the first round of packages is sent)*/
_transmitting_enabled = true; _transmitting_enabled = true;
_transmitting_enabled_commanded = true; _transmitting_enabled_commanded = true;
@@ -2203,53 +2200,23 @@ Mavlink::task_main(int argc, char *argv[])
struct vehicle_command_s vehicle_cmd; struct vehicle_command_s vehicle_cmd;
if (cmd_sub->update(&cmd_time, &vehicle_cmd)) { if (cmd_sub->update(&cmd_time, &vehicle_cmd)) {
if (vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) { if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) &&
bool send_ack = false; (_mode == MAVLINK_MODE_IRIDIUM)) {
if (vehicle_cmd.param1 > 0.5f) {
if (_mode == MAVLINK_MODE_IRIDIUM) { if (!_transmitting_enabled) {
if ((vehicle_cmd.param1 > 0.5f)) { PX4_INFO("Enable transmitting with IRIDIUM mavlink on device %s by command", _device_name);
if (!_transmitting_enabled) {
PX4_INFO("Enable transmitting with IRIDIUM mavlink on device %s by command", _device_name);
}
_transmitting_enabled = true;
_transmitting_enabled_commanded = true;
send_ack = true;
} else if ((vehicle_cmd.param1 <= 0.5f)) {
if (_transmitting_enabled) {
PX4_INFO("Disable transmitting with IRIDIUM mavlink on device %s by command", _device_name);
}
_transmitting_enabled = false;
_transmitting_enabled_commanded = false;
} }
}
if ((_mode != MAVLINK_MODE_IRIDIUM) && (vehicle_cmd.param1 <= 0.5f)) { _transmitting_enabled = true;
send_ack = true; _transmitting_enabled_commanded = true;
}
if (send_ack) { } else {
/* publish ACK */ if (_transmitting_enabled) {
struct vehicle_command_ack_s command_ack = { PX4_INFO("Disable transmitting with IRIDIUM mavlink on device %s by command", _device_name);
.timestamp = vehicle_cmd.timestamp,
.result_param2 = 0,
.command = vehicle_cmd.command,
.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED,
.from_external = !vehicle_cmd.from_external,
.result_param1 = 0,
.target_system = vehicle_cmd.source_system,
.target_component = vehicle_cmd.source_component
};
if (command_ack_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack);
} else {
command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
} }
_transmitting_enabled = false;
_transmitting_enabled_commanded = false;
} }
} }
} }
@@ -2487,8 +2454,6 @@ Mavlink::task_main(int argc, char *argv[])
_mavlink_ulog = nullptr; _mavlink_ulog = nullptr;
} }
orb_unadvertise(command_ack_pub);
PX4_INFO("exiting channel %i", (int)_channel); PX4_INFO("exiting channel %i", (int)_channel);
return OK; return OK;