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;