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             : /**
      17             :  * RDS02UF Note:
      18             :  * Sensor range scope 1.5m~20.0m
      19             :  * Azimuth Coverage ±17°,Elevation Coverage ±3°
      20             :  * Frame Rate 20Hz
      21             :  */
      22             : 
      23             : #include "AP_RangeFinder_config.h"
      24             : 
      25             : #if AP_RANGEFINDER_RDS02UF_ENABLED
      26             : 
      27             : #include "AP_RangeFinder_RDS02UF.h"
      28             : #include <AP_HAL/AP_HAL.h>
      29             : 
      30             : #define RDS02_HEAD                  0x55
      31             : #define RDS02_END                   0xAA
      32             : #define RDS02UF_PRE_DATA_LEN        6
      33             : #define RDS02_TARGET_INFO_FC        0x070C
      34             : #define RDS02UF_IGNORE_ID_BYTE      0x0F0F
      35             : #define RDS02UF_UAV_PRODUCTS_ID     0x03FF
      36             : #define RDS02UF_IGNORE_CRC          0xFF
      37             : #define RDS02UF_NO_ERR              0x00
      38             : 
      39        1349 : bool AP_RangeFinder_RDS02UF::get_reading(float &distance_m)
      40             : {
      41        1349 :     if (uart == nullptr) {
      42           0 :         return false;
      43             :     }
      44             : 
      45        1349 :     const uint32_t nbytes = uart->read(&u.parse_buffer[body_length],
      46        1349 :                                  ARRAY_SIZE(u.parse_buffer)-body_length);
      47        1349 :     if (nbytes == 0) {
      48        1009 :         return false;
      49             :     }
      50         340 :     body_length += nbytes;
      51             : 
      52         340 :     move_header_in_buffer(0);
      53             : 
      54             :     // header byte 1 is correct.
      55         340 :     if (body_length < ARRAY_SIZE(u.parse_buffer)) {
      56             :         // need a full buffer to have a valid message...
      57           0 :         return false;
      58             :     }
      59             : 
      60         340 :     if (u.packet.headermagic2 != RDS02_HEAD) {
      61           0 :         move_header_in_buffer(1);
      62           0 :         return false;
      63             :     }
      64             : 
      65         340 :     const uint16_t read_len = u.packet.length_h << 8 | u.packet.length_l;
      66         340 :     if (read_len != RDS02UF_DATA_LEN) {
      67             :         // we can only accept the fixed length message
      68           0 :         move_header_in_buffer(1);
      69           0 :         return false;
      70             :     }
      71             : 
      72             :     // check for the footer signatures:
      73         340 :     if (u.packet.footermagic1 != RDS02_END) {
      74           0 :         move_header_in_buffer(1);
      75           0 :         return false;
      76             :     }
      77         340 :     if (u.packet.footermagic2 != RDS02_END) {
      78           0 :         move_header_in_buffer(1);
      79           0 :         return false;
      80             :     }
      81             : 
      82             :     // calculate checksum
      83         340 :     const uint8_t checksum = crc8_rds02uf(&u.parse_buffer[2], RDS02UF_PRE_DATA_LEN + RDS02UF_DATA_LEN);
      84         340 :     if (u.packet.checksum != checksum && u.packet.checksum != RDS02UF_IGNORE_CRC) {
      85           0 :         move_header_in_buffer(1);
      86           0 :         return false;
      87             :     }
      88             : 
      89         340 :     const uint16_t fc_code = (u.packet.fc_high << 8 | u.packet.fc_low);
      90         340 :     if (fc_code == RDS02UF_UAV_PRODUCTS_ID && u.packet.error_code == RDS02UF_NO_ERR) {
      91             :         // get target information
      92         340 :         const uint16_t read_info_fc = (u.packet.data[1] << 8 | u.packet.data[0]);
      93         340 :         if ((read_info_fc & RDS02UF_IGNORE_ID_BYTE) == RDS02_TARGET_INFO_FC) {
      94             :             // read_info_fc = 0x70C + ID * 0x10, ID: 0~0xF
      95         340 :             distance_m = (u.packet.data[6] * 256 + u.packet.data[5]) * 0.01f;
      96         340 :             state.last_reading_ms = AP_HAL::millis();
      97             :         }
      98             :     }
      99             : 
     100             :     // consume this message:
     101         340 :     move_header_in_buffer(ARRAY_SIZE(u.parse_buffer));
     102             : 
     103         340 :     return true;
     104             : }
     105             : 
     106             : // find a RDS02UF message in the buffer, starting at
     107             : // initial_offset.  If found, that message (or partial message) will
     108             : // be moved to the start of the buffer.
     109         680 : void AP_RangeFinder_RDS02UF::move_header_in_buffer(uint8_t initial_offset)
     110             : {
     111         680 :     uint8_t* header_ptr = (uint8_t*)memchr(&u.parse_buffer[initial_offset], RDS02_HEAD, body_length - initial_offset);
     112         680 :     if (header_ptr != nullptr) {
     113         340 :         size_t header_offset = header_ptr - &u.parse_buffer[0];
     114         340 :         if (header_offset != 0) {
     115             :             // header was found, but not at index 0; move it back to start of array
     116           0 :             memmove(u.parse_buffer, header_ptr, body_length - header_offset);
     117           0 :             body_length -= header_offset;
     118             :         }
     119             :     } else {
     120             :         // no header found; reset buffer
     121         340 :         body_length = 0;
     122             :     }
     123         680 : }
     124             : 
     125             : #endif  // AP_RANGEFINDER_RDS02UF_ENABLED
     126             :