From c34803db139121545a51d8a1efa380664cbda081 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 9 Jul 2013 09:25:08 +1000 Subject: [PATCH] Plane: pass in adjusted height above field to TECS --- ArduPlane/ArduPlane.pde | 29 ++++++++++++++++++++--------- 1 file changed, 20 insertions(+), 9 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 207e548b3d..6607e5be3b 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -454,9 +454,6 @@ static struct { //////////////////////////////////////////////////////////////////////////////// AP_Airspeed airspeed; -//////////////////////////////////////////////////////////////////////////////// -// Altitude Sensor variables - //////////////////////////////////////////////////////////////////////////////// // flight mode specific //////////////////////////////////////////////////////////////////////////////// @@ -785,10 +782,23 @@ static void fast_loop() */ static void update_speed_height(void) { - if (g.alt_control_algorithm == ALT_CONTROL_TECS && - auto_throttle_mode && !throttle_suppressed) { - // Call TECS 50Hz update - SpdHgt_Controller->update_50hz(); + if (g.alt_control_algorithm == ALT_CONTROL_TECS && auto_throttle_mode && !throttle_suppressed) + { + // height above field elevation in cm + float hgt_afe_cm; + + // calculate the height above field elevation in centimeters + if (barometer.healthy) + { + hgt_afe_cm = (1 - g.altitude_mix) * (g_gps->altitude - home.alt); + hgt_afe_cm += g.altitude_mix * read_barometer(); + } + else if (g_gps->status() >= GPS::GPS_OK_FIX_3D) + { + hgt_afe_cm = g_gps->altitude - home.alt; + } + // Call TECS 50Hz update + SpdHgt_Controller->update_50hz(hgt_afe_cm*0.01f); } } @@ -1249,9 +1259,10 @@ static void update_alt() // add_altitude_data(millis() / 100, g_gps->altitude / 10); // Update the speed & height controller states - if (g.alt_control_algorithm == ALT_CONTROL_TECS && + if (g.alt_control_algorithm == ALT_CONTROL_TECS && auto_throttle_mode && !throttle_suppressed) { - SpdHgt_Controller->update_pitch_throttle(target_altitude_cm - home.alt, target_airspeed_cm, + SpdHgt_Controller->update_pitch_throttle(target_altitude_cm - home.alt + (int32_t(g.alt_offset)*100), + target_airspeed_cm, (control_mode==AUTO && takeoff_complete == false), takeoff_pitch_cd); if (g.log_bitmask & MASK_LOG_TECS) {