diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 5a5a139a27..ba1be7ab0c 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -655,12 +655,16 @@ static struct Location guided_WP_loc; static struct AP_Mission::Mission_Command auto_rtl_command; //////////////////////////////////////////////////////////////////////////////// -// Altitude / Climb rate control -//////////////////////////////////////////////////////////////////////////////// -// The current desired altitude. Altitude is linearly ramped between waypoints. Centimeters -static int32_t target_altitude_cm; -// Altitude difference between previous and current waypoint. Centimeters -static int32_t offset_altitude_cm; +// Altitude control +static struct { + // target altitude above sea level in cm. Used for barometric + // altitude navigation + int32_t amsl_cm; + + // Altitude difference between previous and current waypoint in + // centimeters. Used for glide slope handling + int32_t offset_cm; +} target_altitude; //////////////////////////////////////////////////////////////////////////////// // INS variables @@ -733,7 +737,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { { update_compass, 5, 1200 }, { read_airspeed, 5, 1200 }, { update_alt, 5, 3400 }, - { calc_altitude_error, 5, 1000 }, + { adjust_altitude_target, 5, 1000 }, { obc_fs_check, 5, 1000 }, { gcs_update, 1, 1700 }, { gcs_data_stream_send, 1, 3000 }, @@ -1413,7 +1417,7 @@ static void update_alt() update_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL); } - SpdHgt_Controller->update_pitch_throttle(target_altitude_cm - home.alt + (int32_t(g.alt_offset)*100), + SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(), target_airspeed_cm, flight_stage, auto_state.takeoff_pitch_cd, diff --git a/ArduPlane/altitude.pde b/ArduPlane/altitude.pde new file mode 100644 index 0000000000..c048756960 --- /dev/null +++ b/ArduPlane/altitude.pde @@ -0,0 +1,240 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + altitude handling routines + */ + +/* + adjust altitude target depending on mode + */ +static void adjust_altitude_target() +{ + if (control_mode == FLY_BY_WIRE_B || + control_mode == CRUISE) { + return; + } + if (nav_controller->reached_loiter_target() || (wp_distance <= 30) || (wp_totalDistance<=30)) { + // once we reach a loiter target then lock to the final + // altitude target + set_target_altitude_location(next_WP_loc); + } else if (target_altitude.offset_cm != 0) { + // control climb/descent rate + set_target_altitude_proportion(next_WP_loc, (float)(wp_distance-30) / (float)(wp_totalDistance-30)); + + // stay within the range of the start and end locations in altitude + constrain_target_altitude_location(next_WP_loc, prev_WP_loc); + } else if (mission.get_current_do_cmd().id != MAV_CMD_CONDITION_CHANGE_ALT) { + set_target_altitude_location(next_WP_loc); + } + + altitude_error_cm = calc_altitude_error_cm(); +} + +/* + setup for a gradual glide slope to the next waypoint, if appropriate + */ +static void setup_glide_slope(void) +{ + // establish the distance we are travelling to the next waypoint, + // for calculating out rate of change of altitude + wp_totalDistance = get_distance(current_loc, next_WP_loc); + wp_distance = wp_totalDistance; + + /* + work out if we will gradually change altitude, or try to get to + the new altitude as quickly as possible. + */ + switch (control_mode) { + case RTL: + case GUIDED: + /* glide down slowly if above target altitude, but ascend more + rapidly if below it. See + https://github.com/diydrones/ardupilot/issues/39 + */ + if (above_location(next_WP_loc)) { + set_offset_altitude_location(next_WP_loc); + } else { + reset_offset_altitude(); + } + break; + + case AUTO: + // we only do glide slide handling in AUTO when above 40m or + // when descending. The 40 meter threshold is arbitrary, and + // is basically to prevent situations where we try to slowly + // gain height at low altitudes, potentially hitting + // obstacles. + if (relative_altitude() > 40 || !above_location(next_WP_loc)) { + set_offset_altitude_location(next_WP_loc); + } else { + reset_offset_altitude(); + } + break; + default: + reset_offset_altitude(); + break; + } +} + +/* + return RTL altitude as AMSL altitude + */ +static int32_t get_RTL_altitude() +{ + if (g.RTL_altitude_cm < 0) { + return current_loc.alt; + } + return g.RTL_altitude_cm + home.alt; +} + + +/* + return relative altitude in meters (relative to home) + */ +static float relative_altitude(void) +{ + return (current_loc.alt - home.alt) * 0.01f; +} + +/* + return relative altitude in centimeters, absolute value + */ +static int32_t relative_altitude_abs_cm(void) +{ + return labs(current_loc.alt - home.alt); +} + + +/* + set the target altitude to the current altitude. This is used when + setting up for altitude hold, such as when releasing elevator in + CRUISE mode. + */ +static void set_target_altitude_current(void) +{ + target_altitude.amsl_cm = current_loc.alt; +} + +/* + set the target altitude to the current altitude, with ALT_OFFSET adjustment + */ +static void set_target_altitude_current_adjusted(void) +{ + target_altitude.amsl_cm = adjusted_altitude_cm(); +} + +/* + set target altitude based on a location structure + */ +static void set_target_altitude_location(const Location &loc) +{ + target_altitude.amsl_cm = loc.alt; +} + +/* + return relative to home target altitude in centimeters. Used for + altitude control libraries + */ +static int32_t relative_target_altitude_cm(void) +{ + return target_altitude.amsl_cm - home.alt + (int32_t(g.alt_offset)*100); +} + +/* + change the current target altitude by an amount in centimeters. Used + to cope with changes due to elevator in CRUISE or FBWB + */ +static void change_target_altitude(int32_t change_cm) +{ + target_altitude.amsl_cm += change_cm; +} + +/* + change target altitude by a proportion of the target altitude offset + (difference in height to next WP from previous WP). proportion + should be between 0 and 1. + + When proportion is zero we have reached the destination. When + proportion is 1 we are at the starting waypoint. + + Note that target_altitude is setup initially based on the + destination waypoint + */ +static void set_target_altitude_proportion(const Location &loc, float proportion) +{ + set_target_altitude_location(loc); + proportion = constrain_float(proportion, 0.0f, 1.0f); + change_target_altitude(-target_altitude.offset_cm*proportion); +} + +/* + constrain target altitude to be between two locations. Used to + ensure we stay within two waypoints in altitude + */ +static void constrain_target_altitude_location(const Location &loc1, const Location &loc2) +{ + if (loc1.alt > loc2.alt) { + target_altitude.amsl_cm = constrain_int32(target_altitude.amsl_cm, loc2.alt, loc1.alt); + } else { + target_altitude.amsl_cm = constrain_int32(target_altitude.amsl_cm, loc1.alt, loc2.alt); + } +} + +/* + return error between target altitude and current altitude + */ +static int32_t calc_altitude_error_cm(void) +{ + return target_altitude.amsl_cm - adjusted_altitude_cm(); +} + +/* + check for FBWB_min_altitude_cm violation + */ +static void check_minimum_altitude(void) +{ + if (g.FBWB_min_altitude_cm != 0 && target_altitude.amsl_cm < home.alt + g.FBWB_min_altitude_cm) { + target_altitude.amsl_cm = home.alt + g.FBWB_min_altitude_cm; + } +} + +/* + reset the altitude offset used for glide slopes + */ +static void reset_offset_altitude(void) +{ + target_altitude.offset_cm = 0; +} + +/* + reset the altitude offset used for glide slopes, based on difference + between altitude at a destination and current altitude. If + destination is above the current altitude then the result is + positive. + */ +static void set_offset_altitude_location(const Location &loc) +{ + target_altitude.offset_cm = loc.alt - current_loc.alt; +} + +/* + are we above the altitude given by a location? + */ +static bool above_location(const Location &loc) +{ + return current_loc.alt > loc.alt; +} diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde index 7f8b1d046c..37030007f3 100644 --- a/ArduPlane/commands.pde +++ b/ArduPlane/commands.pde @@ -3,15 +3,6 @@ * logic for dealing with the current command in the mission and home location */ -static int32_t read_alt_to_hold() -{ - if (g.RTL_altitude_cm < 0) { - return current_loc.alt; - } - return g.RTL_altitude_cm + home.alt; -} - - /* * set_next_WP - sets the target location the vehicle should fly to */ @@ -56,12 +47,13 @@ static void set_next_WP(const struct Location& loc) // used to control FBW and limit the rate of climb // ----------------------------------------------- - target_altitude_cm = current_loc.alt; + set_target_altitude_current(); // zero out our loiter vals to watch for missed waypoints loiter_angle_reset(); setup_glide_slope(); + setup_turn_angle(); loiter_angle_reset(); } @@ -84,9 +76,10 @@ static void set_guided_WP(void) // used to control FBW and limit the rate of climb // ----------------------------------------------- - target_altitude_cm = current_loc.alt; + set_target_altitude_current(); setup_glide_slope(); + setup_turn_angle(); loiter_angle_reset(); } diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index a327a3b85f..f03b01c0f8 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -252,7 +252,7 @@ static void do_RTL(void) { control_mode = RTL; prev_WP_loc = current_loc; - next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, read_alt_to_hold()); + next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude()); if (g.loiter_radius < 0) { loiter.direction = -1; @@ -261,6 +261,7 @@ static void do_RTL(void) } setup_glide_slope(); + setup_turn_angle(); if (should_log(MASK_LOG_MODE)) Log_Write_Mode(control_mode); @@ -504,9 +505,10 @@ static void do_change_alt(const AP_Mission::Mission_Command& cmd) if (condition_value < adjusted_altitude_cm()) { condition_rate = -condition_rate; } - target_altitude_cm = adjusted_altitude_cm() + (condition_rate / 10); // condition_rate is climb rate in cm/s. We divide by 10 because this function is called at 10hz + set_target_altitude_current_adjusted(); + change_target_altitude(condition_rate/10); next_WP_loc.alt = condition_value; // For future nav calculations - offset_altitude_cm = 0; // For future nav calculations + reset_offset_altitude(); } static void do_within_distance(const AP_Mission::Mission_Command& cmd) @@ -534,7 +536,9 @@ static bool verify_change_alt() condition_value = 0; return true; } - target_altitude_cm += condition_rate / 10; // condition_rate is climb rate in cm/s. We divide by 10 because this function is called at 10hz + // condition_rate is climb rate in cm/s. + // We divide by 10 because this function is called at 10hz + change_target_altitude(condition_rate/10); return false; } @@ -632,9 +636,10 @@ static void exit_mission_callback() gcs_send_text_fmt(PSTR("Returning to Home")); memset(&auto_rtl_command, 0, sizeof(auto_rtl_command)); auto_rtl_command.content.location = - rally.calc_best_rally_or_home_location(current_loc, read_alt_to_hold()); + rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude()); auto_rtl_command.id = MAV_CMD_NAV_LOITER_UNLIM; setup_glide_slope(); + setup_turn_angle(); start_command(auto_rtl_command); } } diff --git a/ArduPlane/geofence.pde b/ArduPlane/geofence.pde index 31adbcf8ee..7c89436c3b 100644 --- a/ArduPlane/geofence.pde +++ b/ArduPlane/geofence.pde @@ -341,7 +341,7 @@ static void geofence_check(bool altitude_check_only) case FENCE_ACTION_GUIDED: case FENCE_ACTION_GUIDED_THR_PASS: if (g.fence_ret_rally != 0) { //return to a rally point - guided_WP_loc = rally.calc_best_rally_or_home_location(current_loc, read_alt_to_hold()); + guided_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude()); } else { //return to fence return point, not a rally point if (g.fence_retalt > 0) { diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index f8b80f1861..6e41664904 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -123,33 +123,6 @@ static void calc_gndspeed_undershoot() } } -static void calc_altitude_error() -{ - if (control_mode == FLY_BY_WIRE_B || - control_mode == CRUISE) { - return; - } - if (nav_controller->reached_loiter_target() || (wp_distance <= 30) || (wp_totalDistance<=30)) { - // once we reach a loiter target then lock to the final - // altitude target - target_altitude_cm = next_WP_loc.alt; - } else if (offset_altitude_cm != 0) { - // control climb/descent rate - target_altitude_cm = next_WP_loc.alt - (offset_altitude_cm*((float)(wp_distance-30) / (float)(wp_totalDistance-30))); - - // stay within the range of the start and end locations in altitude - if (prev_WP_loc.alt > next_WP_loc.alt) { - target_altitude_cm = constrain_int32(target_altitude_cm, next_WP_loc.alt, prev_WP_loc.alt); - } else { - target_altitude_cm = constrain_int32(target_altitude_cm, prev_WP_loc.alt, next_WP_loc.alt); - } - } else if (mission.get_current_do_cmd().id != MAV_CMD_CONDITION_CHANGE_ALT) { - target_altitude_cm = next_WP_loc.alt; - } - - altitude_error_cm = target_altitude_cm - adjusted_altitude_cm(); -} - static void update_loiter() { nav_controller->update_loiter(next_WP_loc, abs(g.loiter_radius), loiter.direction); @@ -205,19 +178,18 @@ static void update_fbwb_speed_height(void) elevator_input = -elevator_input; } - target_altitude_cm += g.flybywire_climb_rate * elevator_input * delta_us_fast_loop * 0.0001f; + change_target_altitude(g.flybywire_climb_rate * elevator_input * delta_us_fast_loop * 0.0001f); if (elevator_input == 0.0f && last_elevator_input != 0.0f) { // the user has just released the elevator, lock in // the current altitude - target_altitude_cm = current_loc.alt; + set_target_altitude_current(); } // check for FBWB altitude limit - if (g.FBWB_min_altitude_cm != 0 && target_altitude_cm < home.alt + g.FBWB_min_altitude_cm) { - target_altitude_cm = home.alt + g.FBWB_min_altitude_cm; - } - altitude_error_cm = target_altitude_cm - adjusted_altitude_cm(); + check_minimum_altitude(); + + altitude_error_cm = calc_altitude_error_cm(); last_elevator_input = elevator_input; @@ -225,56 +197,6 @@ static void update_fbwb_speed_height(void) calc_nav_pitch(); } -/* - setup for a gradual glide slope to the next waypoint, if appropriate - */ -static void setup_glide_slope(void) -{ - // establish the distance we are travelling to the next waypoint, - // for calculating out rate of change of altitude - wp_totalDistance = get_distance(current_loc, next_WP_loc); - wp_distance = wp_totalDistance; - - /* - work out if we will gradually change altitude, or try to get to - the new altitude as quickly as possible. - */ - switch (control_mode) { - case RTL: - case GUIDED: - /* glide down slowly if above target altitude, but ascend more - rapidly if below it. See - https://github.com/diydrones/ardupilot/issues/39 - */ - if (current_loc.alt > next_WP_loc.alt) { - offset_altitude_cm = next_WP_loc.alt - current_loc.alt; - } else { - offset_altitude_cm = 0; - } - break; - - case AUTO: - // we only do glide slide handling in AUTO when above 40m or - // when descending. The 40 meter threshold is arbitrary, and - // is basically to prevent situations where we try to slowly - // gain height at low altitudes, potentially hitting - // obstacles. - if (relative_altitude() > 40 || next_WP_loc.alt < current_loc.alt) { - offset_altitude_cm = next_WP_loc.alt - current_loc.alt; - } else { - offset_altitude_cm = 0; - } - break; - default: - offset_altitude_cm = 0; - break; - } - - - // calculate turn angle for next leg - setup_turn_angle(); -} - /* calculate the turn angle for the next leg of the mission */ @@ -293,19 +215,3 @@ static void setup_turn_angle(void) } } -/* - return relative altitude in meters (relative to home) - */ -static float relative_altitude(void) -{ - return (current_loc.alt - home.alt) * 0.01f; -} - -/* - return relative altitude in centimeters, absolute value - */ -static int32_t relative_altitude_abs_cm(void) -{ - return labs(current_loc.alt - home.alt); -} - diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index cdb172bc97..80898aef1f 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -322,12 +322,12 @@ static void set_mode(enum FlightMode mode) auto_throttle_mode = true; cruise_state.locked_heading = false; cruise_state.lock_timer_ms = 0; - target_altitude_cm = current_loc.alt; + set_target_altitude_current(); break; case FLY_BY_WIRE_B: auto_throttle_mode = true; - target_altitude_cm = current_loc.alt; + set_target_altitude_current(); break; case CIRCLE: