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             :   driver for Lanbao PSK-CM8JL65-CC5 Lidar
      17             :  */
      18             : #include "AP_RangeFinder_Lanbao.h"
      19             : 
      20             : #if AP_RANGEFINDER_LANBAO_ENABLED
      21             : 
      22             : #include <AP_HAL/AP_HAL.h>
      23             : #include <AP_Math/crc.h>
      24             : #include <stdio.h>
      25             : 
      26             : extern const AP_HAL::HAL& hal;
      27             : 
      28             : /*
      29             :   this sensor has no way of reporting "out of range", it will keep
      30             :   reporting distances at of around 7 to 8 meters even when pointed at
      31             :   the sky. For this reason we limit the max range to 6 meters as
      32             :   otherwise we may be giving false data
      33             :  */
      34             : #define LANBAO_MAX_RANGE_M 6
      35             : 
      36             : // read - return last value measured by sensor
      37        1341 : bool AP_RangeFinder_Lanbao::get_reading(float &reading_m)
      38             : {
      39        1341 :     if (uart == nullptr) {
      40           0 :         return false;
      41             :     }
      42             : 
      43        1341 :     float sum_range = 0;
      44        1341 :     uint32_t count = 0;
      45             : 
      46             :     // format is: [ 0xA5 | 0x5A | distance-MSB-mm | distance-LSB-mm | crc16 ]
      47             : 
      48             :     // read any available lines from the lidar
      49        3405 :     for (auto i=0; i<8192; i++) {
      50             :         uint8_t b;
      51        3405 :         if (!uart->read(b)) {
      52        1341 :             break;
      53             :         }
      54        2064 :         if (buf_len == 0 && b != 0xA5) {
      55             :             // discard
      56           0 :             continue;
      57             :         }
      58        2064 :         if (buf_len == 1 && b != 0x5A) {
      59             :             // discard
      60           0 :             if (b == 0xA5) {
      61           0 :                 buf[0] = b;
      62             :             } else {
      63           0 :                 buf_len = 0;
      64             :             }
      65           0 :             continue;
      66             :         }
      67        2064 :         buf[buf_len++] = b;
      68        2064 :         if (buf_len == sizeof(buf)) {
      69         344 :             buf_len = 0;
      70         344 :             uint16_t crc = (buf[5]<<8) | buf[4];
      71         344 :             if (crc != calc_crc_modbus(buf, 4)) {
      72             :                 // bad CRC, discard
      73           0 :                 continue;
      74             :             }
      75         344 :             sum_range += float((buf[2]<<8) | buf[3]) * 0.001;
      76         344 :             count++;
      77             :         }
      78             :     }
      79        1341 :     if (count > 0) {
      80         337 :         reading_m = (sum_range / count);
      81         337 :         return reading_m <= LANBAO_MAX_RANGE_M?true:false;
      82             :     }
      83        1004 :     return false;
      84             : }
      85             : 
      86             : #endif  // AP_RANGEFINDER_LANBAO_ENABLED