Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Tom Pittenger 2016-03-14 11:42:49 -07:00
parent 02eeb2d4f0
commit 9073ac91ff
1 changed files with 6 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_Mission/AP_Mission.cpp
View File

@ -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 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.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_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; break;
@ -543,6 +544,7 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_
copy_location = true; 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.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_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; break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH: // MAV ID: 20 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; copy_location = true;
cmd.p1 = fabsf(packet.param2); // param2 is radius in meters cmd.p1 = fabsf(packet.param2); // param2 is radius in meters
cmd.content.location.flags.loiter_ccw = (packet.param2 < 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; break;
case MAV_CMD_NAV_SPLINE_WAYPOINT: // MAV ID: 82 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) { if (cmd.content.location.flags.loiter_ccw) {
packet.param3 = -packet.param3; 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; break;
case MAV_CMD_NAV_LOITER_TIME: // MAV ID: 19 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 { } else {
packet.param3 = 1; 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; break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH: // MAV ID: 20 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) { if (cmd.content.location.flags.loiter_ccw) {
packet.param2 = -packet.param2; 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; break;
case MAV_CMD_NAV_SPLINE_WAYPOINT: // MAV ID: 82 case MAV_CMD_NAV_SPLINE_WAYPOINT: // MAV ID: 82