Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: split up altitude handling

Browse Source 
this creates altitude.pde for altitude handling, getting ready to add
terrain following
This commit is contained in:
Andrew Tridgell 2014-07-24 16:21:30 +10:00
parent 8ea2133c4b
commit d18c00d6fc
7 changed files with 274 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduPlane/ArduPlane.pde
View File

@ -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,

240
ArduPlane/altitude.pde Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
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;
}

15
ArduPlane/commands.pde
View File

@ -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();
}

15
ArduPlane/commands_logic.pde
View File

@ -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);
}
}

2
ArduPlane/geofence.pde
View File

@ -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) {

104
ArduPlane/navigation.pde
View File

@ -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);
}

4
ArduPlane/system.pde
View File

@ -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: