Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AP_Math/AP_Math.h>
       4             : #include <Filter/LowPassFilter.h>
       5             : #include <AP_HAL/Semaphores.h>
       6             : 
       7             : 
       8             : class AP_SurfaceDistance {
       9             : private:
      10             : public:
      11             :     struct SurfDistParameters {
      12             :         AP_Float                glitch_alt; // Glitch detection threshold, in meters.
      13             :         AP_Int8                 glitch_num_samples; // Number of consecutive glitches before the value is deemed correct.
      14             :     };
      15             :     SurfDistParameters *parameters;
      16             : 
      17        2002 :     AP_SurfaceDistance(Rotation rot, uint8_t i, SurfDistParameters *surf_dist_params) :
      18        2002 :             parameters(surf_dist_params),
      19        2002 :             rotation(rot),
      20        2002 :             instance(i)
      21        2002 :     {};
      22             : 
      23             :     void update();
      24             : 
      25             :     // check if the last healthy range finder reading is too old to be considered valid
      26             :     bool data_stale(void);
      27             : 
      28             :     // helper to check that rangefinder was last reported as enabled and healthy
      29             :     bool enabled_and_healthy(void) const;
      30             : 
      31             :     // get inertially interpolated rangefinder height
      32             :     bool get_rangefinder_height_interpolated_m(float& height_m) const;
      33             : 
      34             :     bool enabled;                           // not to be confused with rangefinder enabled, this state is to be set by the vehicle.
      35             :     bool alt_healthy;                       // true if we can trust the altitude from the rangefinder
      36             :     float alt_m;                            // tilt compensated altitude (in cm) from rangefinder
      37             :     float ref_pos_u_m;                      // inertial alt at time of last rangefinder sample
      38        2002 :     LowPassFilterFloat alt_m_filt {0.5};    // altitude filter
      39             :     float alt_glitch_protected_m;           // last glitch protected altitude
      40             :     int8_t glitch_count;                    // non-zero number indicates rangefinder is glitching
      41             :     uint32_t glitch_cleared_ms;             // system time glitch cleared
      42             :     float terrain_u_m;                      // filtered terrain offset (e.g. terrain's height above EKF origin)
      43             : 
      44             :     // accessor functions for the params
      45             :     static const struct AP_Param::GroupInfo var_info[];
      46             : 
      47             : private:
      48             : #if HAL_LOGGING_ENABLED
      49             :     void Log_Write(void) const;
      50             : #endif
      51             : 
      52             :     // multi-thread access support
      53             :     HAL_Semaphore sem;
      54             : 
      55             :     const Rotation rotation;
      56             :     const uint8_t instance;
      57             :     uint8_t status;
      58             :     uint32_t last_healthy_ms;
      59             : };