From 29b16dbafd2ff37a8297da2b63400dbb9a522a05 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Mon, 30 Jan 2017 12:48:22 -0700 Subject: [PATCH] Plane: Refactor to request relative altitudes from AHRS Removes the need for plane to do the math for finding the relative height. Also caches the value at the same time we update current_loc, which is a non behaviour change as that was the only time you could see a change in the relative height propegate through the system anyways --- ArduPlane/ArduPlane.cpp | 4 +++- ArduPlane/Attitude.cpp | 4 ++-- ArduPlane/GCS_Mavlink.cpp | 2 +- ArduPlane/Plane.h | 4 ++-- ArduPlane/altitude.cpp | 23 +++-------------------- ArduPlane/commands_logic.cpp | 2 +- ArduPlane/sensors.cpp | 2 +- ArduPlane/servos.cpp | 2 +- 8 files changed, 14 insertions(+), 29 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 0dbaf206b0..033cc7f229 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -450,6 +450,8 @@ void Plane::update_GPS_50Hz(void) { // get position from AHRS have_position = ahrs.get_position(current_loc); + ahrs.get_relative_position_D_home(relative_altitude); + relative_altitude *= -1.0f; static uint32_t last_gps_reading[GPS_MAX_INSTANCES]; gps.update(); @@ -1037,7 +1039,7 @@ float Plane::tecs_hgt_afe(void) } else { // when in normal flight we pass the hgt_afe as relative // altitude to home - hgt_afe = relative_altitude(); + hgt_afe = relative_altitude; } return hgt_afe; } diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index e88742bf19..f66a8942ce 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -225,7 +225,7 @@ void Plane::stabilize_yaw(float speed_scaler) // otherwise use ground steering when no input control and we // are below the GROUND_STEER_ALT steering_control.ground_steering = (channel_roll->get_control_in() == 0 && - fabsf(relative_altitude()) < g.ground_steer_alt); + fabsf(relative_altitude) < g.ground_steer_alt); if (landing.is_on_approach()) { // don't use ground steering on landing approach steering_control.ground_steering = false; @@ -390,7 +390,7 @@ void Plane::stabilize() see if we should zero the attitude controller integrators. */ if (channel_throttle->get_control_in() == 0 && - relative_altitude_abs_cm() < 500 && + fabsf(relative_altitude) < 5.0f && fabsf(barometer.get_climb_rate()) < 0.5f && gps.ground_speed() < 3) { // we are low, with no climb rate, and zero throttle, and very diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 4e4c270307..4e6215d252 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -165,7 +165,7 @@ void Plane::send_location(mavlink_channel_t chan) current_loc.lat, // in 1E7 degrees current_loc.lng, // in 1E7 degrees current_loc.alt * 10UL, // millimeters above sea level - relative_altitude() * 1.0e3f, // millimeters above ground + relative_altitude * 1.0e3f, // millimeters above ground vel.x * 100, // X speed cm/s (+ve North) vel.y * 100, // Y speed cm/s (+ve East) vel.z * -100, // Z speed cm/s (+ve up) diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 9dae847af3..e894a44639 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -700,6 +700,8 @@ private: #endif } target_altitude {}; + float relative_altitude = 0.0f; + // INS variables // The main loop execution time. Seconds // This is the time between calls to the DCM algorithm and is the Integration time for the gyros. @@ -829,8 +831,6 @@ private: void adjust_altitude_target(); void setup_glide_slope(void); int32_t get_RTL_altitude(); - float relative_altitude(void); - int32_t relative_altitude_abs_cm(void); float relative_ground_altitude(bool use_rangefinder_if_available); void set_target_altitude_current(void); void set_target_altitude_current_adjusted(void); diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 5649511a06..859e10a0a1 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -115,23 +115,6 @@ int32_t Plane::get_RTL_altitude() return g.RTL_altitude_cm + home.alt; } - -/* - return relative altitude in meters (relative to home) - */ -float Plane::relative_altitude(void) -{ - return (current_loc.alt - home.alt) * 0.01f; -} - -/* - return relative altitude in centimeters, absolute value - */ -int32_t Plane::relative_altitude_abs_cm(void) -{ - return labs(current_loc.alt - home.alt); -} - /* return relative altitude in meters (relative to terrain, if available, or home otherwise) @@ -150,7 +133,7 @@ float Plane::relative_ground_altitude(bool use_rangefinder_if_available) return altitude; } #endif - return relative_altitude(); + return relative_altitude; } /* @@ -456,7 +439,7 @@ int32_t Plane::adjusted_altitude_cm(void) */ int32_t Plane::adjusted_relative_altitude_cm(void) { - return adjusted_altitude_cm() - home.alt; + return (relative_altitude - mission_alt_offset())*100; } @@ -631,7 +614,7 @@ void Plane::rangefinder_height_update(void) if (rangefinder_state.in_range) { // base correction is the difference between baro altitude and // rangefinder estimate - float correction = relative_altitude() - rangefinder_state.height_estimate; + float correction = relative_altitude - rangefinder_state.height_estimate; #if AP_TERRAIN_AVAILABLE // if we are terrain following then correction is based on terrain data diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index a2ddc9dfcb..64069cbc9a 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -399,7 +399,7 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd) // zero rangefinder state, start to accumulate good samples now memset(&rangefinder_state, 0, sizeof(rangefinder_state)); - landing.do_land(cmd, relative_altitude()); + landing.do_land(cmd, relative_altitude); #if GEOFENCE_ENABLED == ENABLED if (g.fence_autoenable == 1) { diff --git a/ArduPlane/sensors.cpp b/ArduPlane/sensors.cpp index c88d802058..cd6347817e 100644 --- a/ArduPlane/sensors.cpp +++ b/ArduPlane/sensors.cpp @@ -39,7 +39,7 @@ void Plane::read_rangefinder(void) height = height_above_target(); } else { // otherwise just use the best available baro estimate above home. - height = relative_altitude(); + height = relative_altitude; } rangefinder.set_estimated_terrain_height(height); } diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 0eb2509f46..3f1d8dd934 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -101,7 +101,7 @@ bool Plane::suppress_throttle(void) return true; } - if (relative_altitude_abs_cm() >= 1000) { + if (fabsf(relative_altitude) >= 10.0f) { // we're more than 10m from the home altitude throttle_suppressed = false; return false;