aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

APM: added ALT_OFFSET parameter

Browse Source 
useful to adjust for barometric pressure changes during a long flight
This commit is contained in:
Andrew Tridgell 2012-09-19 16:22:53 +10:00
parent 0c864fd4dc
commit 2324997e16
7 changed files with 35 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/ArduPlane.pde
View File

@ -1175,10 +1175,10 @@ static void update_current_flight_mode(void)
if (g.flybywire_elev_reverse) {
elevator_input = -elevator_input;
}
if ((current_loc.alt >= home.alt+g.FBWB_min_altitude_cm) || (g.FBWB_min_altitude_cm == 0)) {
if ((adjusted_altitude_cm() >= home.alt+g.FBWB_min_altitude_cm) || (g.FBWB_min_altitude_cm == 0)) {
altitude_error_cm = elevator_input * g.pitch_limit_min_cd;
} else {
altitude_error_cm = (home.alt + g.FBWB_min_altitude_cm) - current_loc.alt;
altitude_error_cm = (home.alt + g.FBWB_min_altitude_cm) - adjusted_altitude_cm();
if (elevator_input < 0) {
altitude_error_cm += elevator_input * g.pitch_limit_min_cd;
}

2
ArduPlane/Parameters.h
View File

@ -76,6 +76,7 @@ public:
k_param_crosstrack_min_distance,
k_param_rudder_steer,
k_param_throttle_nudge,
k_param_alt_offset,
// 110: Telemetry control
//
@ -299,6 +300,7 @@ public:
AP_Int16 roll_limit_cd;
AP_Int16 pitch_limit_max_cd;
AP_Int16 pitch_limit_min_cd;
AP_Int16 alt_offset;
// Misc
//

9
ArduPlane/Parameters.pde
View File

@ -160,6 +160,15 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Advanced
GSCALAR(alt_control_algorithm, "ALT_CTRL_ALG", ALT_CONTROL_DEFAULT),
// @Param: ALT_OFFSET
// @DisplayName: Altitude offset
// @Description: This is added to the target altitude in automatic flight. It can be used to add a global altitude offset to a mission, or to adjust for barometric pressure changes
// @Units: Meters
// @Range: -32767 32767
// @Increment: 1
// @User: Advanced
GSCALAR(alt_offset, "ALT_OFFSET", 0),
GSCALAR(command_total, "CMD_TOTAL", 0),
GSCALAR(command_index, "CMD_INDEX", 0),

13
ArduPlane/commands_logic.pde
View File

@ -302,7 +302,7 @@ static bool verify_takeoff()
calc_bearing_error();
}
if (current_loc.alt > takeoff_altitude) {
if (adjusted_altitude_cm() > takeoff_altitude) {
hold_course = -1;
takeoff_complete = true;
next_WP = current_loc;
@ -322,7 +322,7 @@ static bool verify_land()
// Set land_complete if we are within 2 seconds distance or within
// 3 meters altitude of the landing point
if (((wp_distance > 0) && (wp_distance <= (g.land_flare_sec*g_gps->ground_speed*0.01)))
|| (current_loc.alt <= next_WP.alt + g.land_flare_alt*100)) {
|| (adjusted_altitude_cm() <= next_WP.alt + g.land_flare_alt*100)) {
land_complete = true;
@ -441,8 +441,10 @@ static void do_change_alt()
{
condition_rate = labs((int)next_nonnav_command.lat);
condition_value = next_nonnav_command.alt;
if(condition_value < current_loc.alt) condition_rate = -condition_rate;
target_altitude_cm = current_loc.alt + (condition_rate / 10); // Divide by ten for 10Hz update
if (condition_value < adjusted_altitude_cm()) {
condition_rate = -condition_rate;
}
target_altitude_cm = adjusted_altitude_cm() + (condition_rate / 10); // Divide by ten for 10Hz update
next_WP.alt = condition_value; // For future nav calculations
offset_altitude_cm = 0; // For future nav calculations
}
@ -467,7 +469,8 @@ static bool verify_wait_delay()
static bool verify_change_alt()
{
if( (condition_rate>=0 && current_loc.alt >= condition_value) || (condition_rate<=0 && current_loc.alt <= condition_value)) {
if( (condition_rate>=0 && adjusted_altitude_cm() >= condition_value) ||
(condition_rate<=0 && adjusted_altitude_cm() <= condition_value)) {
condition_value = 0;
return true;
}

4
ArduPlane/geofence.pde
View File

@ -153,7 +153,7 @@ static bool geofence_check_minalt(void)
if (g.fence_minalt == 0) {
return false;
}
return (current_loc.alt < (g.fence_minalt*100.0) + home.alt);
return (adjusted_altitude_cm() < (g.fence_minalt*100.0) + home.alt);
}
/*
@ -167,7 +167,7 @@ static bool geofence_check_maxalt(void)
if (g.fence_maxalt == 0) {
return false;
}
return (current_loc.alt > (g.fence_maxalt*100.0) + home.alt);
return (adjusted_altitude_cm() > (g.fence_maxalt*100.0) + home.alt);
}

2
ArduPlane/navigation.pde
View File

@ -128,7 +128,7 @@ static void calc_altitude_error()
target_altitude_cm = next_WP.alt;
}
altitude_error_cm = target_altitude_cm - current_loc.alt;
altitude_error_cm = target_altitude_cm - adjusted_altitude_cm();
}
static int32_t wrap_360_cd(int32_t error)

11
ArduPlane/sensors.pde
View File

@ -65,3 +65,14 @@ void read_receiver_rssi(void)
receiver_rssi = constrain(ret, 0, 255);
#endif
}
/*
return current_loc.alt adjusted for ALT_OFFSET
This is useful during long flights to account for barometer changes
from the GCS, or to adjust the flying height of a long mission
*/
static int32_t adjusted_altitude_cm(void)
{
return current_loc.alt - (g.alt_offset*100);
}