aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Mission: squeeze loiter turns radius into high byte of p1

Browse Source
This commit is contained in:
Randy Mackay 2014-06-11 16:17:19 +09:00 committed by Andrew Tridgell
parent b79f729540
commit 2f7dee3ba5
1 changed files with 7 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_Mission/AP_Mission.cpp
View File

@ -442,6 +442,7 @@ bool AP_Mission::mavlink_to_mission_cmd(const mavlink_mission_item_t& packet, AP
{
bool copy_location = false;
bool copy_alt = false;
uint8_t num_turns, radius_m; // used by MAV_CMD_NAV_LOITER_TURNS
// command's position in mission list and mavlink id
cmd.index = packet.seq;
@ -462,7 +463,9 @@ bool AP_Mission::mavlink_to_mission_cmd(const mavlink_mission_item_t& packet, AP
case MAV_CMD_NAV_LOITER_TURNS: // MAV ID: 18
copy_location = true;
cmd.p1 = packet.param1; // number of times to circle is held in location.p1
num_turns = packet.param1; // number of times to circle is held in param1
radius_m = fabs(packet.param3); // radius in meters is held in high in param3
cmd.p1 = (((uint16_t)radius_m)<<8) | (uint16_t)num_turns; // store radius in high byte of p1, num turns in low byte of p1
cmd.content.location.flags.loiter_ccw = (packet.param3 < 0);
break;
@ -681,11 +684,10 @@ bool AP_Mission::mission_cmd_to_mavlink(const AP_Mission::Mission_Command& cmd,
case MAV_CMD_NAV_LOITER_TURNS: // MAV ID: 18
copy_location = true;
packet.param1 = cmd.p1; // number of times to circle is held in location.p1
packet.param1 = LOWBYTE(cmd.p1); // number of times to circle is held in low byte of p1
packet.param3 = HIGHBYTE(cmd.p1); // radius is held in high byte of p1
if (cmd.content.location.flags.loiter_ccw) {
packet.param3 = -1;
}else{
packet.param3 = 1;
packet.param3 = -packet.param3;
}
break;