Copter: move rangefinder variables into structure

moved in rangefinder_alt, rangefinder_alt_health and rangefinder_enabled

ArduCopter/Attitude.cpp

@@ -255,7 +255,7 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current
 
     // reset target altitude if this controller has just been engaged
     if (now - last_call_ms > RANGEFINDER_TIMEOUT_MS) {
-        target_rangefinder_alt = rangefinder_alt + current_alt_target - current_alt;
+        target_rangefinder_alt = rangefinder_state.alt_cm + current_alt_target - current_alt;
     }
     last_call_ms = now;
 
@@ -266,10 +266,10 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current
 
     // do not let target altitude get too far from current altitude above ground
     // Note: the 750cm limit is perhaps too wide but is consistent with the regular althold limits and helps ensure a smooth transition
-    target_rangefinder_alt = constrain_float(target_rangefinder_alt,rangefinder_alt-pos_control.get_leash_down_z(),rangefinder_alt+pos_control.get_leash_up_z());
+    target_rangefinder_alt = constrain_float(target_rangefinder_alt,rangefinder_state.alt_cm-pos_control.get_leash_down_z(),rangefinder_state.alt_cm+pos_control.get_leash_up_z());
 
     // 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_alt) - (current_alt_target - current_alt);
+    distance_error = (target_rangefinder_alt - 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);
 

ArduCopter/Copter.cpp

@@ -25,7 +25,6 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL();
 Copter::Copter(void) :
     DataFlash{FIRMWARE_STRING},
     flight_modes(&g.flight_mode1),
-    rangefinder_enabled(true),
     mission(ahrs, 
             FUNCTOR_BIND_MEMBER(&Copter::start_command, bool, const AP_Mission::Mission_Command &),
             FUNCTOR_BIND_MEMBER(&Copter::verify_command_callback, bool, const AP_Mission::Mission_Command &),
@@ -60,8 +59,6 @@ Copter::Copter(void) :
     frsky_telemetry(ahrs, battery),
 #endif
     climb_rate(0),
-    rangefinder_alt(0),
-    rangefinder_alt_health(0),
     target_rangefinder_alt(0.0f),
     baro_alt(0),
     baro_climbrate(0.0f),

ArduCopter/Copter.h

@@ -171,7 +171,11 @@ private:
 
 #if RANGEFINDER_ENABLED == ENABLED
     RangeFinder rangefinder {serial_manager};
-    bool rangefinder_enabled; // enable user switch for rangefinder
+    struct {
+        bool enabled:1;
+        bool alt_healthy:1; // true if we can trust the altitude from the rangefinder
+        int16_t alt_cm;     // tilt compensated altitude (in cm) from rangefinder
+    } rangefinder_state = { true, false, 0 };
 #endif
 
     AP_RPM rpm_sensor;
@@ -403,8 +407,6 @@ private:
     // Altitude
     // The cm/s we are moving up or down based on filtered data - Positive = UP
     int16_t climb_rate;
-    int16_t rangefinder_alt;        // altitude as reported by the rangefinder in cm
-    uint8_t rangefinder_alt_health; // true if we can trust the altitude from the rangefinder
     float target_rangefinder_alt;   // desired altitude in cm above the ground
     int32_t baro_alt;            // barometer altitude in cm above home
     float baro_climbrate;        // barometer climbrate in cm/s

ArduCopter/Log.cpp

@@ -331,7 +331,7 @@ void Copter::Log_Write_Control_Tuning()
         inav_alt            : inertial_nav.get_altitude() / 100.0f,
         baro_alt            : baro_alt,
         desired_rangefinder_alt : (int16_t)target_rangefinder_alt,
-        rangefinder_alt     : rangefinder_alt,
+        rangefinder_alt     : rangefinder_state.alt_cm,
         terr_alt            : terr_alt,
         desired_climb_rate  : (int16_t)pos_control.get_vel_target_z(),
         climb_rate          : climb_rate

ArduCopter/control_land.cpp

@@ -234,7 +234,7 @@ void Copter::land_nogps_run()
 float Copter::get_land_descent_speed()
 {
 #if RANGEFINDER_ENABLED == ENABLED
-    bool rangefinder_ok = rangefinder_enabled && (rangefinder.status() == RangeFinder::RangeFinder_Good);
+    bool rangefinder_ok = rangefinder_state.enabled && (rangefinder.status() == RangeFinder::RangeFinder_Good);
 #else
     bool rangefinder_ok = false;
 #endif
@@ -252,7 +252,7 @@ float Copter::get_land_descent_speed()
     }
 
     // if we are above 10m and the rangefinder does not sense anything perform regular alt hold descent
-    if ((target_alt_cm >= LAND_START_ALT) && !(rangefinder_ok && rangefinder_alt_health >= RANGEFINDER_HEALTH_MAX)) {
+    if ((target_alt_cm >= LAND_START_ALT) && !(rangefinder_ok && rangefinder_state.alt_healthy)) {
         if (g.land_speed_high > 0) {
             // user has asked for a different landing speed than normal descent rate
             return -abs(g.land_speed_high);

ArduCopter/heli.cpp

@@ -44,7 +44,7 @@ void Copter::check_dynamic_flight(void)
         moving = (motors.get_throttle() > 0.8f || ahrs.pitch_sensor < -1500);
     }
 
-    if (!moving && rangefinder_enabled && rangefinder.status() == RangeFinder::RangeFinder_Good) {
+    if (!moving && rangefinder_state.enabled && rangefinder.status() == RangeFinder::RangeFinder_Good) {
         // when we are more than 2m from the ground with good
         // rangefinder lock consider it to be dynamic flight
         moving = (rangefinder.distance_cm() > 200);

ArduCopter/precision_landing.cpp

@@ -19,7 +19,7 @@ void Copter::update_precland()
 
     // use range finder altitude if it is valid
     if (rangefinder_alt_ok()) {
-        final_alt = rangefinder_alt;
+        final_alt = rangefinder_state.alt_cm;
     }
 
     copter.precland.update(final_alt);

ArduCopter/sensors.cpp

@@ -41,11 +41,11 @@ void Copter::read_rangefinder(void)
 
     // exit immediately if rangefinder is disabled
     if (rangefinder.status() != RangeFinder::RangeFinder_Good) {
-        rangefinder_alt_health = 0;
+        rangefinder_state.alt_healthy = false;
         return;
     }
 
-    rangefinder_alt_health = rangefinder.range_valid_count();
+    rangefinder_state.alt_healthy = (rangefinder.range_valid_count() >= RANGEFINDER_HEALTH_MAX);
 
     int16_t temp_alt = rangefinder.distance_cm();
 
@@ -54,17 +54,17 @@ void Copter::read_rangefinder(void)
     temp_alt = (float)temp_alt * MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z);
  #endif
 
-    rangefinder_alt = temp_alt;
+    rangefinder_state.alt_cm = temp_alt;
 #else
-    rangefinder_alt_health = 0;
+    rangefinder_state.alt_healthy = false;
-    rangefinder_alt = 0;
+    rangefinder_state.alt_cm = 0;
 #endif
 }
 
 // return true if rangefinder_alt can be used
 bool Copter::rangefinder_alt_ok()
 {
-    return (rangefinder_enabled && (rangefinder_alt_health >= RANGEFINDER_HEALTH_MAX));
+    return (rangefinder_state.enabled && rangefinder_state.alt_healthy);
 }
 
 /*

ArduCopter/switches.cpp

@@ -349,9 +349,9 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
             // enable or disable the rangefinder
 #if RANGEFINDER_ENABLED == ENABLED
             if (ch_flag == AUX_SWITCH_HIGH) {
-                rangefinder_enabled = true;
+                rangefinder_state.enabled = true;
             }else{
-                rangefinder_enabled = false;
+                rangefinder_state.enabled = false;
             }
 #endif
             break;