fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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-06 17:45:02 +02:00 committed by Beat Küng
parent 975d7ddcb2
commit ae807fc189
2 changed files with 18 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/commander/commander.cpp
View File

@ -1049,6 +1049,11 @@ Commander::handle_command(vehicle_status_s *status_local,
}
}
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_1:
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_WPNEXT_OFFSET:
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 */
break;

61
src/modules/mavlink/mavlink_main.cpp
View File

@ -1976,9 +1976,6 @@ Mavlink::task_main(int argc, char *argv[])
struct vehicle_status_s 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)*/
_transmitting_enabled = true;
_transmitting_enabled_commanded = true;
@ -2203,53 +2200,23 @@ Mavlink::task_main(int argc, char *argv[])
struct vehicle_command_s vehicle_cmd;
if (cmd_sub->update(&cmd_time, &vehicle_cmd)) {
if (vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) {
bool send_ack = false;
if (_mode == MAVLINK_MODE_IRIDIUM) {
if ((vehicle_cmd.param1 > 0.5f)) {
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 ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) &&
(_mode == MAVLINK_MODE_IRIDIUM)) {
if (vehicle_cmd.param1 > 0.5f) {
if (!_transmitting_enabled) {
PX4_INFO("Enable transmitting with IRIDIUM mavlink on device %s by command", _device_name);
}
}
if ((_mode != MAVLINK_MODE_IRIDIUM) && (vehicle_cmd.param1 <= 0.5f)) {
send_ack = true;
}
_transmitting_enabled = true;
_transmitting_enabled_commanded = true;
if (send_ack) {
/* publish ACK */
struct vehicle_command_ack_s command_ack = {
.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);
} else {
if (_transmitting_enabled) {
PX4_INFO("Disable transmitting with IRIDIUM mavlink on device %s by command", _device_name);
}
_transmitting_enabled = false;
_transmitting_enabled_commanded = false;
}
}
}
@ -2487,8 +2454,6 @@ Mavlink::task_main(int argc, char *argv[])
_mavlink_ulog = nullptr;
}
orb_unadvertise(command_ack_pub);
PX4_INFO("exiting channel %i", (int)_channel);
return OK;