Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 06:36:45 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

fix mavlink: cmd_logging_{start,stop}_acknowledgement flags were not reset

Browse Source 
Regression from https://github.com/PX4/PX4-Autopilot/pull/23043

Also avoids a race condition by making sure the command ack is handled
before sending out the mavlink message (in case an external component
reacts immediately to the mavlink message).
This commit is contained in:
Beat Küng
2024-04-29 10:33:11 +02:00
parent 36ea872e72
commit 9e6dcb1f60
2 changed files with 25 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+24 -30
View File
@@ -2255,9 +2255,6 @@ Mavlink::task_main(int argc, char *argv[])
_task_running.store(true);
bool cmd_logging_start_acknowledgement = false;
bool cmd_logging_stop_acknowledgement = false;
while (!should_exit()) {
/* main loop */
px4_usleep(_main_loop_delay);
@@ -2266,7 +2263,7 @@ Mavlink::task_main(int argc, char *argv[])
check_requested_subscriptions();
handleStatus();
handleCommands();
handleAndGetCurrentCommandAck(cmd_logging_start_acknowledgement, cmd_logging_stop_acknowledgement);
handleAndGetCurrentCommandAck();
continue;
}
@@ -2291,7 +2288,7 @@ Mavlink::task_main(int argc, char *argv[])
handleStatus();
handleCommands();
handleAndGetCurrentCommandAck(cmd_logging_start_acknowledgement, cmd_logging_stop_acknowledgement);
handleAndGetCurrentCommandAck();
/* check for shell output */
if (_mavlink_shell && _mavlink_shell->available() > 0) {
@@ -2330,25 +2327,15 @@ Mavlink::task_main(int argc, char *argv[])
/* check for ulog streaming messages */
if (_mavlink_ulog) {
if (cmd_logging_stop_acknowledgement) {
const int ret = _mavlink_ulog->handle_update(get_channel());
if (ret < 0) { // abort the streaming on error
if (ret != -1) {
PX4_WARN("mavlink ulog stream update failed, stopping (%i)", ret);
}
_mavlink_ulog->stop();
_mavlink_ulog = nullptr;
} else {
if (cmd_logging_start_acknowledgement) {
_mavlink_ulog->start_ack_received();
}
int ret = _mavlink_ulog->handle_update(get_channel());
if (ret < 0) { //abort the streaming on error
if (ret != -1) {
PX4_WARN("mavlink ulog stream update failed, stopping (%i)", ret);
}
_mavlink_ulog->stop();
_mavlink_ulog = nullptr;
}
}
}
@@ -2571,7 +2558,7 @@ void Mavlink::handleCommands()
}
}
void Mavlink::handleAndGetCurrentCommandAck(bool &logging_start_ack, bool &logging_stop_ack)
void Mavlink::handleAndGetCurrentCommandAck()
{
if (_vehicle_command_ack_sub.updated()) {
static constexpr size_t COMMAND_ACK_TOTAL_LEN = MAVLINK_MSG_ID_COMMAND_ACK_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
@@ -2601,6 +2588,20 @@ void Mavlink::handleAndGetCurrentCommandAck(bool &logging_start_ack, bool &loggi
msg.target_system = command_ack.target_system;
msg.target_component = command_ack.target_component;
// Handle logging acks before sending out the mavlink message to prevent a race condition
if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) {
if (_mavlink_ulog) {
_mavlink_ulog->start_ack_received();
}
} else if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP
&& command_ack.result == vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED) {
if (_mavlink_ulog) {
_mavlink_ulog->stop();
_mavlink_ulog = nullptr;
}
}
if (_mode == MAVLINK_MODE_IRIDIUM) {
if (command_ack.from_external) {
// for MAVLINK_MODE_IRIDIUM send only if external
@@ -2611,13 +2612,6 @@ void Mavlink::handleAndGetCurrentCommandAck(bool &logging_start_ack, bool &loggi
mavlink_msg_command_ack_send_struct(get_channel(), &msg);
}
if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) {
logging_start_ack = true;
} else if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP
&& command_ack.result == vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED) {
logging_stop_ack = true;
}
}
}
}
src/modules/mavlink/mavlink_main.h
+1 -1
View File
@@ -710,7 +710,7 @@ private:
void handleCommands();
void handleAndGetCurrentCommandAck(bool &logging_start_ack, bool &logging_stop_ack);
void handleAndGetCurrentCommandAck();
void handleStatus();