diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index d25a8301be..4c33838cb6 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -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; } diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index d36f6981d1..7085531268 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -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 // diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index cbfdaa1722..cfcccbba0a 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -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), diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index b6f46ec32e..9c1eaaeeb1 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -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; } diff --git a/ArduPlane/geofence.pde b/ArduPlane/geofence.pde index e7bc0e8c63..a93bbfdd44 100644 --- a/ArduPlane/geofence.pde +++ b/ArduPlane/geofence.pde @@ -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); } diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index 00a0577944..371538b207 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -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) diff --git a/ArduPlane/sensors.pde b/ArduPlane/sensors.pde index 9e52a5c251..940617d4aa 100644 --- a/ArduPlane/sensors.pde +++ b/ArduPlane/sensors.pde @@ -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); +}