From fb07b8c4cc12160e1903520490f0b3d64400bdca Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Mon, 6 Jun 2016 02:55:22 -0600 Subject: [PATCH] Mission: fix bounds checking of MISSION_ITEM lat/lon --- libraries/AP_Mission/AP_Mission.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index b8471c549e..75c3133616 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -859,8 +859,16 @@ MAV_MISSION_RESULT AP_Mission::mavlink_to_mission_cmd(const mavlink_mission_item default: // all other commands use x and y as lat/lon. We need to // multiply by 1e7 to convert to int32_t - mav_cmd.x = packet.x * 1.0e7f; - mav_cmd.y = packet.y * 1.0e7f; + if (fabsf(packet.x) < 90.0f) { + mav_cmd.x = packet.x * 1.0e7f; + } else { + return MAV_MISSION_INVALID_PARAM5_X; + } + if (fabsf(packet.y) < 180.0f) { + mav_cmd.y = packet.y * 1.0e7f; + } else { + return MAV_MISSION_INVALID_PARAM6_Y; + } break; }