Copter: add upward facing surface tracking

ArduCopter/Copter.h

@@ -282,7 +282,7 @@ private:
     AP_InertialSensor ins;
 
     RangeFinder rangefinder;
-    struct {
+    struct RangeFinderState {
         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
@@ -291,7 +291,7 @@ private:
         int16_t alt_cm_glitch_protected;    // last glitch protected altitude
         int8_t glitch_count;    // non-zero number indicates rangefinder is glitching
         uint32_t glitch_cleared_ms; // system time glitch cleared
-    } rangefinder_state;
+    } rangefinder_state, rangefinder_up_state;
 
     class SurfaceTracking {
     public:
@@ -302,15 +302,26 @@ private:
         bool get_target_alt_cm(float &target_alt_cm) const;
         void set_target_alt_cm(float target_alt_cm);
 
-        // get target altitude (in cm) for logging purposes
+        // get target and actual distances (in m) for logging purposes
-        float logging_target_alt() const;
+        bool get_dist_for_logging(float &target_dist, float &actual_dist) const;
         void invalidate_for_logging() { valid_for_logging = false; }
 
+        // surface tracking states
+        enum class SurfaceTrackingState {
+            SURFACE_TRACKING_DISABLED = 0,
+            SURFACE_TRACKING_GROUND = 1,
+            SURFACE_TRACKING_CEILING = 2
+        };
+        // set direction
+        void set_state(SurfaceTrackingState state);
+
     private:
-        float target_alt_cm;        // desired altitude in cm above the ground
+        SurfaceTrackingState tracking_state = SurfaceTrackingState::SURFACE_TRACKING_GROUND;
+        float target_dist_cm;       // desired distance in cm from ground or ceiling
         uint32_t last_update_ms;    // system time of last update to target_alt_cm
         uint32_t last_glitch_cleared_ms;    // system time of last handle glitch recovery
         bool valid_for_logging;     // true if target_alt_cm is valid for logging
+        bool reset_target;          // true if target should be reset because of change in tracking_state
     } surface_tracking;
 
 #if RPM_ENABLED == ENABLED
@@ -829,6 +840,7 @@ private:
     void init_rangefinder(void);
     void read_rangefinder(void);
     bool rangefinder_alt_ok();
+    bool rangefinder_up_ok();
     void rpm_update();
     void init_optflow();
     void update_optical_flow(void);

ArduCopter/Log.cpp

@@ -16,7 +16,7 @@ struct PACKED log_Control_Tuning {
     float    inav_alt;
     int32_t  baro_alt;
     float    desired_rangefinder_alt;
-    int16_t  rangefinder_alt;
+    float    rangefinder_alt;
     float    terr_alt;
     int16_t  target_climb_rate;
     int16_t  climb_rate;
@@ -39,6 +39,13 @@ void Copter::Log_Write_Control_Tuning()
         target_climb_rate_cms = pos_control->get_vel_target_z();
     }
 
+    // get surface tracking alts
+    float desired_rangefinder_alt, rangefinder_alt;
+    if (!surface_tracking.get_dist_for_logging(desired_rangefinder_alt, rangefinder_alt)) {
+        desired_rangefinder_alt = AP::logger().quiet_nan();
+        rangefinder_alt = AP::logger().quiet_nan();;
+    }
+
     struct log_Control_Tuning pkt = {
         LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
         time_us             : AP_HAL::micros64(),
@@ -49,8 +56,8 @@ 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 : surface_tracking.logging_target_alt(),
+        desired_rangefinder_alt : desired_rangefinder_alt,
-        rangefinder_alt     : rangefinder_state.alt_cm,
+        rangefinder_alt     : rangefinder_alt,
         terr_alt            : terr_alt,
         target_climb_rate   : target_climb_rate_cms,
         climb_rate          : int16_t(inertial_nav.get_velocity_z()) // float -> int16_t
@@ -386,7 +393,7 @@ const struct LogStructure Copter::log_structure[] = {
     { LOG_PARAMTUNE_MSG, sizeof(log_ParameterTuning),
       "PTUN", "QBfff",         "TimeUS,Param,TunVal,TunMin,TunMax", "s----", "F----" },
     { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
-      "CTUN", "Qffffffefcfhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00B0BBBB" },
+      "CTUN", "Qffffffefffhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00B00BBB" },
     { LOG_MOTBATT_MSG, sizeof(log_MotBatt),
       "MOTB", "Qffff",  "TimeUS,LiftMax,BatVolt,BatRes,ThLimit", "s-vw-", "F-00-" },
     { LOG_DATA_INT16_MSG, sizeof(log_Data_Int16t),         

ArduCopter/RC_Channel.cpp

@@ -77,6 +77,7 @@ void RC_Channel_Copter::init_aux_function(const aux_func_t ch_option, const aux_
     case AUX_FUNC::PRECISION_LOITER:
     case AUX_FUNC::INVERTED:
     case AUX_FUNC::WINCH_ENABLE:
+    case AUX_FUNC::SURFACE_TRACKING:
         do_aux_function(ch_option, ch_flag);
         break;
     // the following functions do not need to be initialised:
@@ -541,6 +542,20 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw
 #endif
             break;
 
+        case AUX_FUNC::SURFACE_TRACKING:
+            switch (ch_flag) {
+            case LOW:
+                copter.surface_tracking.set_state(Copter::SurfaceTracking::SurfaceTrackingState::SURFACE_TRACKING_GROUND);
+                break;
+            case MIDDLE:
+                copter.surface_tracking.set_state(Copter::SurfaceTracking::SurfaceTrackingState::SURFACE_TRACKING_DISABLED);
+                break;
+            case HIGH:
+                copter.surface_tracking.set_state(Copter::SurfaceTracking::SurfaceTrackingState::SURFACE_TRACKING_CEILING);
+                break;
+            }
+            break;
+
     default:
         RC_Channel::do_aux_function(ch_option, ch_flag);
         break;

ArduCopter/sensors.cpp

@@ -17,6 +17,10 @@ void Copter::init_rangefinder(void)
    rangefinder.init(ROTATION_PITCH_270);
    rangefinder_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ);
    rangefinder_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_270);
+
+   // upward facing range finder
+   rangefinder_up_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ);
+   rangefinder_up_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_90);
 #endif
 }
 
@@ -26,62 +30,81 @@ void Copter::read_rangefinder(void)
 #if RANGEFINDER_ENABLED == ENABLED
     rangefinder.update();
 
-    rangefinder_state.alt_healthy = ((rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::RangeFinder_Good) && (rangefinder.range_valid_count_orient(ROTATION_PITCH_270) >= RANGEFINDER_HEALTH_MAX));
+#if RANGEFINDER_TILT_CORRECTION == ENABLED
-
+    const float tilt_correction = MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z);
-    int16_t temp_alt = rangefinder.distance_cm_orient(ROTATION_PITCH_270);
+#else
-
+    const float tile_correction = 1.0f;
- #if RANGEFINDER_TILT_CORRECTION == ENABLED
+#endif
-    // correct alt for angle of the rangefinder
-    temp_alt = (float)temp_alt * MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z);
- #endif
-
-    // tilt corrected but unfiltered, not glitch protected alt
-    rangefinder_state.alt_cm = temp_alt;
-
-    // glitch handling.  rangefinder readings more than RANGEFINDER_GLITCH_ALT_CM from the last good reading
-    // are considered a glitch and glitch_count becomes non-zero
-    // glitches clear after RANGEFINDER_GLITCH_NUM_SAMPLES samples in a row.
-    // glitch_cleared_ms is set so surface tracking (or other consumers) can trigger a target reset
-    const int32_t glitch_cm = rangefinder_state.alt_cm - rangefinder_state.alt_cm_glitch_protected;
-    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) {
-        rangefinder_state.glitch_count = MIN(rangefinder_state.glitch_count-1, -1);
-    } else {
-        rangefinder_state.glitch_count = 0;
-        rangefinder_state.alt_cm_glitch_protected = rangefinder_state.alt_cm;
-    }
-    if (abs(rangefinder_state.glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES) {
-        // clear glitch and record time so consumers (i.e. surface tracking) can reset their target altitudes
-        rangefinder_state.glitch_count = 0;
-        rangefinder_state.alt_cm_glitch_protected = rangefinder_state.alt_cm;
-        rangefinder_state.glitch_cleared_ms = AP_HAL::millis();
-    }
-
-    // filter rangefinder for use by AC_WPNav
-    uint32_t now = AP_HAL::millis();
-
-    const bool timed_out = now - rangefinder_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS;
 
-    if (rangefinder_state.alt_healthy) {
+    // iterate through downward and upward facing lidar
-        if (timed_out) {
+    struct {
-            // reset filter if we haven't used it within the last second
+        RangeFinderState &state;
-            rangefinder_state.alt_cm_filt.reset(rangefinder_state.alt_cm);
+        enum Rotation orientation;
+    } rngfnd[2] = { {rangefinder_state, ROTATION_PITCH_270}, {rangefinder_up_state, ROTATION_PITCH_90}};
+
+    for (uint8_t i=0; i < ARRAY_SIZE(rngfnd); i++) {
+        // local variables to make accessing simpler
+        RangeFinderState &rf_state = rngfnd[i].state;
+        enum Rotation rf_orient = rngfnd[i].orientation;
+
+        // update health
+        rf_state.alt_healthy = ((rangefinder.status_orient(rf_orient) == RangeFinder::RangeFinder_Good) &&
+                                (rangefinder.range_valid_count_orient(rf_orient) >= RANGEFINDER_HEALTH_MAX));
+
+        // tilt corrected but unfiltered, not glitch protected alt
+        rf_state.alt_cm = tilt_correction * rangefinder.distance_cm_orient(rf_orient);
+
+        // glitch handling.  rangefinder readings more than RANGEFINDER_GLITCH_ALT_CM from the last good reading
+        // are considered a glitch and glitch_count becomes non-zero
+        // glitches clear after RANGEFINDER_GLITCH_NUM_SAMPLES samples in a row.
+        // glitch_cleared_ms is set so surface tracking (or other consumers) can trigger a target reset
+        const int32_t glitch_cm = rf_state.alt_cm - rf_state.alt_cm_glitch_protected;
+        if (glitch_cm >= RANGEFINDER_GLITCH_ALT_CM) {
+            rf_state.glitch_count = MAX(rf_state.glitch_count+1, 1);
+        } else if (glitch_cm <= -RANGEFINDER_GLITCH_ALT_CM) {
+            rf_state.glitch_count = MIN(rf_state.glitch_count-1, -1);
         } else {
-            rangefinder_state.alt_cm_filt.apply(rangefinder_state.alt_cm, 0.02f);
+            rf_state.glitch_count = 0;
+            rf_state.alt_cm_glitch_protected = rf_state.alt_cm;
+        }
+        if (abs(rf_state.glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES) {
+            // clear glitch and record time so consumers (i.e. surface tracking) can reset their target altitudes
+            rf_state.glitch_count = 0;
+            rf_state.alt_cm_glitch_protected = rf_state.alt_cm;
+            rf_state.glitch_cleared_ms = AP_HAL::millis();
         }
-        rangefinder_state.last_healthy_ms = now;
-    }
 
-    // send rangefinder altitude and health to waypoint navigation library
+        // filter rangefinder altitude
-    if (rangefinder_state.alt_healthy || timed_out) {
+        uint32_t now = AP_HAL::millis();
-        wp_nav->set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
+        const bool timed_out = now - rf_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS;
+        if (rf_state.alt_healthy) {
+            if (timed_out) {
+                // reset filter if we haven't used it within the last second
+                rf_state.alt_cm_filt.reset(rf_state.alt_cm);
+            } else {
+                rf_state.alt_cm_filt.apply(rf_state.alt_cm, 0.02f);
+            }
+            rf_state.last_healthy_ms = now;
+        }
+
+        // send downward facing lidar altitude and health to waypoint navigation library
+        if (rf_orient == ROTATION_PITCH_270) {
+            if (rangefinder_state.alt_healthy || timed_out) {
+                wp_nav->set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
+            }
+        }
     }
 
 #else
+    // downward facing rangefinder
     rangefinder_state.enabled = false;
     rangefinder_state.alt_healthy = false;
     rangefinder_state.alt_cm = 0;
+
+    // upward facing rangefinder
+    rangefinder_up_state.enabled = false;
+    rangefinder_up_state.alt_healthy = false;
+    rangefinder_up_state.alt_cm = 0;
 #endif
 }
 
@@ -91,6 +114,12 @@ bool Copter::rangefinder_alt_ok()
     return (rangefinder_state.enabled && rangefinder_state.alt_healthy);
 }
 
+// return true if rangefinder_alt can be used
+bool Copter::rangefinder_up_ok()
+{
+    return (rangefinder_up_state.enabled && rangefinder_up_state.alt_healthy);
+}
+
 /*
   update RPM sensors
  */

ArduCopter/surface_tracking.cpp

@@ -6,39 +6,42 @@
 float Copter::SurfaceTracking::adjust_climb_rate(float target_rate)
 {
 #if RANGEFINDER_ENABLED == ENABLED
-    // if rangefinder alt is not ok or glitching, do not do surface tracking
+    // check tracking state and that range finders are healthy
-    if (!copter.rangefinder_alt_ok() ||
+    if ((tracking_state == SurfaceTrackingState::SURFACE_TRACKING_DISABLED) ||
-        (copter.rangefinder_state.glitch_count != 0)) {
+        ((tracking_state == SurfaceTrackingState::SURFACE_TRACKING_GROUND) && (!copter.rangefinder_alt_ok() || (copter.rangefinder_state.glitch_count != 0))) ||
+        ((tracking_state == SurfaceTrackingState::SURFACE_TRACKING_CEILING) && !copter.rangefinder_up_ok()) || (copter.rangefinder_up_state.glitch_count != 0)) {
         return target_rate;
     }
 
     // calculate current ekf based altitude error
     const float current_alt_error = copter.pos_control->get_alt_target() - copter.inertial_nav.get_altitude();
 
+    // init based on tracking direction/state
+    RangeFinderState &rf_state = (tracking_state == SurfaceTrackingState::SURFACE_TRACKING_GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state;
+    const float dir = (tracking_state == SurfaceTrackingState::SURFACE_TRACKING_GROUND) ? 1.0f : -1.0f;
+
     // reset target altitude if this controller has just been engaged
+    // target has been changed between upwards vs downwards
+    // or glitch has cleared
     const uint32_t now = millis();
-    if (now - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) {
+    if ((now - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) ||
-        target_alt_cm = copter.rangefinder_state.alt_cm + current_alt_error;
+        reset_target ||
-        last_glitch_cleared_ms = copter.rangefinder_state.glitch_cleared_ms;
+        (last_glitch_cleared_ms != rf_state.glitch_cleared_ms)) {
+        target_dist_cm = rf_state.alt_cm + (dir * current_alt_error);
+        reset_target = false;
+        last_glitch_cleared_ms = rf_state.glitch_cleared_ms;\
     }
     last_update_ms = now;
 
     // adjust rangefinder target alt if motors have not hit their limits
     if ((target_rate<0 && !copter.motors->limit.throttle_lower) || (target_rate>0 && !copter.motors->limit.throttle_upper)) {
-        target_alt_cm += target_rate * copter.G_Dt;
+        target_dist_cm += dir * target_rate * copter.G_Dt;
     }
     valid_for_logging = true;
 
-    // handle glitch recovery by resetting target
-    if (copter.rangefinder_state.glitch_cleared_ms != last_glitch_cleared_ms) {
-        // shift to the new rangefinder reading
-        target_alt_cm = copter.rangefinder_state.alt_cm + current_alt_error;
-        last_glitch_cleared_ms = copter.rangefinder_state.glitch_cleared_ms;
-    }
-
     // calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations)
-    const float distance_error = (target_alt_cm - copter.rangefinder_state.alt_cm) - current_alt_error;
+    const float distance_error = (target_dist_cm - rf_state.alt_cm) - (dir * current_alt_error);
-    float velocity_correction = distance_error * copter.g.rangefinder_gain;
+    float velocity_correction = dir * distance_error * copter.g.rangefinder_gain;
     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
@@ -52,25 +55,56 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate)
 // returns true if there is a valid target
 bool Copter::SurfaceTracking::get_target_alt_cm(float &_target_alt_cm) const
 {
+    // fail if we are not tracking downwards
+    if (tracking_state != SurfaceTrackingState::SURFACE_TRACKING_GROUND) {
+        return false;
+    }
     // check target has been updated recently
     if (AP_HAL::millis() - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) {
         return false;
     }
-    _target_alt_cm = target_alt_cm;
+    _target_alt_cm = target_dist_cm;
     return true;
 }
 
 // set target altitude (in cm) above ground
 void Copter::SurfaceTracking::set_target_alt_cm(float _target_alt_cm)
 {
-    target_alt_cm = _target_alt_cm;
+    // fail if we are not tracking downwards
+    if (tracking_state != SurfaceTrackingState::SURFACE_TRACKING_GROUND) {
+        return;
+    }
+    target_dist_cm = _target_alt_cm;
     last_update_ms = AP_HAL::millis();
 }
 
-float Copter::SurfaceTracking::logging_target_alt() const
+bool Copter::SurfaceTracking::get_dist_for_logging(float &target_dist, float &actual_dist) const
+{
+    if (!valid_for_logging || (tracking_state == SurfaceTrackingState::SURFACE_TRACKING_DISABLED)) {
+        return false;
+    }
+    target_dist = target_dist_cm * 0.01f;
+    actual_dist = ((tracking_state == SurfaceTrackingState::SURFACE_TRACKING_GROUND) ? copter.rangefinder_state.alt_cm : copter.rangefinder_up_state.alt_cm) * 0.01f;
+    return true;
+}
+
+// set direction
+void Copter::SurfaceTracking::set_state(SurfaceTrackingState state)
 {
-    if (!valid_for_logging) {
+    if (tracking_state == state) {
-        return AP::logger().quiet_nan();
+        return;
+    }
+    // check we have a range finder in the correct direction
+    if ((state == SurfaceTrackingState::SURFACE_TRACKING_GROUND) && !copter.rangefinder.has_orientation(ROTATION_PITCH_270)) {
+        copter.gcs().send_text(MAV_SEVERITY_WARNING, "SurfaceTracking: no downward rangefinder");
+        AP_Notify::events.user_mode_change_failed = 1;
+        return;
+    }
+    if ((state == SurfaceTrackingState::SURFACE_TRACKING_CEILING) && !copter.rangefinder.has_orientation(ROTATION_PITCH_90)) {
+        copter.gcs().send_text(MAV_SEVERITY_WARNING, "SurfaceTracking: no upward rangefinder");
+        AP_Notify::events.user_mode_change_failed = 1;
+        return;
     }
-    return target_alt_cm * 0.01f; // cm->m
+    tracking_state = state;
+    reset_target = true;
 }