Line data    Source code

       1             : #include "AP_RangeFinder_config.h"
       2             : 
       3             : #if AP_RANGEFINDER_TOFSENSEP_CAN_ENABLED
       4             : 
       5             : #include "AP_RangeFinder_TOFSenseP_CAN.h"
       6             : #include <AP_BoardConfig/AP_BoardConfig.h>
       7             : #include <AP_HAL/utility/sparse-endian.h>
       8             : #include <AP_HAL/AP_HAL.h>
       9             : 
      10             : // handler for incoming frames. These come in at 10-30Hz
      11           0 : bool AP_RangeFinder_TOFSenseP_CAN::handle_frame(AP_HAL::CANFrame &frame)
      12             : {
      13           0 :     WITH_SEMAPHORE(_sem);
      14           0 :     const uint32_t id = frame.id - 0x200U;
      15             : 
      16           0 :     if (!is_correct_id(id)) {
      17           0 :         return false;
      18             :     }
      19             : 
      20           0 :     const int32_t dist_mm = (int32_t)(frame.data[0] << 8U | frame.data[1] << 16U | frame.data[2] << 24U) >> 8;
      21           0 :     const uint8_t status = frame.data[3];
      22           0 :     const uint16_t snr = le16toh_ptr(&frame.data[4]);
      23             : 
      24           0 :     if ((snr_min != 0 && snr < uint16_t(snr_min.get())) || status > 0) {
      25             :         // too low signal strength or bad status
      26           0 :         return false;
      27             :     }
      28             : 
      29           0 :     accumulate_distance_m(dist_mm * 0.001);
      30           0 :     return true;
      31           0 : }
      32             : 
      33             : #endif  // AP_RANGEFINDER_TOFSENSEP_CAN_ENABLED