Line data    Source code

       1             : #include "AP_RangeFinder_config.h"
       2             : 
       3             : #if AP_RANGEFINDER_NRA24_CAN_DRIVER_ENABLED
       4             : 
       5             : #include "AP_RangeFinder_NRA24_CAN.h"
       6             : #include <AP_BoardConfig/AP_BoardConfig.h>
       7             : #include <AP_HAL/AP_HAL.h>
       8             : 
       9             : // update the state of the sensor
      10           0 : void AP_RangeFinder_NRA24_CAN::update(void)
      11             : {
      12           0 :     WITH_SEMAPHORE(_sem);
      13           0 :     if (get_reading(state.distance_m)) {
      14             :         // update range_valid state based on distance measured
      15           0 :         state.last_reading_ms = AP_HAL::millis();
      16           0 :         update_status();
      17           0 :     } else if (AP_HAL::millis() - state.last_reading_ms > read_timeout_ms()) {
      18           0 :         if (AP_HAL::millis() - last_heartbeat_ms > read_timeout_ms()) {
      19             :             // no heartbeat, must be disconnected
      20           0 :             set_status(RangeFinder::Status::NotConnected);
      21             :         } else {
      22             :             // Have heartbeat, just no data. Probably because this sensor doesn't output data when there is no relative motion infront of the radar.
      23             :             // This case has special pre-arm check handling
      24           0 :             set_status(RangeFinder::Status::NoData);
      25             :         }
      26             :     }
      27           0 : }
      28             : 
      29             : // handler for incoming frames
      30           0 : bool AP_RangeFinder_NRA24_CAN::handle_frame(AP_HAL::CANFrame &frame)
      31             : {
      32           0 :     WITH_SEMAPHORE(_sem);
      33           0 :     const uint32_t id = frame.id;
      34             : 
      35           0 :     if (!is_correct_id(get_radar_id(id))) {
      36           0 :         return false;
      37             :     }
      38             : 
      39           0 :     switch (id & 0xFU) {
      40           0 :         case 0xAU:
      41             :             // heart beat in the form of Radar Status. The contents of this message aren't really useful so we won't parse them for now
      42           0 :             last_heartbeat_ms = AP_HAL::millis();
      43           0 :             break;
      44             : 
      45           0 :         case 0xCU:
      46             :         {
      47             :             // Target Information
      48           0 :             const float dist_m = (frame.data[2] * 0x100U + frame.data[3]) * 0.01;
      49           0 :             const uint8_t snr = frame.data[7] - 128;
      50             : 
      51           0 :             if ((snr_min != 0 && snr < uint16_t(snr_min.get()))) {
      52             :                 // too low signal strength
      53           0 :                 return false;
      54             :             }
      55           0 :             accumulate_distance_m(dist_m);
      56             :         }
      57           0 :             break;
      58             : 
      59           0 :         default:
      60             :             // not parsing these messages
      61           0 :             break;
      62             :     }
      63             : 
      64           0 :     return true;
      65           0 : }
      66             : 
      67             : #endif  // AP_RANGEFINDER_NRA24_CAN_DRIVER_ENABLED