diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d1c3326aca..df877ef5b7 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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;