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_LeddarOne.h"
      17             : 
      18             : #if AP_RANGEFINDER_LEDDARONE_ENABLED
      19             : 
      20             : #include <AP_HAL/AP_HAL.h>
      21             : #include <AP_Math/crc.h>
      22             : 
      23             : extern const AP_HAL::HAL& hal;
      24             : 
      25             : // read - return last value measured by sensor
      26        1356 : bool AP_RangeFinder_LeddarOne::get_reading(float &reading_m)
      27             : {
      28             :     uint8_t number_detections;
      29             :     LeddarOne_Status leddarone_status;
      30             : 
      31        1356 :     if (uart == nullptr) {
      32           0 :         return false;
      33             :     }
      34             : 
      35        1356 :     switch (modbus_status) {
      36         336 :     case LEDDARONE_MODBUS_STATE_INIT: {
      37         336 :         uart->discard_input();
      38             : 
      39             :         // clear buffer and buffer_len
      40         336 :         memset(read_buffer, 0, sizeof(read_buffer));
      41         336 :         read_len = 0;
      42         336 :         modbus_status = LEDDARONE_MODBUS_STATE_PRE_SEND_REQUEST;
      43             :         }
      44             : 
      45             :         // fall through to next state LEDDARONE_MODBUS_STATE_PRE_SEND_REQUEST
      46             :         // immediately
      47             :         FALLTHROUGH;
      48             : 
      49         342 :     case LEDDARONE_MODBUS_STATE_PRE_SEND_REQUEST:
      50             :         // send a request message for Modbus function 4
      51         342 :         uart->write(send_request_buffer, sizeof(send_request_buffer));
      52         342 :         modbus_status = LEDDARONE_MODBUS_STATE_SENT_REQUEST;
      53         342 :         last_sending_request_ms = AP_HAL::millis();
      54             :         FALLTHROUGH;
      55             : 
      56        1015 :     case LEDDARONE_MODBUS_STATE_SENT_REQUEST:
      57        1015 :         if (uart->available()) {
      58             :             // change mod_bus status to read available buffer
      59         342 :             modbus_status = LEDDARONE_MODBUS_STATE_AVAILABLE;
      60         342 :             last_available_ms = AP_HAL::millis();
      61             :         } else {
      62         673 :             if (AP_HAL::millis() - last_sending_request_ms > 200) {
      63             :                 // reset mod_bus status to read new buffer
      64             :                 // if read_len is zero, send request without initialize
      65           0 :                 modbus_status = (read_len == 0) ? LEDDARONE_MODBUS_STATE_PRE_SEND_REQUEST : LEDDARONE_MODBUS_STATE_INIT;
      66             :             }
      67             :         }
      68        1015 :         break;
      69             : 
      70         341 :     case LEDDARONE_MODBUS_STATE_AVAILABLE:
      71             :         // parse a response message, set number_detections, detections and sum_distance
      72         341 :         leddarone_status = parse_response(number_detections);
      73             : 
      74         341 :         if (leddarone_status == LEDDARONE_STATE_OK) {
      75         335 :             reading_m = (sum_distance_mm * 0.001f) / number_detections;
      76             : 
      77             :             // reset mod_bus status to read new buffer
      78         335 :             modbus_status = LEDDARONE_MODBUS_STATE_INIT;
      79             : 
      80         335 :             return true;
      81             :         }
      82             :         // if status is not reading buffer, reset mod_bus status to read new buffer
      83           6 :         else if (leddarone_status != LEDDARONE_STATE_READING_BUFFER || AP_HAL::millis() - last_available_ms > 200) {
      84             :             // if read_len is zero, send request without initialize
      85           6 :             modbus_status = (read_len == 0) ? LEDDARONE_MODBUS_STATE_PRE_SEND_REQUEST : LEDDARONE_MODBUS_STATE_INIT;
      86             :         }
      87           6 :         break;
      88             :     }
      89             : 
      90        1021 :     return false;
      91             : }
      92             : 
      93             : /*
      94             :    CRC16
      95             :    CRC-16-IBM(x16+x15+x2+1)
      96             : */
      97         335 : bool AP_RangeFinder_LeddarOne::CRC16(uint8_t *aBuffer, uint8_t aLength, bool aCheck)
      98             : {
      99         335 :     uint16_t crc = calc_crc_modbus(aBuffer, aLength);
     100             : 
     101         335 :     uint8_t lCRCLo = LOWBYTE(crc);
     102         335 :     uint8_t lCRCHi = HIGHBYTE(crc);
     103             : 
     104         335 :     if (aCheck) {
     105         335 :         return (aBuffer[aLength] == lCRCLo) && (aBuffer[aLength+1] == lCRCHi);
     106             :     } else {
     107           0 :         aBuffer[aLength] = lCRCLo;
     108           0 :         aBuffer[aLength+1] = lCRCHi;
     109           0 :         return true;
     110             :     }
     111             : }
     112             : 
     113             :  /*
     114             :     parse a response message from Modbus
     115             :     -----------------------------------------------
     116             :     [ read buffer packet ]
     117             :     -----------------------------------------------
     118             :       0: slave address (LEDDARONE_DEFAULT_ADDRESS)
     119             :       1: functions code
     120             :       2: byte count
     121             : 5-6-3-4: timestamp
     122             :     7-8: internal temperature
     123             :    9-10: number of detections
     124             :   11-12: first distance
     125             :   13-14: first amplitude
     126             :   15-16: second distance
     127             :   17-18: second amplitude
     128             :   19-20: third distances
     129             :   21-22: third amplitude
     130             :      23: CRC Low
     131             :      24: CRC High
     132             :     -----------------------------------------------
     133             :   */
     134         341 : LeddarOne_Status AP_RangeFinder_LeddarOne::parse_response(uint8_t &number_detections)
     135             : {
     136         341 :     uint8_t index_offset = LEDDARONE_DETECTION_DATA_INDEX_OFFSET;
     137             : 
     138             :     // read serial
     139         341 :     uint32_t nbytes = uart->available();
     140             : 
     141         341 :     if (nbytes != 0)  {
     142        8866 :         for (uint8_t index=read_len; index<nbytes+read_len; index++) {
     143        8531 :             if (index >= LEDDARONE_READ_BUFFER_SIZE) {
     144           6 :                 return LEDDARONE_STATE_ERR_BAD_RESPONSE;
     145             :             }
     146        8525 :             read_buffer[index] = uart->read();
     147             :         }
     148             : 
     149         335 :         read_len += nbytes;
     150             : 
     151         335 :         if (read_len < LEDDARONE_READ_BUFFER_SIZE) {
     152           0 :             return LEDDARONE_STATE_READING_BUFFER;
     153             :         }
     154             :     }
     155             : 
     156             :     // read_len is not 25 byte or function code is not 0x04
     157         335 :     if (read_len != LEDDARONE_READ_BUFFER_SIZE || read_buffer[1] != LEDDARONE_MODOBUS_FUNCTION_CODE) {
     158           0 :         return LEDDARONE_STATE_ERR_BAD_RESPONSE;
     159             :     }
     160             : 
     161             :     // CRC16
     162         335 :     if (!CRC16(read_buffer, read_len-2, true)) {
     163           0 :         return LEDDARONE_STATE_ERR_BAD_CRC;
     164             :     }
     165             : 
     166             :     // number of detections (index:10)
     167         335 :     number_detections = read_buffer[LEDDARONE_DETECTION_DATA_NUMBER_INDEX];
     168             : 
     169             :     // if the number of detection is over or zero , it is false
     170         335 :     if (number_detections > LEDDARONE_DETECTIONS_MAX || number_detections == 0) {
     171           0 :         return LEDDARONE_STATE_ERR_NUMBER_DETECTIONS;
     172             :     }
     173             : 
     174         335 :     sum_distance_mm = 0;
     175         670 :     for (uint8_t index=0; index<number_detections; index++) {
     176             :         // construct data word from two bytes
     177         335 :         sum_distance_mm += read_buffer[index_offset]<<8 | read_buffer[index_offset+1];
     178             : 
     179             :         // add index offset (4) to read next detection data
     180         335 :         index_offset += LEDDARONE_DETECTION_DATA_OFFSET;
     181             :     }
     182             : 
     183         335 :     return LEDDARONE_STATE_OK;
     184             : }
     185             : 
     186             : #endif  // AP_RANGEFINDER_LEDDARONE_ENABLED