Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include "AP_RangeFinder_config.h"
       4             : 
       5             : #if AP_RANGEFINDER_JRE_SERIAL_ENABLED
       6             : 
       7             : #include "AP_RangeFinder.h"
       8             : #include "AP_RangeFinder_Backend_Serial.h"
       9             : 
      10             : /*
      11             : The data received from the radio altimeter is as follows.
      12             : The format of the received data frame varies depending on the mode, and is stored in "read_buff" with a fixed value of 16 bytes, 32 bytes, or 48 bytes.
      13             : 
      14             : [1]
      15             : Measurement mode: 1 data mode
      16             : Packet length: 16 bytes
      17             : Altitude data used: 4,5 bytes
      18             : |----------------------------------------------------------------------------------------------------------------------------------------------------|
      19             : | BYTE |   0    |   1    |    2     |    3     |    4     |    5     | 6 | 7 | 8 | 9 |     10    |     11    |    12     |    13     |  14   |  15   |
      20             : |------|---------------------------------------------------------------------------------------------------------------------------------------------|
      21             : | NAME | header | header | version  |  frame   | altitude | altitude |    reserved   | FFT value | FFT value |  status   |  status   |  CRC  |  CRC  |
      22             : |      |  (H)   |  (L)   |          |  count   |    (H)   |    (L)   |               |    (H)    |    (L)    |   (H)     |   (L)     |  (L)  |  (H)  |
      23             : |------|---------------------------------------------------------------------------------------------------------------------------------------------|
      24             : |      |        |        |          |          |                     |               |                       | BIT 15to8: reserved   |               |
      25             : |      |        |        |          | unsigned |       unsigned      |               |       unsigned        | BIT 7to4: GAIN detail |   CRC result  |
      26             : | DATA |  'R'   |  'A'   | 0 to 255 | 0 to 255 |      LSB;0.01 m     |      N/A      |        LSB:1          | BIT 3-2: reserved     |   from BYTE   |
      27             : |      |        |        |          |          |      0 to 65535     |               |      0 to 65535       | BIT 1: TRK / NTRK     |    0 to 13    |
      28             : |      |        |        |          |          |                     |               |                       | BIT 0: GAIN LOW/HIGH  |               |
      29             : |----------------------------------------------------------------------------------------------------------------------------------------------------|
      30             : 
      31             : [2]
      32             : Measurement mode: 3 data mode
      33             : Packet length: 32 bytes
      34             : Altitude data used: 4,5 bytes
      35             : |--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
      36             : | BYTE |   0    |   1    |    2     |    3     |    4     |    5     | 6 | 7 | 8 | 9 |     10    |     11    |    12    |    13    | 14| 15| 16| 17|     18    |     19    |    20    |    21    | 22| 23| 24| 25|     26    |     27    |    28     |    29     |  30   |  31   |
      37             : |------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
      38             : | NAME | header | header | version  |  frame   | altitude | altitude |    reserved   | FFT value | FFT value | altitude | altitude |    reserved   | FFT value | FFT value | altitude | altitude |    reserved   | FFT value | FFT value |  status   |  status   |  CRC  |  CRC  |
      39             : |      |  (H)   |  (L)   |          |  count   |    (H)   |    (L)   |               |    (H)    |    (L)    |    (H)   |    (L)   |               |    (H)    |    (L)    |    (H)   |    (L)   |               |    (H)    |    (L)    |    (H)    |    (L)    |  (L)  |  (H)  |
      40             : |------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
      41             : |      |        |        |          |          |                     |               |                       |                     |               |                       |                     |               |                       | BIT 15to8: reserved   |               |
      42             : |      |        |        |          | unsigned |       unsigned      |               |       unsigned        |       unsigned      |               |       unsigned        |       unsigned      |               |       unsigned        | BIT 7to4: GAIN detail |   CRC result  |
      43             : | DATA |  'R'   |  'B'   | 0 to 255 | 0 to 255 |      LSB;0.01 m     |      N/A      |        LSB:1          |      LSB;0.01 m     |      N/A      |        LSB:1          |      LSB;0.01 m     |      N/A      |        LSB:1          | BIT 3-2: reserved     |   from BYTE   |
      44             : |      |        |        |          |          |      0 to 65535     |               |      0 to 65535       |      0 to 65535     |               |      0 to 65535       |      0 to 65535     |               |      0 to 65535       | BIT 1: TRK / NTRK     |    0 to 13    |
      45             : |      |        |        |          |          |                     |               |                       |                     |               |                       |                     |               |                       | BIT 0: GAIN LOW/HIGH  |               |
      46             : |--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
      47             : 
      48             : [3]
      49             : Measurement mode: 5 data mode
      50             : Packet length: 48 bytes
      51             : Altitude data used: 4,5 bytes
      52             : |------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
      53             : | BYTE |   0    |   1    |    2     |    3     |    4     |    5     | 6 | 7 | 8 | 9 |     10    |     11    |    12    |    13    | 14| 15| 16| 17|     18    |     19    |    20    |    21    | 22| 23| 24| 25|     26    |     27    |    28    |    29    | 30| 31| 32| 33|     34    |     35    |    36    |    37    | 38| 39| 40| 41|     42    |     43    |    44     |    45     |  46   |  47   |
      54             : |------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
      55             : | NAME | header | header | version  |  frame   | altitude | altitude |    reserved   | FFT value | FFT value | altitude | altitude |    reserved   | FFT value | FFT value | altitude | altitude |    reserved   | FFT value | FFT value | altitude | altitude |    reserved   | FFT value | FFT value | altitude | altitude |    reserved   | FFT value | FFT value |  status   |  status   |  CRC  |  CRC  |
      56             : |      |  (H)   |  (L)   |          |  count   |    (H)   |    (L)   |               |    (H)    |    (L)    |    (H)   |    (L)   |               |    (H)    |    (L)    |    (H)   |    (L)   |               |    (H)    |    (L)    |    (H)   |    (L)   |               |    (H)    |    (L)    |    (H)   |    (L)   |               |    (H)    |    (L)    |    (H)    |    (L)    |  (L)  |  (H)  |
      57             : |------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
      58             : |      |        |        |          |          |                     |               |                       |                     |               |                       |                     |               |                       |                     |               |                       |                     |               |                       | BIT 15to8: reserved   |               |
      59             : |      |        |        |          | unsigned |       unsigned      |               |       unsigned        |       unsigned      |               |       unsigned        |       unsigned      |               |       unsigned        |       unsigned      |               |       unsigned        |       unsigned      |               |       unsigned        | BIT 7to4: GAIN detail |   CRC result  |
      60             : | DATA |  'R'   |  'C'   | 0 to 255 | 0 to 255 |      LSB;0.01 m     |      N/A      |        LSB:1          |      LSB;0.01 m     |      N/A      |        LSB:1          |      LSB;0.01 m     |      N/A      |        LSB:1          |      LSB;0.01 m     |      N/A      |        LSB:1          |      LSB;0.01 m     |      N/A      |        LSB:1          | BIT 3-2: reserved     |   from BYTE   |
      61             : |      |        |        |          |          |      0 to 65535     |               |      0 to 65535       |      0 to 65535     |               |      0 to 65535       |      0 to 65535     |               |      0 to 65535       |      0 to 65535     |               |      0 to 65535       |      0 to 65535     |               |      0 to 65535       | BIT 1: TRK / NTRK     |    0 to 13    |
      62             : |      |        |        |          |          |                     |               |                       |                     |               |                       |                     |               |                       |                     |               |                       |                     |               |                       | BIT 0: GAIN LOW/HIGH  |               |
      63             : |------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
      64             : */
      65             : class AP_RangeFinder_JRE_Serial : public AP_RangeFinder_Backend_Serial
      66             : {
      67             : 
      68             : public:
      69           0 :     static AP_RangeFinder_Backend_Serial *create(
      70             :         RangeFinder::RangeFinder_State &_state,
      71             :         AP_RangeFinder_Params &_params)
      72             :     {
      73           0 :         return NEW_NOTHROW AP_RangeFinder_JRE_Serial(_state, _params);
      74             :     }
      75             : 
      76             : protected:
      77             : 
      78             :     using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
      79             : 
      80           0 :     MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override
      81             :     {
      82           0 :         return MAV_DISTANCE_SENSOR_RADAR;
      83             :     }
      84             : 
      85           0 :     int8_t get_signal_quality_pct() const override
      86             :     {
      87           0 :         return no_signal ? RangeFinder::SIGNAL_QUALITY_MIN : signal_quality_pct;
      88             :     }
      89             : 
      90             : private:
      91             : 
      92             :     // get a reading
      93             :     bool get_reading(float &reading_m) override;
      94             : 
      95             :     void move_preamble_in_buffer(uint8_t search_start_pos);
      96             : 
      97             :     uint8_t data_buff[48 * 3];  // 48 is longest possible packet
      98             :     uint8_t data_buff_ofs;      // index where next item will be added in data_buff
      99             : 
     100             :     bool no_signal;     // true if the latest read attempt found no valid distances
     101             : 
     102             :     int8_t signal_quality_pct;
     103             : };
     104             : #endif  // AP_RANGEFINDER_JRE_SERIAL_ENABLED