Copter: move surface tracking variables into structure

ArduCopter/Attitude.cpp

@@ -129,7 +129,6 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate)
         return target_rate;
     }
 
-    static uint32_t last_call_ms = 0;
     const float current_alt = inertial_nav.get_altitude();
     const float current_alt_target = pos_control->get_alt_target();
     float distance_error;
@@ -137,17 +136,17 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate)
 
     uint32_t now = millis();
 
-    target_rangefinder_alt_used = true;
+    surface_tracking.valid_for_logging = true;
 
     // reset target altitude if this controller has just been engaged
-    if (now - last_call_ms > RANGEFINDER_TIMEOUT_MS) {
-        target_rangefinder_alt = rangefinder_state.alt_cm + current_alt_target - current_alt;
+    if (now - surface_tracking.last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) {
+        surface_tracking.target_alt_cm = rangefinder_state.alt_cm + current_alt_target - current_alt;
     }
-    last_call_ms = now;
+    surface_tracking.last_update_ms = now;
 
     // adjust rangefinder target alt if motors have not hit their limits
     if ((target_rate<0 && !motors->limit.throttle_lower) || (target_rate>0 && !motors->limit.throttle_upper)) {
-        target_rangefinder_alt += target_rate * G_Dt;
+        surface_tracking.target_alt_cm += target_rate * G_Dt;
     }
 
     /*
@@ -158,7 +157,7 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate)
       row. When that happens we reset the target altitude to the new
       reading
      */
-    int32_t glitch_cm = rangefinder_state.alt_cm - target_rangefinder_alt;
+    int32_t glitch_cm = rangefinder_state.alt_cm - surface_tracking.target_alt_cm;
     if (glitch_cm >= RANGEFINDER_GLITCH_ALT_CM) {
         rangefinder_state.glitch_count = MAX(rangefinder_state.glitch_count+1,1);
     } else if (glitch_cm <= -RANGEFINDER_GLITCH_ALT_CM) {
@@ -168,7 +167,7 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate)
     }
     if (abs(rangefinder_state.glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES) {
         // shift to the new rangefinder reading
-        target_rangefinder_alt = rangefinder_state.alt_cm;
+        surface_tracking.target_alt_cm = rangefinder_state.alt_cm;
         rangefinder_state.glitch_count = 0;
     }
     if (rangefinder_state.glitch_count != 0) {
@@ -177,9 +176,9 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate)
     }
 
     // calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations)
-    distance_error = (target_rangefinder_alt - rangefinder_state.alt_cm) - (current_alt_target - current_alt);
+    distance_error = (surface_tracking.target_alt_cm - rangefinder_state.alt_cm) - (current_alt_target - current_alt);
     velocity_correction = distance_error * g.rangefinder_gain;
-    velocity_correction = constrain_float(velocity_correction, -THR_SURFACE_TRACKING_VELZ_MAX, THR_SURFACE_TRACKING_VELZ_MAX);
+    velocity_correction = constrain_float(velocity_correction, -SURFACE_TRACKING_VELZ_MAX, SURFACE_TRACKING_VELZ_MAX);
 
     // return combined pilot climb rate + rate to correct rangefinder alt error
     return (target_rate + velocity_correction);

ArduCopter/Copter.h

@@ -247,6 +247,12 @@ private:
         int8_t glitch_count;
     } rangefinder_state = { false, false, 0, 0 };
 
+    struct {
+        float target_alt_cm;        // desired altitude in cm above the ground
+        uint32_t last_update_ms;    // system time of last update to target_alt_cm
+        bool valid_for_logging;     // true if target_alt_cm is valid for logging
+    } surface_tracking;
+
 #if RPM_ENABLED == ENABLED
     AP_RPM rpm_sensor;
 #endif
@@ -402,8 +408,6 @@ private:
 #endif
 
     // Altitude
-    float target_rangefinder_alt;   // desired altitude in cm above the ground
-    bool target_rangefinder_alt_used; // true if mode is using target_rangefinder_alt
     int32_t baro_alt;            // barometer altitude in cm above home
     LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests
 

ArduCopter/Log.cpp

@@ -39,11 +39,11 @@ void Copter::Log_Write_Control_Tuning()
         target_climb_rate_cms = pos_control->get_vel_target_z();
     }
 
-    float _target_rangefinder_alt;
-    if (target_rangefinder_alt_used) {
-        _target_rangefinder_alt = target_rangefinder_alt * 0.01f; // cm->m
+    float surface_tracking_target_alt;
+    if (surface_tracking.valid_for_logging) {
+        surface_tracking_target_alt = surface_tracking.target_alt_cm * 0.01f; // cm->m
     } else {
-        _target_rangefinder_alt = logger.quiet_nan();
+        surface_tracking_target_alt = logger.quiet_nan();
     }
     struct log_Control_Tuning pkt = {
         LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
@@ -55,7 +55,7 @@ void Copter::Log_Write_Control_Tuning()
         desired_alt         : des_alt_m,
         inav_alt            : inertial_nav.get_altitude() / 100.0f,
         baro_alt            : baro_alt,
-        desired_rangefinder_alt : _target_rangefinder_alt,
+        desired_rangefinder_alt : surface_tracking_target_alt,
         rangefinder_alt     : rangefinder_state.alt_cm,
         terr_alt            : terr_alt,
         target_climb_rate   : target_climb_rate_cms,

ArduCopter/config.h

@@ -88,16 +88,20 @@
  # define RANGEFINDER_HEALTH_MAX 3          // number of good reads that indicates a healthy rangefinder
 #endif
 
+#ifndef RANGEFINDER_TIMEOUT_MS
+# define RANGEFINDER_TIMEOUT_MS 1000        // rangefinder filter reset if no updates from sensor in 1 second
+#endif
+
 #ifndef RANGEFINDER_GAIN_DEFAULT
  # define RANGEFINDER_GAIN_DEFAULT 0.8f     // gain for controlling how quickly rangefinder range adjusts target altitude (lower means slower reaction)
 #endif
 
-#ifndef THR_SURFACE_TRACKING_VELZ_MAX
- # define THR_SURFACE_TRACKING_VELZ_MAX 150 // max vertical speed change while surface tracking with rangefinder
+#ifndef SURFACE_TRACKING_VELZ_MAX
+ # define SURFACE_TRACKING_VELZ_MAX 150     // max vertical speed change while surface tracking with rangefinder
 #endif
 
-#ifndef RANGEFINDER_TIMEOUT_MS
- # define RANGEFINDER_TIMEOUT_MS  1000      // desired rangefinder alt will reset to current rangefinder alt after this many milliseconds without a good rangefinder alt
+#ifndef SURFACE_TRACKING_TIMEOUT_MS
+ # define SURFACE_TRACKING_TIMEOUT_MS  1000 // surface tracking target alt will reset to current rangefinder alt after this many milliseconds without a good rangefinder alt
 #endif
 
 #ifndef RANGEFINDER_WPNAV_FILT_HZ

ArduCopter/mode.cpp

@@ -265,7 +265,7 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
 // called at 100hz or more
 void Copter::update_flight_mode()
 {
-    target_rangefinder_alt_used = false;
+    surface_tracking.valid_for_logging = false; // invalidate surface tracking alt, flight mode will set to true if used
 
     flightmode->run();
 }