Line data    Source code

       1             : #include "AP_SurfaceDistance.h"
       2             : 
       3             : #include <AP_RangeFinder/AP_RangeFinder.h>
       4             : 
       5             : #ifndef RANGEFINDER_TIMEOUT_MS
       6             :  # define RANGEFINDER_TIMEOUT_MS 1000        // rangefinder filter reset if no updates from sensor in 1 second
       7             : #endif
       8             : 
       9             : #if AP_RANGEFINDER_ENABLED
      10             : 
      11             : #include <AP_AHRS/AP_AHRS.h>
      12             : #include <AP_Logger/AP_Logger.h>
      13             : 
      14             : #ifndef RANGEFINDER_TILT_CORRECTION         // by disable tilt correction for use of range finder data by EKF
      15             :  # define RANGEFINDER_TILT_CORRECTION 1
      16             : #endif
      17             : 
      18             : #ifndef RANGEFINDER_HEALTH_MIN
      19             :  # define RANGEFINDER_HEALTH_MIN 3          // number of good reads that indicates a healthy rangefinder
      20             : #endif
      21             : 
      22             : 
      23     3039756 : void AP_SurfaceDistance::update()
      24             : {
      25     3039756 :     WITH_SEMAPHORE(sem);
      26             : 
      27     3039756 :     const RangeFinder *rangefinder = RangeFinder::get_singleton();
      28     3039756 :     if (rangefinder == nullptr) {
      29           0 :         alt_healthy = false;
      30           0 :         return;
      31             :     }
      32             : 
      33             : #if RANGEFINDER_TILT_CORRECTION == 1
      34     3039756 :     const float tilt_correction = MAX(0.707f, AP::ahrs().get_rotation_body_to_ned().c.z);
      35             : #else
      36             :     const float tilt_correction = 1.0f;
      37             : #endif
      38             : 
      39     3039756 :     const uint32_t now = AP_HAL::millis();
      40             : 
      41             :     // assemble bitmask assistance, definition is used to generate log documentation
      42             :     enum class Surface_Distance_Status : uint8_t {
      43             :         Enabled         = 1U<<0, // true if rangefinder has been set to enable by vehicle
      44             :         Unhealthy       = 1U<<1, // true if rangefinder is considered unhealthy
      45             :         Stale_Data      = 1U<<2, // true if the last healthy rangefinder reading is no longer valid
      46             :         Glitch_Detected = 1U<<3, // true if a measurement glitch detected
      47             :     };
      48             : 
      49             :     // reset status and add to the bitmask as we progress through the update
      50     3039756 :     status = 0;
      51             : 
      52             :     // update enabled status
      53     3039756 :     if (enabled) {
      54      101632 :         status |= (uint8_t)Surface_Distance_Status::Enabled;
      55             :     }
      56             : 
      57             :     // update health
      58     3119974 :     alt_healthy = (rangefinder->status_orient(rotation) == RangeFinder::Status::Good) &&
      59       80218 :                             (rangefinder->range_valid_count_orient(rotation) >= RANGEFINDER_HEALTH_MIN);
      60     3039756 :     if (!alt_healthy) {
      61     2959787 :         status |= (uint8_t)Surface_Distance_Status::Unhealthy;
      62             :     }
      63             : 
      64             :     // tilt corrected but unfiltered, not glitch protected alt
      65     3039756 :     alt_m = tilt_correction * rangefinder->distance_orient(rotation);
      66             : 
      67             :     // Glitch Handling. Rangefinder readings more than glitch_alt_m from the last good reading
      68             :     // are considered a glitch and glitch_count becomes non-zero
      69             :     // glitches clear after RANGEFINDER_GLITCH_NUM_SAMPLES samples in a row.
      70             :     // glitch_cleared_ms is set so surface tracking (or other consumers) can trigger a target reset
      71     3039756 :     const float glitch_m = alt_m - alt_glitch_protected_m;
      72     3039756 :     bool reset_terrain = false;
      73     3039756 :     if ((glitch_m >= parameters->glitch_alt) && (parameters->glitch_alt > 0)) {
      74          12 :         glitch_count = MAX(glitch_count+1, 1);
      75          12 :         status |= (uint8_t)Surface_Distance_Status::Glitch_Detected;
      76     3039744 :     } else if ((glitch_m <= -parameters->glitch_alt) && (parameters->glitch_alt > 0)) {
      77         285 :         glitch_count = MIN(glitch_count-1, -1);
      78         285 :         status |= (uint8_t)Surface_Distance_Status::Glitch_Detected;
      79             :     } else {
      80     3039459 :         glitch_count = 0;
      81     3039459 :         alt_glitch_protected_m = alt_m;
      82             :     }
      83     3039756 :     if ((abs(glitch_count) >= parameters->glitch_num_samples) && (parameters->glitch_num_samples > 0)) {
      84             :         // clear glitch and record time so consumers (i.e. surface tracking) can reset their target altitudes
      85          15 :         glitch_count = 0;
      86          15 :         alt_glitch_protected_m = alt_m;
      87          15 :         glitch_cleared_ms = now;
      88          15 :         reset_terrain = true;
      89             :     }
      90             : 
      91             :     // filter rangefinder altitude
      92     3039756 :     const bool timed_out = now - last_healthy_ms > RANGEFINDER_TIMEOUT_MS;
      93     3039756 :     if (alt_healthy) {
      94       79969 :         if (timed_out) {
      95             :             // reset filter if we haven't used it within the last second
      96          87 :             alt_m_filt.reset(alt_m);
      97          87 :             reset_terrain = true;
      98          87 :             status |= (uint8_t)Surface_Distance_Status::Stale_Data;
      99             :         } else {
     100             :             // TODO: When we apply this library in plane we will need to be able to set the filter freq
     101             :             // we should be using the updated dt not 0.05
     102       79882 :             alt_m_filt.apply(alt_m, 0.05);
     103             :         }
     104       79969 :         last_healthy_ms = now;
     105             :     }
     106             : 
     107             :     // remember inertial alt to allow us to interpolate rangefinder
     108             :     float pos_d_m;
     109     3039756 :     if (AP::ahrs().get_relative_position_D_origin_float(pos_d_m)) {
     110     2918130 :         ref_pos_u_m = -pos_d_m;
     111             :     }
     112             : 
     113             :     // handle reset of terrain offset
     114     3039756 :     if (reset_terrain) {
     115          99 :         if (rotation == ROTATION_PITCH_90) {
     116             :             // upward facing
     117           0 :             terrain_u_m = ref_pos_u_m + alt_m;
     118             :         } else {
     119             :             // assume downward facing
     120          99 :             terrain_u_m = ref_pos_u_m - alt_m;
     121             :         }
     122             :     }
     123             : #if HAL_LOGGING_ENABLED
     124     3039756 :     Log_Write();
     125             : #endif
     126     3039756 : }
     127             : 
     128             : /*
     129             :   get inertially interpolated rangefinder height. Inertial height is
     130             :   recorded whenever we update the rangefinder height, then we use the
     131             :   difference between the inertial height at that time and the current
     132             :   inertial height to give us interpolation of height from rangefinder
     133             :  */
     134     9367196 : bool AP_SurfaceDistance::get_rangefinder_height_interpolated_m(float& height_m) const
     135             : {
     136     9367196 :     if (!enabled_and_healthy()) {
     137     8783793 :         return false;
     138             :     }
     139             : 
     140             :     // get inertial alt
     141             :     float pos_d_m;
     142      583403 :     if (!AP::ahrs().get_relative_position_D_origin_float(pos_d_m)) {
     143         524 :         return false;
     144             :     }
     145             : 
     146      582879 :     height_m = alt_m_filt.get();
     147      582879 :     height_m += -pos_d_m - ref_pos_u_m;
     148      582879 :     return true;
     149             : }
     150             : 
     151             : #if HAL_LOGGING_ENABLED
     152     3039756 : void AP_SurfaceDistance::Log_Write(void) const
     153             : {
     154             :     // Don't log if there has never been data
     155     3039756 :     if (last_healthy_ms == 0) {
     156     2948837 :         return;
     157             :     }
     158             : 
     159             :     // @LoggerMessage: SURF
     160             :     // @Vehicles: Copter
     161             :     // @Description: Surface distance measurement
     162             :     // @Field: TimeUS: Time since system startup
     163             :     // @Field: I: Instance
     164             :     // @Field: St: Surface distance status
     165             :     // @FieldBitmaskEnum: St: Surface_Distance_Status
     166             :     // @Field: D: Raw Distance
     167             :     // @Field: FD: Filtered Distance
     168             :     // @Field: TO: Terrain Offset
     169             : 
     170             :     //Write to data flash log
     171      181838 :     AP::logger().WriteStreaming("SURF",
     172             :                                 "TimeUS,I,St,D,FD,TO",
     173             :                                 "s#-mmm",
     174             :                                 "F--000",
     175             :                                 "QBBfff",
     176             :                                 AP_HAL::micros64(),
     177       90919 :                                 instance,
     178       90919 :                                 status,
     179       90919 :                                 (float)alt_m,
     180       90919 :                                 (float)alt_m_filt.get(),
     181       90919 :                                 (float)terrain_u_m
     182             :                                 );
     183             : }
     184             : #endif  // HAL_LOGGING_ENABLED
     185             : 
     186             : #endif // AP_RANGEFINDER_ENABLED
     187             : 
     188    30253133 : bool AP_SurfaceDistance::data_stale(void)
     189             : {
     190    30253133 :     WITH_SEMAPHORE(sem);
     191    30253133 :     return (AP_HAL::millis() - last_healthy_ms) > RANGEFINDER_TIMEOUT_MS;
     192    30253133 : }
     193             : 
     194    64161278 : bool AP_SurfaceDistance::enabled_and_healthy(void) const
     195             : {
     196    64161278 :     return enabled && alt_healthy;
     197             : }