Line data    Source code

       1             : #include "AP_RangeFinder_USD1_CAN.h"
       2             : 
       3             : #if AP_RANGEFINDER_USD1_CAN_ENABLED
       4             : 
       5             : #include <AP_HAL/AP_HAL.h>
       6             : 
       7             : // handler for incoming frames. These come in at 100Hz
       8           0 : bool AP_RangeFinder_USD1_CAN::handle_frame(AP_HAL::CANFrame &frame)
       9             : {
      10           0 :     WITH_SEMAPHORE(_sem);
      11           0 :     const uint16_t id = frame.id & AP_HAL::CANFrame::MaskStdID;
      12             : 
      13           0 :     if (!is_correct_id(id)) {
      14           0 :         return false;
      15             :     }
      16             : 
      17           0 :     const uint16_t dist_cm = (frame.data[0]<<8) | frame.data[1];
      18           0 :     accumulate_distance_m(dist_cm * 0.01);
      19           0 :     return true;
      20           0 : }
      21             : 
      22             : #endif  // AP_RANGEFINDER_USD1_CAN_ENABLED