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

AP_Mission: waypoint command parsing

This commit is contained in:
Eugene Shamaev 2016-12-18 16:39:44 +02:00 committed by Tom Pittenger
parent 5bf45dbffd
commit dc3d10a28b
1 changed files with 11 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_Mission/AP_Mission.cpp
View File

@ -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;