diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 3d483b2dee..8acdc4586e 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -864,7 +864,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) // Sets the region of interest (ROI) for the camera case MAV_CMD_DO_SET_ROI: // sanity check location - if (fabsf(packet.param5) > 90.0f || fabsf(packet.param6) > 180.0f) { + if (!check_latlng(packet.param5, packet.param6)) { break; } Location roi_loc; @@ -1095,7 +1095,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) break; } // sanity check location - if (fabsf(packet.param5) > 90.0f || fabsf(packet.param6) > 180.0f) { + if (!check_latlng(packet.param5, packet.param6)) { break; } Location new_home_loc {}; @@ -1268,6 +1268,11 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) mavlink_hil_state_t packet; mavlink_msg_hil_state_decode(msg, &packet); + // sanity check location + if (!check_latlng(packet.lat, packet.lon)) { + break; + } + // set gps hil sensor Location loc; loc.lat = packet.lat;