dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavlink: improve command param5/6, x/y handling 

This adds support to handle INT32_MAX for COMMAND_INT.x/y by converting
it to NAN internally.

It also adds paranoid checks to prevent:
- NAN being used sent by a COMMAND_INT by mistake.
- INT32_MAX being used sent by a COMMAND_LONG by mistake.
This commit is contained in:
Julian Oes 2021-05-12 09:27:17 +02:00
parent 4bb53e2afc
commit 1f40a65210
1 changed files with 30 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -373,6 +373,18 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
vcmd.timestamp = hrt_absolute_time();
const float before_int32_max = nextafterf((float)INT32_MAX, 0.0f);
const float after_int32_max = nextafterf((float)INT32_MAX, (float)INFINITY);
if (cmd_mavlink.param5 >= before_int32_max && cmd_mavlink.param5 <= after_int32_max &&
cmd_mavlink.param6 >= before_int32_max && cmd_mavlink.param6 <= after_int32_max) {
// This looks suspicously like INT32_MAX was sent in a COMMAND_LONG instead of
// a COMMAND_INT.
PX4_ERR("param5/param6 invalid of command %" PRIu16, cmd_mavlink.command);
acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, vehicle_command_ack_s::VEHICLE_RESULT_DENIED);
return;
}
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
vcmd.param1 = cmd_mavlink.param1;
vcmd.param2 = cmd_mavlink.param2;
@ -402,13 +414,29 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
vehicle_command_s vcmd{};
vcmd.timestamp = hrt_absolute_time();
if (cmd_mavlink.x == 0x7ff80000 && cmd_mavlink.y == 0x7ff80000) {
// This looks like NAN was by accident sent as int.
PX4_ERR("x/y invalid of command %" PRIu16, cmd_mavlink.command);
acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, vehicle_command_ack_s::VEHICLE_RESULT_DENIED);
return;
}
/* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */
vcmd.param1 = cmd_mavlink.param1;
vcmd.param2 = cmd_mavlink.param2;
vcmd.param3 = cmd_mavlink.param3;
vcmd.param4 = cmd_mavlink.param4;
vcmd.param5 = ((double)cmd_mavlink.x) / 1e7;
vcmd.param6 = ((double)cmd_mavlink.y) / 1e7;
if (cmd_mavlink.x == INT32_MAX && cmd_mavlink.y == INT32_MAX) {
// INT32_MAX for x and y means to ignore it.
vcmd.param5 = (double)NAN;
vcmd.param6 = (double)NAN;
} else {
vcmd.param5 = ((double)cmd_mavlink.x) / 1e7;
vcmd.param6 = ((double)cmd_mavlink.y) / 1e7;
}
vcmd.param7 = cmd_mavlink.z;
vcmd.command = cmd_mavlink.command;
vcmd.target_system = cmd_mavlink.target_system;