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_LightWareSerial.h"
      17             : 
      18             : #if AP_RANGEFINDER_LIGHTWARE_SERIAL_ENABLED
      19             : 
      20             : #include <AP_HAL/AP_HAL.h>
      21             : #include <ctype.h>
      22             : 
      23             : extern const AP_HAL::HAL& hal;
      24             : 
      25             : #define LIGHTWARE_DIST_MAX_CM           10000
      26             : #define LIGHTWARE_OUT_OF_RANGE_ADD_CM   100
      27             : 
      28             : // read - return last value measured by sensor
      29        5572 : bool AP_RangeFinder_LightWareSerial::get_reading(float &reading_m)
      30             : {
      31        5572 :     if (uart == nullptr) {
      32           0 :         return false;
      33             :     }
      34             : 
      35        5572 :     float sum = 0;              // sum of all readings taken
      36        5572 :     uint16_t valid_count = 0;   // number of valid readings
      37        5572 :     uint16_t invalid_count = 0; // number of invalid readings
      38             : 
      39             :     // max distance the sensor can reliably measure - read from parameters
      40        5572 :     const auto distance_cm_max = max_distance()*100;
      41             : 
      42             :     // read any available lines from the lidar
      43       12392 :     for (auto i=0; i<8192; i++) {
      44             :         uint8_t c;
      45       12392 :         if (!uart->read(c)) {
      46        5572 :             break;
      47             :         }
      48             :         // use legacy protocol
      49        6820 :         if (protocol_state == ProtocolState::UNKNOWN || protocol_state == ProtocolState::LEGACY) {
      50        6152 :             if (c == '\r') {
      51        1056 :                 linebuf[linebuf_len] = 0;
      52        1056 :                 const float dist = strtof(linebuf, nullptr);
      53        1056 :                 if (!is_negative(dist) && !is_lost_signal_distance(dist * 100, distance_cm_max)) {
      54         676 :                     sum += dist;
      55         676 :                     valid_count++;
      56             :                     // if still determining protocol update legacy valid count
      57         676 :                     if (protocol_state == ProtocolState::UNKNOWN) {
      58          22 :                         legacy_valid_count++;
      59             :                     }
      60             :                 } else {
      61         380 :                     invalid_count++;
      62             :                 }
      63        1056 :                 linebuf_len = 0;
      64        5096 :             } else if (isdigit(c) || c == '.' || c == '-') {
      65        5074 :                 linebuf[linebuf_len++] = c;
      66        5074 :                 if (linebuf_len == sizeof(linebuf)) {
      67             :                     // too long, discard the line
      68           0 :                     linebuf_len = 0;
      69             :                 }
      70             :             }
      71             :         }
      72             : 
      73             :         // use binary protocol
      74        6820 :         if (protocol_state == ProtocolState::UNKNOWN || protocol_state == ProtocolState::BINARY) {
      75         800 :             bool msb_set = BIT_IS_SET(c, 7);
      76         800 :             if (msb_set) {
      77             :                 // received the high byte
      78         345 :                 high_byte = c;
      79         345 :                 high_byte_received = true;
      80             :             } else {
      81             :                 // received the low byte which should be second
      82         455 :                 if (high_byte_received) {
      83         345 :                     const int16_t dist = (high_byte & 0x7f) << 7 | (c & 0x7f);
      84         345 :                     if (dist >= 0 && !is_lost_signal_distance(dist, distance_cm_max)) {
      85         345 :                         sum += dist * 0.01f;
      86         345 :                         valid_count++;
      87             :                         // if still determining protocol update binary valid count
      88         345 :                         if (protocol_state == ProtocolState::UNKNOWN) {
      89          11 :                             binary_valid_count++;
      90             :                         }
      91             :                     } else {
      92           0 :                         invalid_count++;
      93             :                     }
      94             :                 }
      95         455 :                 high_byte_received = false;
      96             :             }
      97             :         }
      98             :     }
      99             : 
     100             :     // protocol set after 10 successful reads
     101        5572 :     if (protocol_state == ProtocolState::UNKNOWN) {
     102         116 :         if (binary_valid_count > 10) {
     103           1 :             protocol_state = ProtocolState::BINARY;
     104         115 :         } else if (legacy_valid_count > 10) {
     105           2 :             protocol_state = ProtocolState::LEGACY;
     106             :         }
     107             :     }
     108             : 
     109        5572 :     uint32_t now = AP_HAL::millis();
     110        5572 :     if (last_init_ms == 0 ||
     111        5569 :         (now - last_init_ms > 1000 &&
     112        5509 :          now - state.last_reading_ms > 1000)) {
     113             :         // send enough serial transitions to trigger LW20 into serial
     114             :         // mode. It starts in dual I2C/serial mode, and wants to see
     115             :         // enough transitions to switch into serial mode.
     116           3 :         uart->write("www\r\n");
     117           3 :         last_init_ms = now;
     118             :     } else {
     119        5569 :         uart->write('d');
     120             :     }
     121             : 
     122             :     // return average of all valid readings
     123        5572 :     if (valid_count > 0) {
     124        1015 :         reading_m = sum / valid_count;
     125        1015 :         no_signal = false;
     126        1015 :         return true;
     127             :     }
     128             : 
     129             :     // all readings were invalid so return out-of-range-high value
     130        4557 :     if (invalid_count > 0) {
     131         380 :         reading_m = MAX(LIGHTWARE_DIST_MAX_CM, distance_cm_max + LIGHTWARE_OUT_OF_RANGE_ADD_CM);
     132         380 :         no_signal = true;
     133         380 :         return true;
     134             :     }
     135             : 
     136             :     // no readings so return false
     137        4177 :     return false;
     138             : }
     139             : 
     140             : // check to see if distance returned by the LiDAR is a known lost-signal distance flag
     141        1401 : bool AP_RangeFinder_LightWareSerial::is_lost_signal_distance(int16_t distance_cm, int16_t distance_cm_max)
     142             : {
     143        1401 :     if (distance_cm < distance_cm_max + LIGHTWARE_OUT_OF_RANGE_ADD_CM) {
     144             :         // in-range
     145         928 :         return false;
     146             :     }
     147         473 :     const int16_t bad_distances[] { 13000, 16000, 23000, 25000 };
     148         845 :     for (const auto bad_distance_cm : bad_distances) {
     149         752 :         if (distance_cm == bad_distance_cm) {
     150         380 :             return true;
     151             :         }
     152             :     }
     153          93 :     return false;
     154             : }
     155             : 
     156             : #endif