Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : #pragma once
      17             : 
      18             : #include "AP_RangeFinder_config.h"
      19             : 
      20             : #if AP_RANGEFINDER_RDS02UF_ENABLED
      21             : 
      22             : #include "AP_RangeFinder.h"
      23             : #include "AP_RangeFinder_Backend_Serial.h"
      24             : 
      25             : #define RDS02_BUFFER_SIZE           50
      26             : #define RDS02UF_DIST_MAX            20.00
      27             : #define RDS02UF_DIST_MIN            1.50
      28             : #define RDS02UF_DATA_LEN            10
      29             : 
      30             : class AP_RangeFinder_RDS02UF : public AP_RangeFinder_Backend_Serial
      31             : {
      32             : 
      33             : public:
      34             : 
      35           1 :     static AP_RangeFinder_Backend_Serial *create(
      36             :         RangeFinder::RangeFinder_State &_state,
      37             :         AP_RangeFinder_Params &_params) {
      38           1 :         return NEW_NOTHROW AP_RangeFinder_RDS02UF(_state, _params);
      39             :     }
      40             : 
      41             : protected:
      42             : 
      43         338 :     MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
      44         338 :         return MAV_DISTANCE_SENSOR_RADAR;
      45             :     }
      46             : 
      47             : private:
      48             :     using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
      49             : 
      50             :     // find a RDS02UF message in the buffer, starting at
      51             :     // initial_offset.  If found, that message (or partial message) will
      52             :     // be moved to the start of the buffer.
      53             :     void move_header_in_buffer(uint8_t initial_offset);
      54             : 
      55             :     // get a distance reading
      56             :     bool get_reading(float &reading_m) override;
      57        1009 :     uint16_t read_timeout_ms() const override { return 500; }
      58             : 
      59             :     // make sure readings go out-of-range when necessary
      60         679 :     float max_distance() const override  {
      61         679 :         return MIN(AP_RangeFinder_Backend::max_distance(), RDS02UF_DIST_MAX);
      62             :     }
      63         678 :     float min_distance() const override {
      64         678 :         return MAX(AP_RangeFinder_Backend::min_distance(), RDS02UF_DIST_MIN);
      65             :     }
      66             : 
      67             :      // Data Format for Benewake Rds02UF
      68             :     // ===============================
      69             :     // 21 bytes total per message:
      70             :     // 1) 0x55
      71             :     // 2) 0x55
      72             :     // 3) address
      73             :     // 4) error_code
      74             :     // 5) FC_CODE_L (low 8bit)
      75             :     // 6) FC_CODE_H (high 8bit)
      76             :     // 7) LENGTH_L (low 8bit)
      77             :     // 8) LENGTH_H (high 8bit)
      78             :     // 9) REAL_DATA (10Byte)
      79             :     // 10) CRC8
      80             :     // 11) END_1 0xAA
      81             :     // 12) END_2 0xAA
      82             :     struct PACKED RDS02UFPacket {
      83             :         uint8_t headermagic1;
      84             :         uint8_t headermagic2;
      85             :         uint8_t address;
      86             :         uint8_t error_code;
      87             :         uint8_t fc_low;
      88             :         uint8_t fc_high;
      89             :         uint8_t length_l;
      90             :         uint8_t length_h;
      91             :         uint8_t data[RDS02UF_DATA_LEN];
      92             :         uint8_t checksum;
      93             :         uint8_t footermagic1;
      94             :         uint8_t footermagic2;
      95             :     };
      96             : 
      97             :     union RDS02UF_Union {
      98             :         uint8_t parse_buffer[21];
      99             :         struct RDS02UFPacket packet;
     100             :     };
     101             :     RDS02UF_Union u;
     102             : 
     103             :     // number of bytes currently in the buffer
     104             :     uint8_t body_length;
     105             : };
     106             : #endif  // AP_RANGEFINDER_RDS02UF_ENABLED