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

Fix wp altitude issues 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2244 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
mich146@hotmail.com 2011-05-11 11:55:13 +00:00
parent 80993acc47
commit d2d166d419
3 changed files with 18 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopterMega/ArduCopterMega.pde
View File

@ -532,7 +532,7 @@ void fast_loop()
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_MODE != HIL_MODE_DISABLED #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_MODE != HIL_MODE_DISABLED
// HIL for a copter needs very fast update of the servo values // 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 #endif
} }

5
ArduCopterMega/commands.pde
View File

@ -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 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(temp.options & WP_OPTION_RELATIVE){
// If were relative, just offset from home // If were relative, just offset from home
temp.lat += home.lat; temp.lat += home.lat;

11
ArduCopterMega/commands_logic.pde
View File

@ -226,7 +226,11 @@ void do_takeoff()
Location temp = current_loc; Location temp = current_loc;
// next_command.alt is a relative altitude!!! // next_command.alt is a relative altitude!!!
if (next_command.options & WP_OPTION_ALT_RELATIVE) {
temp.alt = next_command.alt + home.alt; 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 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; 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 is a relative altitude!!!
next_command.alt += home.alt; // next_command.alt += home.alt;
set_next_WP(&next_command); set_next_WP(&next_command);
@ -467,7 +472,11 @@ void do_change_alt()
{ {
Location temp = next_WP; Location temp = next_WP;
condition_start = current_loc.alt; condition_start = current_loc.alt;
if (next_command.options & WP_OPTION_ALT_RELATIVE) {
condition_value = next_command.alt + home.alt; condition_value = next_command.alt + home.alt;
} else {
condition_value = next_command.alt;
}
temp.alt = condition_value; temp.alt = condition_value;
set_next_WP(&temp); set_next_WP(&temp);
} }