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_LeddarVu8.h"
      17             : 
      18             : #if AP_RANGEFINDER_LEDDARVU8_ENABLED
      19             : 
      20             : #include <AP_HAL/AP_HAL.h>
      21             : #include <ctype.h>
      22             : 
      23             : extern const AP_HAL::HAL& hal;
      24             : 
      25             : // LeddarVu8 uses the modbus RTU protocol
      26             : // https://autonomoustuff.com/product/leddartech-vu8/
      27             : 
      28             : #define LEDDARVU8_ADDR_DEFAULT              0x01        // modbus default device id
      29             : #define LEDDARVU8_DIST_MAX                  185.00      // maximum possible distance reported by lidar
      30             : #define LEDDARVU8_OUT_OF_RANGE_ADD          1.00        // add this many metres to out-of-range values
      31             : #define LEDDARVU8_TIMEOUT_MS                200         // timeout in milliseconds if no distance messages received
      32             : 
      33             : // distance returned in reading_cm, signal_ok is set to true if sensor reports a strong signal
      34           0 : bool AP_RangeFinder_LeddarVu8::get_reading(float &reading_m)
      35             : {
      36           0 :     if (uart == nullptr) {
      37           0 :         return false;
      38             :     }
      39             : 
      40             :     // check for timeout receiving messages
      41           0 :     uint32_t now_ms = AP_HAL::millis();
      42           0 :     if (((now_ms - last_distance_ms) > LEDDARVU8_TIMEOUT_MS) && ((now_ms - last_distance_request_ms) > LEDDARVU8_TIMEOUT_MS)) {
      43           0 :         request_distances();
      44             :     }
      45             : 
      46             :     // variables for averaging results from multiple messages
      47           0 :     float sum_cm = 0;
      48           0 :     uint16_t count = 0;
      49           0 :     uint16_t count_out_of_range = 0;
      50           0 :     uint16_t latest_dist_cm = 0;
      51           0 :     bool latest_dist_valid = false;
      52             : 
      53             :     // read any available characters from the lidar
      54           0 :     for (auto i=0; i<8192; i++) {
      55             :         uint8_t b;
      56           0 :         if (!uart->read(b)) {
      57           0 :             break;
      58             :         }
      59           0 :         if (parse_byte(b, latest_dist_valid, latest_dist_cm)) {
      60           0 :             if (latest_dist_valid) {
      61           0 :                 sum_cm += latest_dist_cm;
      62           0 :                 count++;
      63             :             } else {
      64           0 :                 count_out_of_range++;
      65             :             }
      66             : 
      67             :         }
      68             :     }
      69             : 
      70           0 :     if (count > 0 || count_out_of_range > 0) {
      71             :         // record time of successful read and request another reading
      72           0 :         last_distance_ms = now_ms;
      73           0 :         request_distances();
      74             : 
      75           0 :         if (count > 0) {
      76             :             // return average distance of readings
      77           0 :             reading_m = (sum_cm * 0.01f) / count;
      78             :         } else {
      79             :             // if only out of range readings return larger of
      80             :             // driver defined maximum range for the model and user defined max range + 1m
      81           0 :             reading_m = MAX(LEDDARVU8_DIST_MAX, max_distance() + LEDDARVU8_OUT_OF_RANGE_ADD);
      82             :         }
      83           0 :         return true;
      84             :     }
      85             : 
      86             :     // no readings so return false
      87           0 :     return false;
      88             : }
      89             : 
      90             : // get sensor address from RNGFNDx_ADDR parameter
      91           0 : uint8_t AP_RangeFinder_LeddarVu8::get_sensor_address() const
      92             : {
      93           0 :     if (params.address == 0) {
      94           0 :         return LEDDARVU8_ADDR_DEFAULT;
      95             :     }
      96           0 :     return params.address;
      97             : }
      98             : 
      99             : // send request to device to provide distances
     100           0 : void AP_RangeFinder_LeddarVu8::request_distances()
     101             : {
     102           0 :     uint8_t req_buf[] = {
     103           0 :             get_sensor_address(),                       // address
     104             :             (uint8_t)FunctionCode::READ_INPUT_REGISTER, // function code low
     105             :             0,                                          // function code high
     106             :             (uint8_t)RegisterNumber::FIRST_DISTANCE0,   // register number low
     107             :             0,                                          // register number high
     108             :             8,                                          // register count
     109             :             0,                                          // crc low
     110             :             0                                           // crc high
     111           0 :     };
     112           0 :     const uint8_t req_buf_len = sizeof(req_buf);
     113             : 
     114             :     // fill in crc bytes
     115           0 :     uint16_t crc = calc_crc_modbus(req_buf, req_buf_len - 2);
     116           0 :     req_buf[req_buf_len - 2] = LOWBYTE(crc);
     117           0 :     req_buf[req_buf_len - 1] = HIGHBYTE(crc);
     118             : 
     119             :     // send request to device
     120           0 :     uart->write(req_buf, req_buf_len);
     121             : 
     122             :     // record time of request
     123           0 :     last_distance_request_ms = AP_HAL::millis();
     124           0 : }
     125             : 
     126             : // process one byte received on serial port
     127             : // returns true if successfully parsed a message
     128             : // if distances are valid, valid_readings is set to true and distance is stored in reading_cm
     129           0 : bool AP_RangeFinder_LeddarVu8::parse_byte(uint8_t b, bool &valid_reading, uint16_t &reading_cm)
     130             : {
     131             :     // process byte depending upon current state
     132           0 :     switch (parsed_msg.state) {
     133             : 
     134           0 :     case ParseState::WAITING_FOR_ADDRESS: {
     135           0 :         if (b == get_sensor_address()) {
     136           0 :             parsed_msg.address = b;
     137           0 :             parsed_msg.state = ParseState::WAITING_FOR_FUNCTION_CODE;
     138             :         }
     139           0 :         break;
     140             :     }
     141             : 
     142           0 :     case ParseState::WAITING_FOR_FUNCTION_CODE:
     143           0 :         if (b == (uint8_t)FunctionCode::READ_INPUT_REGISTER) {
     144           0 :             parsed_msg.function_code = b;
     145           0 :             parsed_msg.state = ParseState::WAITING_FOR_PAYLOAD_LEN;
     146             :         } else {
     147           0 :             parsed_msg.state = ParseState::WAITING_FOR_ADDRESS;
     148             :         }
     149           0 :         break;
     150             : 
     151           0 :     case ParseState::WAITING_FOR_PAYLOAD_LEN:
     152             :         // only parse messages of the expected length
     153           0 :         if (b == LEDDARVU8_PAYLOAD_LENGTH) {
     154           0 :             parsed_msg.payload_len = b;
     155           0 :             parsed_msg.payload_recv = 0;
     156           0 :             parsed_msg.state = ParseState::WAITING_FOR_PAYLOAD;
     157             :         } else {
     158           0 :             parsed_msg.state = ParseState::WAITING_FOR_ADDRESS;
     159             :         }
     160           0 :         break;
     161             : 
     162           0 :     case ParseState::WAITING_FOR_PAYLOAD:
     163           0 :         if (parsed_msg.payload_recv < parsed_msg.payload_len) {
     164           0 :             if (parsed_msg.payload_recv < ARRAY_SIZE(parsed_msg.payload)) {
     165           0 :                 parsed_msg.payload[parsed_msg.payload_recv] = b;
     166             :             }
     167           0 :             parsed_msg.payload_recv++;
     168             :         }
     169           0 :         if (parsed_msg.payload_recv == parsed_msg.payload_len) {
     170           0 :             parsed_msg.state = ParseState::WAITING_FOR_CRC_LOW;
     171             :         }
     172           0 :         break;
     173             : 
     174           0 :     case ParseState::WAITING_FOR_CRC_LOW:
     175           0 :         parsed_msg.crc = b;
     176           0 :         parsed_msg.state = ParseState::WAITING_FOR_CRC_HIGH;
     177           0 :         break;
     178             : 
     179           0 :     case ParseState::WAITING_FOR_CRC_HIGH: {
     180           0 :         parsed_msg.crc |= ((uint16_t)b << 8);
     181           0 :         parsed_msg.state = ParseState::WAITING_FOR_ADDRESS;
     182             : 
     183             :         // check crc
     184           0 :         uint16_t expected_crc = calc_crc_modbus(&parsed_msg.address, 3+parsed_msg.payload_recv);
     185           0 :         if (expected_crc == parsed_msg.crc) {
     186             :             // calculate and return shortest distance
     187           0 :             reading_cm = 0;
     188           0 :             valid_reading = false;
     189           0 :             for (uint8_t i=0; i<8; i++) {
     190           0 :                 uint8_t ix2 = i*2;
     191           0 :                 const uint16_t dist_cm = (uint16_t)parsed_msg.payload[ix2] << 8 | (uint16_t)parsed_msg.payload[ix2+1];
     192           0 :                 if ((dist_cm > 0) && (!valid_reading || (dist_cm < reading_cm))) {
     193           0 :                     reading_cm = dist_cm;
     194           0 :                     valid_reading = true;
     195             :                 }
     196             :             }
     197           0 :             return true;
     198             :         }
     199           0 :         break;
     200             :     }
     201             :     }
     202             : 
     203           0 :     valid_reading = false;
     204           0 :     return false;
     205             : }
     206             : 
     207             : #endif  // AP_RANGEFINDER_LEDDARVU8_ENABLED