mirror of https://github.com/ArduPilot/ardupilot
Fix wp altitude issues
git-svn-id: https://arducopter.googlecode.com/svn/trunk@2244 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
80993acc47
commit
d2d166d419
|
@ -532,7 +532,7 @@ void fast_loop()
|
|||
|
||||
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_MODE != HIL_MODE_DISABLED
|
||||
// HIL for a copter needs very fast update of the servo values
|
||||
gcs.send_message(MSG_RADIO_OUT);
|
||||
hil.send_message(MSG_RADIO_OUT);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
|
@ -70,6 +70,11 @@ struct Location get_command_with_index(int i)
|
|||
temp.lng = (long)eeprom_read_dword((uint32_t*)mem); // lon is stored in decimal * 10,000,000
|
||||
}
|
||||
|
||||
// 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) && temp.options & WP_OPTION_ALT_RELATIVE){
|
||||
temp.alt += home.alt;
|
||||
}
|
||||
|
||||
if(temp.options & WP_OPTION_RELATIVE){
|
||||
// If were relative, just offset from home
|
||||
temp.lat += home.lat;
|
||||
|
|
|
@ -226,7 +226,11 @@ void do_takeoff()
|
|||
Location temp = current_loc;
|
||||
|
||||
// next_command.alt is a relative altitude!!!
|
||||
if (next_command.options & WP_OPTION_ALT_RELATIVE) {
|
||||
temp.alt = next_command.alt + home.alt;
|
||||
} else {
|
||||
temp.alt = next_command.alt;
|
||||
}
|
||||
|
||||
takeoff_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction
|
||||
|
||||
|
@ -238,8 +242,9 @@ void do_nav_wp()
|
|||
{
|
||||
wp_control = WP_MODE;
|
||||
|
||||
// no longer needed as get_command_with_index takes care of this
|
||||
// next_command.alt is a relative altitude!!!
|
||||
next_command.alt += home.alt;
|
||||
// next_command.alt += home.alt;
|
||||
|
||||
set_next_WP(&next_command);
|
||||
|
||||
|
@ -467,7 +472,11 @@ void do_change_alt()
|
|||
{
|
||||
Location temp = next_WP;
|
||||
condition_start = current_loc.alt;
|
||||
if (next_command.options & WP_OPTION_ALT_RELATIVE) {
|
||||
condition_value = next_command.alt + home.alt;
|
||||
} else {
|
||||
condition_value = next_command.alt;
|
||||
}
|
||||
temp.alt = condition_value;
|
||||
set_next_WP(&temp);
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue