From dc3d10a28b7e49b54579a3475d261348841abfc8 Mon Sep 17 00:00:00 2001 From: Eugene Shamaev Date: Sun, 18 Dec 2016 16:39:44 +0200 Subject: [PATCH] AP_Mission: waypoint command parsing --- libraries/AP_Mission/AP_Mission.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index de3b789511..25786fee71 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -526,6 +526,7 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ return MAV_MISSION_INVALID; case MAV_CMD_NAV_WAYPOINT: // MAV ID: 16 + { copy_location = true; /* the 15 byte limit means we can't fit both delay and radius @@ -533,12 +534,15 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ we can do this properly */ #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) - // acceptance radius in meters - cmd.p1 = packet.param2; + // acceptance radius in meters and pass by distance in meters + uint16_t acp = packet.param2; // param 2 is acceptance radius in meters is held in low p1 + uint16_t passby = packet.param3; // param 3 is pass by distance in meters is held in high p1 + cmd.p1 = (passby << 8) | (acp & 0x00FF); #else - // delay at waypoint in seconds - cmd.p1 = packet.param1; + // delay at waypoint in seconds (this is for copters???) + cmd.p1 = packet.param1; #endif + } break; case MAV_CMD_NAV_LOITER_UNLIM: // MAV ID: 17 @@ -975,7 +979,9 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c copy_location = true; #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) // acceptance radius in meters - packet.param2 = cmd.p1; + + packet.param2 = LOWBYTE(cmd.p1); // param 2 is acceptance radius in meters is held in low p1 + packet.param3 = HIGHBYTE(cmd.p1); // param 3 is pass by distance in meters is held in high p1 #else // delay at waypoint in seconds packet.param1 = cmd.p1;