commands_logic: Removed some old unused code

This commit is contained in:
Jason Short 2012-07-03 17:19:19 -07:00
parent 4a2e9b692d
commit 8a2df85ee8
1 changed files with 5 additions and 18 deletions

View File

@ -216,14 +216,8 @@ static void do_takeoff()
// Start with current location // Start with current location
Location temp = current_loc; Location temp = current_loc;
// command_nav_queue.alt is a relative altitude!!! // alt is always relative
if (command_nav_queue.options & MASK_OPTIONS_RELATIVE_ALT) { temp.alt = command_nav_queue.alt;
temp.alt = command_nav_queue.alt + home.alt;
//Serial.printf("rel alt: %ld",temp.alt);
} else {
temp.alt = command_nav_queue.alt;
//Serial.printf("abs alt: %ld",temp.alt);
}
// prevent flips // prevent flips
reset_I_all(); reset_I_all();
@ -236,10 +230,6 @@ static void do_nav_wp()
{ {
wp_control = WP_MODE; wp_control = WP_MODE;
// command_nav_queue.alt is a relative altitude!!!
if (command_nav_queue.options & MASK_OPTIONS_RELATIVE_ALT) {
command_nav_queue.alt += home.alt;
}
set_next_WP(&command_nav_queue); set_next_WP(&command_nav_queue);
// this is our bitmask to verify we have met all conditions to move on // this is our bitmask to verify we have met all conditions to move on
@ -251,9 +241,7 @@ static void do_nav_wp()
// this is the delay, stored in seconds and expanded to millis // this is the delay, stored in seconds and expanded to millis
loiter_time_max = command_nav_queue.p1 * 1000; loiter_time_max = command_nav_queue.p1 * 1000;
// if we don't require an altitude minimum, we save this flag as passed (1) if((next_WP.options & WP_OPTION_ALT_REQUIRED) == false){
if((next_WP.options & MASK_OPTIONS_RELATIVE_ALT) == 0){
// we don't need to worry about it
wp_verify_byte |= NAV_ALTITUDE; wp_verify_byte |= NAV_ALTITUDE;
} }
} }
@ -428,10 +416,9 @@ static bool verify_land_baro()
static bool verify_nav_wp() static bool verify_nav_wp()
{ {
// Altitude checking // Altitude checking
if(next_WP.options & MASK_OPTIONS_RELATIVE_ALT){ if(next_WP.options & WP_OPTION_ALT_REQUIRED){
// we desire a certain minimum altitude // we desire a certain minimum altitude
//if (current_loc.alt > next_WP.alt){ if(alt_change_flag == REACHED_ALT){
if (current_loc.alt > target_altitude){
// we have reached that altitude // we have reached that altitude
wp_verify_byte |= NAV_ALTITUDE; wp_verify_byte |= NAV_ALTITUDE;