Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include "AP_RangeFinder_config.h"
       4             : 
       5             : /*
       6             :  questions:
       7             :  - bug on page 22 on malfunction codes
       8             :  - fixed length seems strange at 28 bytes
       9             :  - definition of snr field is missing in documentation
      10             :  - roll/pitch limits are in conflict, 3.2 vs 
      11             : */
      12             : #if AP_RANGEFINDER_AINSTEIN_LR_D1_ENABLED
      13             : 
      14             : #include "AP_RangeFinder.h"
      15             : #include "AP_RangeFinder_Backend_Serial.h"
      16             : 
      17             : class AP_RangeFinder_Ainstein_LR_D1 : public AP_RangeFinder_Backend_Serial
      18             : {
      19             : 
      20             : public:
      21             : 
      22           3 :     static AP_RangeFinder_Backend_Serial *create(
      23             :         RangeFinder::RangeFinder_State &_state,
      24             :         AP_RangeFinder_Params &_params) {
      25           3 :         return NEW_NOTHROW AP_RangeFinder_Ainstein_LR_D1(_state, _params);
      26             :     }
      27             : 
      28             : protected:
      29             : 
      30        1767 :     MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
      31        1767 :         return MAV_DISTANCE_SENSOR_RADAR;
      32             :     }
      33             : 
      34           3 :     uint32_t initial_baudrate(uint8_t serial_instance) const override {
      35           3 :         return 115200;
      36             :     }
      37             : 
      38             :     // make sure readings go out-of-range when necessary
      39      119488 :     float max_distance() const override  {
      40      119488 :         return MIN(AP_RangeFinder_Backend::max_distance(), 500);
      41             :     }
      42        3407 :     float min_distance() const override {
      43        3407 :         return MAX(AP_RangeFinder_Backend::min_distance(), 0.7);
      44             :     }
      45             : 
      46             : private:
      47             : 
      48             :     using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
      49             : 
      50             :     // get_reading - read all samples, return last.  The device sends at
      51             :     // 40Hz, so there should only ever be one sample available.  Assuming
      52             :     // that something's gone wrong with scheduling, we will simply return
      53             :     // the last.
      54             :     bool get_reading(float &reading_m) override;
      55             : 
      56             :     // get a reading
      57             :     bool get_one_reading(float &reading_m);
      58             : 
      59             :     // 0 is no return value, 100 is perfect.  false means signal
      60             :     // quality is not available
      61        3536 :     int8_t get_signal_quality_pct() const override { return signal_quality_pct; };
      62             : 
      63             :     static void report_malfunction(const uint16_t _malfunction_alert_, const uint16_t _malfunction_alert_prev_);
      64             : 
      65             :     enum class MalfunctionAlert : uint8_t {
      66             :         Temperature       = (1U << 0),  // 0x01
      67             :         Voltage           = (1U << 1),  // 0x02
      68             :         IFSignalSaturation= (1U << 6),  // 0x40
      69             :         AltitudeReading   = (1U << 7),  // 0x80
      70             :     };
      71             :     // see page 25
      72             :     static void report_malfunction_v19000(const uint16_t malfunction_alert, const uint16_t malfunction_alert_prev);
      73             :     enum class MalfunctionAlert_v19000 : uint16_t {
      74             :         MCUTemperature         = (1U << 0),   // 0x0001
      75             :         MCUVoltage             = (1U << 1),   // 0x0002
      76             :         IFTemperature          = (1U << 4),   // 0x0010
      77             :         IFSignalSaturation     = (1U << 7),   // 0x0080
      78             :         Software               = (1U << 8),   // 0x0100
      79             :         AltitudeReadingOverflow= (1U << 10),  // 0x0400
      80             :         Voltage                = (1U << 15),  // 0x8000
      81             :     };
      82             : 
      83             :     // Ainstein LR-D1 firmware emits a single message with the same
      84             :     // header bytes, size, device_id and length, but different
      85             :     // meanings for some of the fields!  We differentiate between the
      86             :     // "original" packet and the new-style packet by looking at "byte
      87             :     // 12" (at offset 11).  If it is not 0xff then we consider the
      88             :     // packet to be of the "v19.0.0" format.  In this new format there
      89             :     // is no object count, there are 16 malfunction alert bits, "byte
      90             :     // 12" contains the unit's self-assessed height-valid information
      91             :     // and a lot of fields move from "other object readings" to
      92             :     // "reserved"
      93             :     union LRD1Union {
      94             :         struct PACKED LRD1Packet {
      95             :             uint8_t header_msb;
      96             :             uint8_t header_lsb;
      97             :             uint8_t device_id;
      98             :             uint8_t length;  // "fixed as 28 bytes"
      99             :             uint8_t malfunction_alert;
     100             :             uint8_t objects_number;  // "fixed as 1"
     101             :             uint16_t object1_alt;
     102             :             uint8_t object1_snr;
     103             :             uint16_t object1_velocity;
     104             :             uint8_t unused[20];
     105             :             uint8_t checksum;  // (data4+data5+…+data29+data31) bitwise-AND with 0xFF
     106             :         } packet;
     107             :         struct PACKED LRD1Packet_v19000 {
     108             :             uint8_t header_msb;
     109             :             uint8_t header_lsb;
     110             :             uint8_t device_id;
     111             :             uint8_t length;  // "fixed as 28 bytes"
     112             :             uint16_t malfunction_alert;
     113             :             uint16_t object1_alt;
     114             :             uint8_t object1_snr;
     115             :             uint16_t object1_velocity;
     116             :             uint8_t altitude_valid;
     117             :             uint8_t reserved[19];
     118             :             uint8_t checksum;  // (data4+data5+…+data29+data31) bitwise-AND with 0xFF
     119             :         } packet_v19000;
     120             :         uint8_t buffer[64];  // each packet is 32 bytes long
     121             : 
     122             :         // return checksum calculated from data in buffer
     123             :         uint8_t calculate_checksum() const;
     124             :     } u;
     125             :     uint8_t buffer_used;
     126             : 
     127             :     uint16_t malfunction_alert_prev;
     128             :     uint32_t malfunction_alert_last_send_ms;
     129             : 
     130             :     int8_t signal_quality_pct = RangeFinder::SIGNAL_QUALITY_UNKNOWN;    
     131             : 
     132             :     // ensures that there is a packet starting at offset 0 in the
     133             :     // buffer.  If that's not the case this returns false.  Search
     134             :     // starts at offset start in the buffer - if a packet header is
     135             :     // found at a non-zero offset then the data is moved to the start
     136             :     // of the buffer.
     137             :     bool move_signature_in_buffer(uint8_t start);
     138             : };
     139             : #endif  // AP_RANGEFINDER_AINSTEIN_LR_D1_ENABLED