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             :    This program is distributed in the hope that it will be useful,
       7             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       8             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
       9             :    GNU General Public License for more details.
      10             :    You should have received a copy of the GNU General Public License
      11             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      12             :  */
      13             : 
      14             : /*
      15             :  * ArduPilot device driver for Inno-Maker LD06 LiDAR
      16             :  *
      17             :  * ALL INFORMATION REGARDING PROTOCOL WAS DERIVED FROM InnoMaker DATASHEET:
      18             :  *
      19             :  * http://wiki.inno-maker.com/display/HOMEPAGE/LD06?preview=/6949506/6949511/LDROBOT_LD06_Development%20manual_v1.0_en.pdf
      20             :  *
      21             :  * Author: Adithya Patil, Georgia Institute of Technology
      22             :  * Based on the SLAMTEC RPLiDAR code written by Steven Josefs, IAV GmbH and CYGBOT D1 LiDAR code
      23             :  *
      24             :  */
      25             : 
      26             : #include "AP_Proximity_config.h"
      27             : 
      28             : #if AP_PROXIMITY_LD06_ENABLED
      29             : #include "AP_Proximity_LD06.h"
      30             : 
      31             : #define LD_START_CHAR       0x54
      32             : #define PROXIMITY_LD06_TIMEOUT_MS 50
      33             : 
      34             : // Indices in data array where each value starts being recorded
      35             : // See comment below about data payload for more info about formatting
      36             : #define START_DATA_LENGTH           1
      37             : #define START_BEGIN_ANGLE           4
      38             : #define START_PAYLOAD               6
      39             : #define START_END_ANGLE             42
      40             : #define START_CHECK_SUM             46
      41             : #define MEASUREMENT_PAYLOAD_LENGTH  3
      42             : #define PAYLOAD_COUNT               12
      43             : 
      44             : // confidence for each measurement must be this value or higher
      45             : #define CONFIDENCE_THRESHOLD 20
      46             : 
      47             :  /* ------------------------------------------
      48             :     Data Packet Structure:
      49             :     Start Character : 1 Byte
      50             :     Data Length : 1 Byte
      51             :     Radar Speed : 2 Bytes
      52             :     Start Angle : 2 Bytes
      53             :     Data Measurements : 36 Bytes
      54             :         Contains 12 measurements of 3 Bytes each
      55             :         Each measurement has 2 Bytes for distance to closest object
      56             :         Each measurement has the 3rd Byte as measurement Confidence
      57             :     End Angle : 2 Bytes
      58             :     Timestamp : 2 Bytes
      59             :     Checksum : 1 Byte
      60             :     ------------------------------------------ */
      61             : // ----> 47 data bytes in total for one packet
      62             : 
      63             : // Update the sensor readings
      64        8243 : void AP_Proximity_LD06::update(void)
      65             : {
      66             :     // Escape if no connection detected/supported while running
      67        8243 :     if (_uart == nullptr) {
      68           0 :         return;
      69             :     }
      70             : 
      71             :     // Begin getting sensor readings
      72             :     // Calls method that repeatedly reads through UART channel
      73        8243 :     get_readings();
      74             : 
      75             :     // Check if the data is being received correctly and sets Proximity Status
      76        8243 :     if (_last_distance_received_ms == 0 || (AP_HAL::millis() - _last_distance_received_ms > PROXIMITY_LD06_TIMEOUT_MS)) {
      77           0 :         set_status(AP_Proximity::Status::NoData);
      78             :     } else {
      79        8243 :         set_status(AP_Proximity::Status::Good);
      80             :     }
      81             : }
      82             : 
      83             : // Called repeatedly to get the readings at the current instant
      84        8243 : void AP_Proximity_LD06::get_readings()
      85             : {
      86        8243 :     if (_uart == nullptr) {
      87           0 :         return;
      88             :     }
      89             : 
      90             :     // Store the number of bytes available on the UART input
      91        8243 :     uint32_t nbytes = MIN((uint16_t) 4000,  _uart->available());
      92             : 
      93             :     // Loops through all bytes that were received
      94      989644 :     while (nbytes-- > 0) {
      95             : 
      96             :         uint8_t c;
      97      981401 :         if (!_uart->read(c)) {
      98           0 :             break;
      99             :         }
     100             : 
     101             :         // Stores the byte in an array if the byte is a start byte or we have already read a start byte
     102      981401 :         if (c == LD_START_CHAR || _byte_count) {
     103             :             // Stores the next byte in an array
     104      981360 :             _response[_byte_count] = c;
     105      981360 :             if (_byte_count < START_DATA_LENGTH) {
     106       20880 :                 _byte_count++;
     107       20880 :                 continue;
     108             :             }
     109             : 
     110             :             // total_packet_length = sizeof(header) + datalength + sizeof(footer):
     111      960480 :             const uint32_t total_packet_length = 6 + 3*(_response[START_DATA_LENGTH] & 0x1F) + 5;
     112      960480 :             if ((_response[START_DATA_LENGTH] & 0x1F) != PAYLOAD_COUNT ||
     113             :                 total_packet_length > ARRAY_SIZE(_response)) {
     114             :                 // invalid packet received; throw away all data and
     115             :                 // start again.
     116           0 :                 _byte_count = 0;
     117           0 :                 _uart->discard_input();
     118           0 :                 break;
     119             :             }
     120             : 
     121      960480 :             _byte_count++;
     122             : 
     123      960480 :             if (_byte_count == total_packet_length) {
     124             : 
     125       20880 :                 const uint32_t current_ms = AP_HAL::millis();
     126             : 
     127       20880 :                 _last_distance_received_ms =  current_ms;
     128             : 
     129             :                 // Updates the temporary boundary and passes off the completed data
     130       20880 :                 parse_response_data();
     131             : 
     132             :                 // Resets the bytes read and whether or not we are reading data to accept a new payload
     133       20880 :                 _byte_count = 0;
     134             :             }
     135             :         }
     136             :     }
     137             : }
     138             : 
     139             : // Parses the data packet received from the LiDAR
     140       20880 : void AP_Proximity_LD06::parse_response_data()
     141             : {
     142             : 
     143             :     // Data interpretation based on:
     144             :     // http://wiki.inno-maker.com/display/HOMEPAGE/LD06?preview=/6949506/6949511/LDROBOT_LD06_Development%20manual_v1.0_en.pdf
     145             : 
     146             :     // Second byte in array stores length of data - not used but stored for debugging
     147             :     // const uint8_t data_length = _response[START_DATA_LENGTH];
     148             : 
     149             :     // Verify the checksum that is stored in the last element of the response array
     150             :     // Return if checksum is incorrect - i.e. bad data, bad readings, etc.
     151       20880 :     const uint8_t check_sum = _response[START_CHECK_SUM];
     152       20880 :     if (check_sum != crc8_generic(&_response[0], sizeof(_response) / sizeof(_response[0]) - 1, 0x4D)) {
     153           1 :         return;
     154             :     }
     155             : 
     156             :     // Respective bits store the radar speed, start/end angles
     157             :     // Use bitwise operations to correctly obtain correct angles
     158             :     // Divide angles by 100 as per manual
     159       20879 :     const float start_angle = float(UINT16_VALUE(_response[START_BEGIN_ANGLE + 1], _response[START_BEGIN_ANGLE])) * 0.01;
     160       20879 :     const float end_angle = float(UINT16_VALUE(_response[START_END_ANGLE + 1], _response[START_END_ANGLE])) * 0.01;
     161             : 
     162             :     float angle_step;
     163       20879 :     if (start_angle < end_angle) {
     164       20546 :         angle_step = (end_angle - start_angle) / (PAYLOAD_COUNT - 1);
     165             :     } else {
     166         333 :         angle_step = (end_angle + 360 - start_angle) / (PAYLOAD_COUNT - 1);
     167             :     }
     168             :     // Each recording point is three bytes long, goes through all of that and updates database
     169      271427 :     for (uint16_t i = START_PAYLOAD; i < START_PAYLOAD + MEASUREMENT_PAYLOAD_LENGTH * PAYLOAD_COUNT; i += MEASUREMENT_PAYLOAD_LENGTH) {
     170             : 
     171             :         // Gets the distance recorded and converts to meters
     172      250548 :         const float angle_deg = correct_angle_for_orientation(start_angle + angle_step * (i / MEASUREMENT_PAYLOAD_LENGTH));
     173      250548 :         const float distance_m = _dist_filt_mm.apply(UINT16_VALUE(_response[i + 1], _response[i])) * 0.001;
     174      250548 :         const float confidence = _response[i + 2];
     175             : 
     176             :         // ignore distance that are out-of-range or have low confidence
     177      250548 :         if (distance_m < distance_min_m() || distance_m > distance_max_m() || confidence < CONFIDENCE_THRESHOLD) {
     178      219781 :             continue;
     179             :         }
     180             : 
     181             :         // ignore distances that are out-of-range (based on user parameters) or within ignore areas
     182       30767 :         if (ignore_reading(angle_deg, distance_m)) {
     183           0 :             continue;
     184             :         }
     185             : 
     186             :         // use the shortest distance within 2 degree sectors
     187       30767 :         const uint16_t a2d = (int)(angle_deg / 2.0) * 2;
     188       30767 :         if (_angle_2deg == a2d) {
     189       22782 :             if (distance_m < _dist_2deg_m) {
     190        6063 :                 _dist_2deg_m = distance_m;
     191             :             }
     192             :         } else {
     193             :             // new 2 degree sector, process the old one
     194             : 
     195             :             // check if new sector is also a new face
     196        7985 :             const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face((float)_angle_2deg);
     197        7985 :             if (face != _last_face) {
     198             :                 // distance is for a new face, the previous one can be updated now
     199        1340 :                 if (_last_distance_valid) {
     200        1339 :                     frontend.boundary.set_face_attributes(_last_face, _last_angle_deg, _last_distance_m, state.instance);
     201             :                 } else {
     202             :                     // reset distance from last face
     203           1 :                     frontend.boundary.reset_face(face, state.instance);
     204             :                 }
     205             : 
     206             :                 // initialize the new face
     207        1340 :                 _last_face = face;
     208        1340 :                 _last_distance_valid = false;
     209             :             }
     210             : 
     211             :             // update face's shortest distance
     212        7985 :             if (!_last_distance_valid || (_dist_2deg_m < _last_distance_m)) {
     213        4590 :                 _last_distance_m = _dist_2deg_m;
     214        4590 :                 _last_distance_valid = true;
     215        4590 :                 _last_angle_deg = (float)_angle_2deg;
     216             :             }
     217             : 
     218             :             // update OA database with the 2 degree sectors distance
     219        7985 :             database_push(_angle_2deg, _dist_2deg_m);
     220             : 
     221             :             // advance to the next 2 degree sector
     222        7985 :             _angle_2deg = a2d;
     223        7985 :             _dist_2deg_m = distance_m;
     224             :         }
     225             :     }
     226             : }
     227             : #endif // AP_PROXIMITY_LD06_ENABLED