Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: cope with NaNs being passed in when doing conversion to command_int

This commit is contained in:
Peter Barker 2023-10-05 16:48:14 +11:00 committed by Peter Barker
parent 3b2dec7ec6
commit 77945be031
1 changed files with 18 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -4895,6 +4895,21 @@ bool GCS_MAVLINK::command_long_stores_location(const MAV_CMD command)
return false; return false;
} }
// returns a value suitable for COMMAND_INT.x or y based on a value
// coming in from COMMAND_LONG.p5 or p6:
static int32_t convert_COMMAND_LONG_loc_param(float param, bool stores_location)
{
if (isnan(param)) {
return 0;
}
if (stores_location) {
return param *1e7;
}
return param;
}
void GCS_MAVLINK::convert_COMMAND_LONG_to_COMMAND_INT(const mavlink_command_long_t &in, mavlink_command_int_t &out, MAV_FRAME frame) void GCS_MAVLINK::convert_COMMAND_LONG_to_COMMAND_INT(const mavlink_command_long_t &in, mavlink_command_int_t &out, MAV_FRAME frame)
{ {
out = {}; out = {};
@ -4908,13 +4923,9 @@ void GCS_MAVLINK::convert_COMMAND_LONG_to_COMMAND_INT(const mavlink_command_long
out.param2 = in.param2; out.param2 = in.param2;
out.param3 = in.param3; out.param3 = in.param3;
out.param4 = in.param4; out.param4 = in.param4;
if (command_long_stores_location((MAV_CMD)in.command)) { const bool stores_location = command_long_stores_location((MAV_CMD)in.command);
out.x = in.param5 *1e7; out.x = convert_COMMAND_LONG_loc_param(in.param5, stores_location);
out.y = in.param6 *1e7; out.y = convert_COMMAND_LONG_loc_param(in.param6, stores_location);
} else {
out.x = in.param5;
out.y = in.param6;
}
out.z = in.param7; out.z = in.param7;
} }