Skip to content
Snippets Groups Projects
Commit 4ba5be5c authored by Michael Day's avatar Michael Day
Browse files

AP_Mission: NAV_LOITER_TO_ALT now correctly using parameter 1

Parameter 1 is for verifying heading towards next waypoint
before breaking out of loiter (see MAVLink spec).
parent 8263c99a
No related branches found
No related tags found
No related merge requests found
......@@ -595,6 +595,9 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_
case MAV_CMD_NAV_LOITER_TO_ALT: // MAV ID: 31
copy_location = true;
cmd.p1 = fabsf(packet.param2); // param2 is radius in meters
//param1 is whether or not to verify heading towards next waypoint
//before breaking out of loiter
cmd.content.location.flags.loiter_hdg_chk = (packet.param1 != 0);
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;
......@@ -1037,6 +1040,7 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c
case MAV_CMD_NAV_LOITER_TO_ALT: // MAV ID: 31
copy_location = true;
packet.param1 = cmd.content.location.flags.loiter_hdg_chk;
packet.param2 = cmd.p1; // loiter radius(m)
if (cmd.content.location.flags.loiter_ccw) {
packet.param2 = -packet.param2;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment