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

APM: fixed fetch of relative altitude waypoints 

when we fetch relative altitude waypoints from APM to a file, we need
to preserve the altitude
This commit is contained in:
Andrew Tridgell 2012-09-10 09:40:29 +10:00
parent 90b74f5bab
commit e2779523ea
2 changed files with 19 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduPlane/GCS_Mavlink.pde
View File

@ -1191,7 +1191,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break; break;
// send waypoint // send waypoint
tell_command = get_cmd_with_index(packet.seq); tell_command = get_cmd_with_index_raw(packet.seq);
// set frame of waypoint // set frame of waypoint
uint8_t frame; uint8_t frame;
@ -1218,11 +1218,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// command needs scaling // command needs scaling
x = tell_command.lat/1.0e7; // local (x), global (latitude) x = tell_command.lat/1.0e7; // local (x), global (latitude)
y = tell_command.lng/1.0e7; // local (y), global (longitude) y = tell_command.lng/1.0e7; // local (y), global (longitude)
if ((tell_command.options & MASK_OPTIONS_RELATIVE_ALT) && tell_command.id != MAV_CMD_NAV_TAKEOFF) { z = tell_command.alt/1.0e2;
z = (tell_command.alt - home.alt) / 1.0e2; // because tell_command.alt already includes a += home.alt
} else {
z = tell_command.alt/1.0e2; // local (z), global/relative (altitude)
}
} }
switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields

21
ArduPlane/commands.pde
View File

@ -38,9 +38,10 @@ static void reload_commands_airstart()
decrement_cmd_index(); decrement_cmd_index();
} }
// Getters /*
// ------- fetch a mission item from EEPROM
static struct Location get_cmd_with_index(int16_t i) */
static struct Location get_cmd_with_index_raw(int16_t i)
{ {
struct Location temp; struct Location temp;
uint16_t mem; uint16_t mem;
@ -71,6 +72,18 @@ static struct Location get_cmd_with_index(int16_t i)
temp.lng = (long)eeprom_read_dword((uint32_t*)(uintptr_t)mem); temp.lng = (long)eeprom_read_dword((uint32_t*)(uintptr_t)mem);
} }
return temp;
}
/*
fetch a mission item from EEPROM. Adjust altitude to be absolute
*/
static struct Location get_cmd_with_index(int16_t i)
{
struct Location temp;
temp = get_cmd_with_index_raw(i);
// Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative // Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative
if ((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && if ((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) &&
(temp.options & MASK_OPTIONS_RELATIVE_ALT) && (temp.options & MASK_OPTIONS_RELATIVE_ALT) &&
@ -90,7 +103,7 @@ static void set_cmd_with_index(struct Location temp, int16_t i)
// Set altitude options bitmask // Set altitude options bitmask
// XXX What is this trying to do? // XXX What is this trying to do?
if (temp.options & MASK_OPTIONS_RELATIVE_ALT && i != 0) { if ((temp.options & MASK_OPTIONS_RELATIVE_ALT) && i != 0) {
temp.options = MASK_OPTIONS_RELATIVE_ALT; temp.options = MASK_OPTIONS_RELATIVE_ALT;
} else { } else {
temp.options = 0; temp.options = 0;