Line data    Source code

       1             : #include "AP_DAL_RangeFinder.h"
       2             : 
       3             : #include <AP_Logger/AP_Logger.h>
       4             : 
       5             : #include <AP_RangeFinder/AP_RangeFinder_Backend.h>
       6             : 
       7             : #if AP_RANGEFINDER_ENABLED
       8             : 
       9             : #include "AP_DAL.h"
      10             : #include <AP_BoardConfig/AP_BoardConfig.h>
      11             : #include <AP_Vehicle/AP_Vehicle_Type.h>
      12             : #include <AP_InternalError/AP_InternalError.h>
      13             : 
      14         172 : AP_DAL_RangeFinder::AP_DAL_RangeFinder()
      15             : {
      16             : #if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
      17         172 :     _RRNH.num_sensors = AP::rangefinder()->num_sensors();
      18         370 :     _RRNI = NEW_NOTHROW log_RRNI[_RRNH.num_sensors];
      19         172 :     _backend = NEW_NOTHROW AP_DAL_RangeFinder_Backend *[_RRNH.num_sensors];
      20         172 :     if (!_RRNI || !_backend) {
      21           0 :         goto failed;
      22             :     }
      23         370 :     for (uint8_t i=0; i<_RRNH.num_sensors; i++) {
      24         198 :         _RRNI[i].instance = i;
      25             :     }
      26         370 :     for (uint8_t i=0; i<_RRNH.num_sensors; i++) {
      27             :         // this avoids having to discard a const....
      28         198 :         _backend[i] = NEW_NOTHROW AP_DAL_RangeFinder_Backend(_RRNI[i]);
      29         198 :         if (!_backend[i]) {
      30           0 :             goto failed;
      31             :         }
      32             :     }
      33         172 :     return;
      34           0 : failed:
      35           0 :     AP_BoardConfig::allocation_error("DAL backends");
      36             : #endif
      37             : }
      38             : 
      39     1857646 : float AP_DAL_RangeFinder::ground_clearance_orient(enum Rotation orientation) const
      40             : {
      41             : #if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone)
      42     1857646 :     const auto *rangefinder = AP::rangefinder();
      43             : 
      44     1857646 :     if (orientation != ROTATION_PITCH_270) {
      45             :         // the EKF only asks for this from a specific orientation.  Thankfully.
      46           0 :         INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
      47           0 :         return rangefinder->ground_clearance_orient(orientation);
      48             :     }
      49             : #endif
      50             : 
      51     1857646 :     return _RRNH.ground_clearance;
      52             : }
      53             : 
      54      154191 : float AP_DAL_RangeFinder::max_distance_orient(enum Rotation orientation) const
      55             : {
      56             : #if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone)
      57      154191 :     if (orientation != ROTATION_PITCH_270) {
      58           0 :         const auto *rangefinder = AP::rangefinder();
      59             :         // the EKF only asks for this from a specific orientation.  Thankfully.
      60           0 :         INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
      61           0 :         return rangefinder->max_distance_orient(orientation);
      62             :     }
      63             : #endif
      64             : 
      65      154191 :     return _RRNH.max_distance;
      66             : }
      67             : 
      68     4663689 : void AP_DAL_RangeFinder::start_frame()
      69             : {
      70     4663689 :     const auto *rangefinder = AP::rangefinder();
      71     4663689 :     if (rangefinder == nullptr) {
      72           0 :         return;
      73             :     }
      74             : 
      75     4663689 :     const log_RRNH old = _RRNH;
      76             : 
      77             :     // EKF only asks for this *down*.
      78     4663689 :     _RRNH.ground_clearance = rangefinder->ground_clearance_orient(ROTATION_PITCH_270);
      79     4663689 :     _RRNH.max_distance = rangefinder->max_distance_orient(ROTATION_PITCH_270);
      80             : 
      81     4663689 :     WRITE_REPLAY_BLOCK_IFCHANGED(RRNH, _RRNH, old);
      82             : 
      83     9891562 :     for (uint8_t i=0; i<_RRNH.num_sensors; i++) {
      84     5227873 :         auto *backend = rangefinder->get_backend(i);
      85     5227873 :         if (backend == nullptr) {
      86       52406 :             continue;
      87             :         }
      88     5175467 :         _backend[i]->start_frame(backend);
      89             :     }
      90             : }
      91             : 
      92             : 
      93             : 
      94             : 
      95         198 : AP_DAL_RangeFinder_Backend::AP_DAL_RangeFinder_Backend(struct log_RRNI &RRNI) :
      96         198 :     _RRNI(RRNI)
      97             : {
      98         198 : }
      99             : 
     100     5175467 : void AP_DAL_RangeFinder_Backend::start_frame(AP_RangeFinder_Backend *backend) {
     101     5175467 :     const log_RRNI old = _RRNI;
     102     5175467 :     _RRNI.orientation = backend->orientation();
     103     5175467 :     _RRNI.status = (uint8_t)backend->status();
     104     5175467 :     _RRNI.pos_offset = backend->get_pos_offset();
     105     5175467 :     _RRNI.distance = backend->distance();
     106     5175467 :     WRITE_REPLAY_BLOCK_IFCHANGED(RRNI, _RRNI, old);
     107     5175467 : }
     108             : 
     109             : // return true if we have a range finder with the specified orientation
     110           0 : bool AP_DAL_RangeFinder::has_orientation(enum Rotation orientation) const
     111             : {
     112           0 :     for (uint8_t i=0; i<_RRNH.num_sensors; i++) {
     113           0 :         if (_RRNI[i].orientation == orientation) {
     114           0 :             return true;
     115             :         }
     116             :     }
     117           0 :     return false;
     118             : }
     119             : 
     120             : 
     121      974403 : AP_DAL_RangeFinder_Backend *AP_DAL_RangeFinder::get_backend(uint8_t id) const
     122             : {
     123      974403 :    if (id >= RANGEFINDER_MAX_INSTANCES) {
     124           0 :        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
     125           0 :        return nullptr;
     126             :    }
     127      974403 :    if (id >= _RRNH.num_sensors) {
     128      375353 :         return nullptr;
     129             :     }
     130             : 
     131      599050 :    return _backend[id];
     132             : }
     133             : 
     134           0 : void AP_DAL_RangeFinder::handle_message(const log_RRNH &msg)
     135             : {
     136           0 :     _RRNH = msg;
     137           0 :     if (_RRNH.num_sensors > 0 && _RRNI == nullptr) {
     138           0 :         _RRNI = NEW_NOTHROW log_RRNI[_RRNH.num_sensors];
     139           0 :         _backend = NEW_NOTHROW AP_DAL_RangeFinder_Backend *[_RRNH.num_sensors];
     140             :     }
     141           0 : }
     142             : 
     143           0 : void AP_DAL_RangeFinder::handle_message(const log_RRNI &msg)
     144             : {
     145           0 :     if (_RRNI != nullptr && msg.instance < _RRNH.num_sensors) {
     146           0 :         _RRNI[msg.instance] = msg;
     147           0 :         if (_backend != nullptr && _backend[msg.instance] == nullptr) {
     148           0 :             _backend[msg.instance] = NEW_NOTHROW AP_DAL_RangeFinder_Backend(_RRNI[msg.instance]);
     149             :         }
     150             :     }
     151           0 : }
     152             : 
     153             : #endif  // AP_RANGEFINDER_ENABLED