Line data    Source code

       1             : #include <AP_HAL/AP_HAL.h>
       2             : #include <AP_BoardConfig/AP_BoardConfig.h>
       3             : #include "AP_RangeFinder_Benewake_CAN.h"
       4             : #include <AP_HAL/utility/sparse-endian.h>
       5             : 
       6             : #if AP_RANGEFINDER_BENEWAKE_CAN_ENABLED
       7             : 
       8             : // handler for incoming frames for H30 radar
       9           0 : bool AP_RangeFinder_Benewake_CAN::handle_frame_H30(AP_HAL::CANFrame &frame)
      10             : {
      11             :     /*
      12             :       The H30 produces 3 targets, each as 16 bit unsigned integers in
      13             :       cm. Only look at target1 for now
      14             :     */
      15           0 :     const uint16_t target1_cm = be16toh_ptr(&frame.data[0]);
      16           0 :     if (target1_cm == 0) {
      17             :         // no target gives 0
      18           0 :         return false;
      19             :     }
      20             :     //uint16_t target2 = be16toh_ptr(&frame.data[2]);
      21             :     //uint16_t target3 = be16toh_ptr(&frame.data[4]);
      22             : 
      23           0 :     accumulate_distance_m(target1_cm * 0.01);
      24             : 
      25           0 :     return true;
      26             : }
      27             : 
      28             : // handler for incoming frames. These come in at 100Hz
      29           0 : bool AP_RangeFinder_Benewake_CAN::handle_frame(AP_HAL::CANFrame &frame)
      30             : {
      31           0 :     WITH_SEMAPHORE(_sem);
      32           0 :     if (frame.isExtended()) {
      33             :         // H30 radar uses extended frames
      34           0 :         const int32_t id = int32_t(frame.id & AP_HAL::CANFrame::MaskExtID);
      35           0 :         if (!is_correct_id(id)) {
      36           0 :             return false;
      37             :         }
      38           0 :         return handle_frame_H30(frame);
      39             :     }
      40             : 
      41           0 :     const uint16_t id = frame.id & AP_HAL::CANFrame::MaskStdID;
      42           0 :     if (!is_correct_id(id)) {
      43           0 :         return false;
      44             :     }
      45             : 
      46           0 :     const uint16_t dist_cm = le16toh_ptr(&frame.data[0]);
      47           0 :     const uint16_t snr = le16toh_ptr(&frame.data[2]);
      48           0 :     if (snr_min != 0 && snr < uint16_t(snr_min.get())) {
      49             :         // too low signal strength
      50           0 :         return true;
      51             :     }
      52             : 
      53           0 :     accumulate_distance_m(dist_cm * 0.01);
      54           0 :     return true;
      55           0 : }
      56             : 
      57             : #endif  // AP_RANGEFINDER_BENEWAKE_CAN_ENABLED