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             : #include "AP_RangeFinder_config.h"
      17             : 
      18             : #if AP_RANGEFINDER_JRE_SERIAL_ENABLED
      19             : 
      20             : #include "AP_RangeFinder_JRE_Serial.h"
      21             : #include <AP_Math/AP_Math.h>
      22             : 
      23             : #define FRAME_HEADER_1    'R'    // 0x52
      24             : #define FRAME_HEADER_2_A  'A'    // 0x41
      25             : #define FRAME_HEADER_2_B  'B'    // 0x42
      26             : #define FRAME_HEADER_2_C  'C'    // 0x43
      27             : 
      28             : #define DIST_MAX 50.00
      29             : #define OUT_OF_RANGE_ADD   1.0  // metres
      30             : 
      31           0 : void AP_RangeFinder_JRE_Serial::move_preamble_in_buffer(uint8_t search_start_pos)
      32             : {
      33             :     uint8_t i;
      34           0 :     for (i=search_start_pos; i<data_buff_ofs; i++) {
      35           0 :         if (data_buff[i] == FRAME_HEADER_1) {
      36           0 :             break;
      37             :         }
      38             :     }
      39           0 :     if (i == 0) {
      40           0 :         return;
      41             :     }
      42           0 :     memmove(data_buff, &data_buff[i], data_buff_ofs-i);
      43           0 :     data_buff_ofs = data_buff_ofs - i;
      44             : }
      45             : 
      46           0 : bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m)
      47             : {
      48             :     // uart instance check
      49           0 :     if (uart == nullptr) {
      50           0 :         return false;  // not update
      51             :     }
      52             : 
      53           0 :     uint16_t valid_count = 0;   // number of valid readings
      54           0 :     uint16_t invalid_count = 0; // number of invalid readings
      55           0 :     float sum = 0.0f;
      56             : 
      57           0 :     float fft_value = 0.0f;
      58           0 :     float sum_fft_value = 0.0f;
      59             :     uint16_t sq_pct;
      60             : 
      61             :     // read a maximum of 8192 bytes per call to this function:
      62           0 :     uint16_t bytes_available = MIN(uart->available(), 8192U);
      63             : 
      64           0 :     while (bytes_available > 0) {
      65             :         // fill buffer
      66           0 :         const auto num_bytes_to_read = MIN(bytes_available, ARRAY_SIZE(data_buff) - data_buff_ofs);
      67           0 :         const auto num_bytes_read = uart->read(&data_buff[data_buff_ofs], num_bytes_to_read);
      68           0 :         if (num_bytes_read == 0) {
      69           0 :             break;
      70             :         }
      71           0 :         if (bytes_available < num_bytes_read) {
      72             :             // this is a bug in the uart call.
      73           0 :             break;
      74             :         }
      75           0 :         bytes_available -= num_bytes_read;
      76           0 :         data_buff_ofs += num_bytes_read;
      77             : 
      78             :         // move header frame header in buffer
      79           0 :         move_preamble_in_buffer(0);
      80             : 
      81             :         // ensure we have a packet type:
      82           0 :         if (data_buff_ofs < 2) {
      83           0 :             continue;
      84             :         }
      85             : 
      86             :         // determine packet length for incoming packet:
      87             :         uint8_t packet_length;
      88           0 :         switch (data_buff[1]) {
      89           0 :         case FRAME_HEADER_2_A:
      90           0 :             packet_length = 16;
      91           0 :             break;
      92           0 :         case FRAME_HEADER_2_B:
      93           0 :             packet_length = 32;
      94           0 :             break;
      95           0 :         case FRAME_HEADER_2_C:
      96           0 :             packet_length = 48;
      97           0 :             break;
      98           0 :         default:
      99           0 :             move_preamble_in_buffer(1);
     100           0 :             continue;
     101             :         }
     102             : 
     103             :         // check there are enough bytes for message type
     104           0 :         if (data_buff_ofs < packet_length) {
     105           0 :             continue;
     106             :         }
     107             : 
     108             :         // check the checksum
     109           0 :         const uint16_t crc = crc16_ccitt_r(data_buff, packet_length - 2, 0xffff, 0xffff);
     110           0 :         if (crc != ((data_buff[packet_length-1] << 8) | data_buff[packet_length-2])) {
     111             :             // CRC failure
     112           0 :             move_preamble_in_buffer(1);
     113           0 :             continue;
     114             :         }
     115             : 
     116             :         // check random bit to for magic value:
     117           0 :         if (data_buff[packet_length-3] & 0x02) { // NTRK
     118           0 :             invalid_count++;
     119             :             // discard entire packet:
     120           0 :             move_preamble_in_buffer(packet_length);
     121           0 :             continue;
     122             :         }
     123             : 
     124             :         // good message, extract rangefinder reading:
     125           0 :         reading_m = (data_buff[4] * 256 + data_buff[5]) * 0.01f;
     126           0 :         sum += reading_m;
     127           0 :         fft_value = (data_buff[10] * 256 + data_buff[11]);
     128           0 :         sum_fft_value += fft_value;
     129           0 :         valid_count++;
     130           0 :         move_preamble_in_buffer(packet_length);
     131             :     }
     132             : 
     133             :     // return average of all valid readings
     134           0 :     if (valid_count > 0) {
     135           0 :         no_signal = false;
     136           0 :         reading_m = sum / valid_count;
     137           0 :         sq_pct = (uint16_t)((sum_fft_value / valid_count) * (100.0f / 512.0f) + 0.5f);
     138           0 :         if (sq_pct >= 100) {
     139           0 :             signal_quality_pct = 100;
     140             :         } else {
     141           0 :             signal_quality_pct = (int8_t)sq_pct;
     142             :         }
     143           0 :         return true;
     144             :     }
     145             : 
     146             :     // all readings were invalid so return out-of-range-high value
     147           0 :     if (invalid_count > 0) {
     148           0 :         no_signal = true;
     149           0 :         reading_m = MAX(DIST_MAX, max_distance() + OUT_OF_RANGE_ADD);
     150           0 :         return true;
     151             :     }
     152             : 
     153           0 :     return false;
     154             : }
     155             : #endif  // AP_RANGEFINDER_JRE_SERIAL_ENABLED