Plane: moved sink_rate calculation to update_alt()

this makes it available to non-landing code

ArduPlane/ArduPlane.cpp

@@ -503,7 +503,6 @@ void Plane::handle_auto_mode(void)
         // are for takeoff and landing
         steer_state.hold_course_cd = -1;
         auto_state.land_complete = false;
-        auto_state.land_sink_rate = 0;
         calc_nav_roll();
         calc_nav_pitch();
         calc_throttle();
@@ -783,6 +782,20 @@ void Plane::update_alt()
         Log_Write_Baro();
     }
 
+    // calculate the sink rate.
+    float sink_rate;
+    Vector3f vel;
+    if (ahrs.get_velocity_NED(vel)) {
+        sink_rate = vel.z;
+    } else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.have_vertical_velocity()) {
+        sink_rate = gps.velocity().z;
+    } else {
+        sink_rate = -barometer.get_climb_rate();        
+    }
+
+    // low pass the sink rate to take some of the noise out
+    auto_state.sink_rate = 0.8f * auto_state.sink_rate + 0.2f*sink_rate;
+    
     geofence_check(true);
 
     update_flight_stage();
@@ -852,7 +865,7 @@ void Plane::determine_is_flying(void)
           make is_flying() more accurate for landing approach
          */
         if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH &&
-            fabsf(auto_state.land_sink_rate) > 0.2f) {
+            fabsf(auto_state.sink_rate) > 0.2f) {
             isFlyingBool = true;
         }
     } else {

ArduPlane/Plane.h

@@ -452,7 +452,7 @@ private:
         float next_turn_angle {90};
 
         // filtered sink rate for landing
-        float land_sink_rate;
+        float sink_rate;
 
         // time when we first pass min GPS speed on takeoff
         uint32_t takeoff_speed_time_ms;

ArduPlane/commands_logic.cpp

@@ -16,7 +16,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
     if (AP_Mission::is_nav_cmd(cmd)) {
         // set land_complete to false to stop us zeroing the throttle
         auto_state.land_complete = false;
-        auto_state.land_sink_rate = 0;
+        auto_state.sink_rate = 0;
 
         // set takeoff_complete to true so we don't add extra evevator
         // except in a takeoff

ArduPlane/landing.cpp

@@ -30,20 +30,6 @@ bool Plane::verify_land()
     // use rangefinder to correct if possible
     height -= rangefinder_correction();
 
-    // calculate the sink rate.
-    float sink_rate;
-    Vector3f vel;
-    if (ahrs.get_velocity_NED(vel)) {
-        sink_rate = vel.z;
-    } else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.have_vertical_velocity()) {
-        sink_rate = gps.velocity().z;
-    } else {
-        sink_rate = -barometer.get_climb_rate();        
-    }
-
-    // low pass the sink rate to take some of the noise out
-    auto_state.land_sink_rate = 0.8f * auto_state.land_sink_rate + 0.2f*sink_rate;
-    
     /* Set land_complete (which starts the flare) under 3 conditions:
        1) we are within LAND_FLARE_ALT meters of the landing altitude
        2) we are within LAND_FLARE_SEC of the landing point vertically
@@ -58,16 +44,16 @@ bool Plane::verify_land()
     bool rangefinder_in_range = false;
 #endif
     if (height <= g.land_flare_alt ||
-        (aparm.land_flare_sec > 0 && height <= auto_state.land_sink_rate * aparm.land_flare_sec) ||
+        (aparm.land_flare_sec > 0 && height <= auto_state.sink_rate * aparm.land_flare_sec) ||
         (!rangefinder_in_range && location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) ||
-        (fabsf(auto_state.land_sink_rate) < 0.2f && !is_flying())) {
+        (fabsf(auto_state.sink_rate) < 0.2f && !is_flying())) {
 
         if (!auto_state.land_complete) {
             if (!is_flying() && (millis()-auto_state.last_flying_ms) > 3000) {
                 gcs_send_text_fmt(PSTR("Flare crash detected: speed=%.1f"), (double)gps.ground_speed());
             } else {
                 gcs_send_text_fmt(PSTR("Flare %.1fm sink=%.2f speed=%.1f"), 
-                        (double)height, (double)auto_state.land_sink_rate, (double)gps.ground_speed());
+                        (double)height, (double)auto_state.sink_rate, (double)gps.ground_speed());
             }
         }
         auto_state.land_complete = true;