Line data    Source code

       1             : #include "AP_RangeFinder_LightWare_GRF_Common.h"
       2             : 
       3             : #if AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED || AP_RANGEFINDER_LIGHTWARE_GRF_I2C_ENABLED
       4             : 
       5             : #include <AP_HAL/AP_HAL.h>
       6             : #include <AP_HAL/utility/sparse-endian.h>
       7             : #include <AP_Common/AP_Common.h>
       8             : 
       9             : const AP_Param::GroupInfo AP_RangeFinder_LightWare_GRF_Common::var_info[] = {
      10             :     // @Param: RET
      11             :     // @DisplayName: LightWare GRF Distance Return Type
      12             :     // @Description: Selects which single return to use.
      13             :     // @Values: 0:FirstRaw,1:FirstFiltered,2:LastRaw,3:LastFiltered
      14             :     // @User: Standard
      15             :     // @RebootRequired: True
      16             :     AP_GROUPINFO("RET", 1, AP_RangeFinder_LightWare_GRF_Common, return_selection, (uint8_t)GRF_ReturnSelection::FIRST_RAW),
      17             : 
      18             :     // @Param: ST
      19             :     // @DisplayName: LightWare GRF Minimum Return Strength
      20             :     // @Description: Minimum acceptable return signal strength in dB. Returns weaker than this will be ignored. Set to 0 to disable filtering.
      21             :     // @Range: 0 255
      22             :     // @User: Advanced
      23             :     // @RebootRequired: True
      24             :     AP_GROUPINFO("ST", 2, AP_RangeFinder_LightWare_GRF_Common, minimum_return_strength, 0),
      25             : 
      26             :     // @Param: RATE
      27             :     // @DisplayName: LightWare GRF Update Rate
      28             :     // @Description: The update rate of the sensor in Hz.
      29             :     // @Range: 1 50
      30             :     // @User: Advanced
      31             :     // @RebootRequired: True
      32             :     AP_GROUPINFO("RATE", 3, AP_RangeFinder_LightWare_GRF_Common, update_rate, GRF_DEFAULT_RATE_HZ),
      33             : 
      34             :     AP_GROUPEND
      35             : };
      36             : 
      37           4 : AP_RangeFinder_LightWare_GRF_Common::AP_RangeFinder_LightWare_GRF_Common()
      38             : {
      39           4 :     AP_Param::setup_object_defaults(this, var_info);
      40           4 : }
      41             : 
      42           2 : bool AP_RangeFinder_LightWare_GRF_Common::matches_product_name(const uint8_t *buf, uint16_t len)
      43             : {
      44             :     // Must be at least "GRFXXX\0" = 7 bytes
      45           2 :     if (len < 7) {
      46           0 :         return false;
      47             :     }
      48             : 
      49             :     // Compare the first 3 bytes with "GRF"
      50           2 :     return strncmp((const char*)buf, "GRF", 3) == 0;
      51             : }
      52             : 
      53           4 : uint32_t AP_RangeFinder_LightWare_GRF_Common::build_distance_output_bitmask() const
      54             : {
      55             :     // Two bits out of the six below are set: one picks the distance value
      56             :     // we want, the other asks for the matching strength reading alongside.
      57             :     //   0: first raw   1: first filtered  2: first strength
      58             :     //   3: last raw    4: last filtered   5: last strength
      59           4 :     uint8_t data_bit = 1;      // default: first filtered
      60           4 :     uint8_t strength_bit = 2;  // default: first strength
      61           4 :     switch (GRF_ReturnSelection(return_selection.get())) {
      62           4 :     case GRF_ReturnSelection::FIRST_RAW:
      63           4 :         data_bit = 0;
      64           4 :         break;
      65           0 :     case GRF_ReturnSelection::LAST_RAW:
      66           0 :         data_bit = 3;
      67           0 :         strength_bit = 5;
      68           0 :         break;
      69           0 :     case GRF_ReturnSelection::LAST_FILTERED:
      70           0 :         data_bit = 4;
      71           0 :         strength_bit = 5;
      72           0 :         break;
      73           0 :     case GRF_ReturnSelection::FIRST_FILTERED:
      74           0 :         break;
      75             :     }
      76           4 :     return (1U << data_bit) | (1U << strength_bit);
      77             : }
      78             : 
      79        6851 : bool AP_RangeFinder_LightWare_GRF_Common::parse_distance_cm_payload(const uint8_t *payload,
      80             :                                                                     uint16_t len,
      81             :                                                                     float &distance_m) const
      82             : {
      83        6851 :     if (len < 8) {
      84           0 :         return false;
      85             :     }
      86             : 
      87             :     // Sensor reports distance in 0.1 m steps, so scale up to cm before use.
      88        6851 :     const uint32_t distance_cm = le32toh_ptr(&payload[0]) * 10;
      89        6851 :     const uint32_t strength_db = le32toh_ptr(&payload[4]);
      90             : 
      91        6851 :     if (distance_cm == 0 || distance_cm > GRF_MAX_DISTANCE_CM) {
      92           0 :         return false;
      93             :     }
      94             : 
      95        6851 :     if (minimum_return_strength > 0 && strength_db < (uint32_t)minimum_return_strength) {
      96           0 :         return false;
      97             :     }
      98             : 
      99        6851 :     distance_m = distance_cm * 0.01f;
     100        6851 :     return true;
     101             : }
     102             : 
     103             : #endif  // AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED || AP_RANGEFINDER_LIGHTWARE_GRF_I2C_ENABLED