From 9073ac91ffcb45b1a3ebe57793605c85afc0daeb Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Mon, 14 Mar 2016 11:42:49 -0700 Subject: [PATCH] AP_Mission: 2of3 add loiter_xtrack option flag for post-loiter navigation via param4 0 to crosstrack from center of waypoint, 1 to crosstrack from tangent exit location --- libraries/AP_Mission/AP_Mission.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index edec9356a1..089af90be1 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -536,6 +536,7 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ uint16_t radius_m = fabsf(packet.param3); // param 3 is radius in meters is held in high p1 cmd.p1 = (radius_m<<8) | (num_turns & 0x00FF); // store radius in high byte of p1, num turns in low byte of p1 cmd.content.location.flags.loiter_ccw = (packet.param3 < 0); + cmd.content.location.flags.loiter_xtrack = (packet.param4 > 0); // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location } break; @@ -543,6 +544,7 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ copy_location = true; cmd.p1 = packet.param1; // loiter time in seconds uses all 16 bits, 8bit seconds is too small. No room for radius. cmd.content.location.flags.loiter_ccw = (packet.param3 < 0); + cmd.content.location.flags.loiter_xtrack = (packet.param4 > 0); // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location break; case MAV_CMD_NAV_RETURN_TO_LAUNCH: // MAV ID: 20 @@ -571,6 +573,7 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ copy_location = true; cmd.p1 = fabsf(packet.param2); // param2 is radius in meters cmd.content.location.flags.loiter_ccw = (packet.param2 < 0); + cmd.content.location.flags.loiter_xtrack = (packet.param4 > 0); // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location break; case MAV_CMD_NAV_SPLINE_WAYPOINT: // MAV ID: 82 @@ -950,6 +953,7 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c if (cmd.content.location.flags.loiter_ccw) { packet.param3 = -packet.param3; } + packet.param4 = cmd.content.location.flags.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location break; case MAV_CMD_NAV_LOITER_TIME: // MAV ID: 19 @@ -960,6 +964,7 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c } else { packet.param3 = 1; } + packet.param4 = cmd.content.location.flags.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location break; case MAV_CMD_NAV_RETURN_TO_LAUNCH: // MAV ID: 20 @@ -990,6 +995,7 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c if (cmd.content.location.flags.loiter_ccw) { packet.param2 = -packet.param2; } + packet.param4 = cmd.content.location.flags.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location break; case MAV_CMD_NAV_SPLINE_WAYPOINT: // MAV ID: 82