From 7aa360b176c9b4d9ab192e85714bf3cc0b02f0ce Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 10 Jul 2013 13:40:13 +1000 Subject: [PATCH] Plane: added relative_altitude helper functions prevents some code duplication --- ArduPlane/ArduPlane.pde | 4 ++-- ArduPlane/Attitude.pde | 4 ++-- ArduPlane/GCS_Mavlink.pde | 4 ++-- ArduPlane/navigation.pde | 17 +++++++++++++++++ 4 files changed, 23 insertions(+), 6 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index d510199fd5..f7686393c1 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -785,7 +785,7 @@ 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((current_loc.alt - home.alt) * 0.01f); + SpdHgt_Controller->update_50hz(relative_altitude()); } } @@ -1255,7 +1255,7 @@ static void update_alt() (control_mode==AUTO && takeoff_complete == false), takeoff_pitch_cd, throttle_nudge, - float(hgt_afe_cm)*0.01f); + relative_altitude()); if (g.log_bitmask & MASK_LOG_TECS) { Log_Write_TECS_Tuning(); } diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index a4c450c757..7d17f9bf58 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -257,7 +257,7 @@ static void stabilize() see if we should zero the attitude controller integrators. */ if (channel_throttle->control_in == 0 && - abs(current_loc.alt - home.alt) < 500 && + relative_altitude_abs_cm() < 500 && fabs(barometer.get_climb_rate()) < 0.5f && g_gps->ground_speed < 300) { // we are low, with no climb rate, and zero throttle, and very @@ -511,7 +511,7 @@ static bool suppress_throttle(void) return false; } - if (labs(home.alt - current_loc.alt) >= 1000) { + if (relative_altitude_abs_cm() >= 1000) { // we're more than 10m from the home altitude throttle_suppressed = false; return false; diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 8440f3cffe..7b89a5884f 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -264,8 +264,8 @@ static void NOINLINE send_location(mavlink_channel_t chan) fix_time, current_loc.lat, // in 1E7 degrees current_loc.lng, // in 1E7 degrees - g_gps->altitude * 10, // millimeters above sea level - (current_loc.alt-home.alt) * 10, // millimeters above ground + g_gps->altitude * 10, // millimeters above sea level + relative_altitude() * 1.0e3, // millimeters above ground g_gps->velocity_north() * 100, // X speed cm/s (+ve North) g_gps->velocity_east() * 100, // Y speed cm/s (+ve East) g_gps->velocity_down() * -100, // Z speed cm/s (+ve up) diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index 77fd53efe1..9e92ad2012 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -193,3 +193,20 @@ static void setup_glide_slope(void) break; } } + +/* + 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); +} +