From 84252405c35f1005fb15aea75b039963ebfa2d5e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 27 Aug 2015 14:36:15 +0900 Subject: [PATCH] Rover: sanity check ROI target --- APMrover2/GCS_Mavlink.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 45032eee9e..98f3829288 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -886,6 +886,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) #if MOUNT == ENABLED // 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) { + break; + } Location roi_loc; roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f); roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f);