AP_RangeFinder: investigate and resolve RPLidar startup issue

[rmackay9]

This reports comes from this discussion in the Rover-4.0 category: https://discuss.ardupilot.org/t/bad-lidar-health-message-with-rplidar-a2m8/52350

At least 3 users have reported that the RPLidar must be powered up before the flight controller or "Bad Lidar Health" messages appear on the HUD.

We should investigate and then try to resolve this issue. Perhaps there are some startup messages sent by the driver that we should keep resending until we start receiving distances

[andresrcdev]

The issue could be here in AP_Proximity_RPLidarA2.cpp

bool AP_Proximity_RPLidarA2::initialise()
{
// initialise boundary
init_boundary();

if (!_initialised) {
    reset_rplidar();            // set to a known state
    Debug(1, "LIDAR initialised");
}

return true;

}

This returns _initialised=true, but it is not really checked that the lidar is working.

Also, following slamtec rplidar documentation, initialising scheme should be:

Let me check the code to see if I can help with something.

[andresrcdev]

I have checked the code and I would like to make this suggestion to modify the initialise function.
The problem is that my coding skills are limited so I would need someone that could review the code and upload to test.
The AP_Proximity_RPLidarA2.cpp could be like that

#include <AP_HAL/AP_HAL.h>
#include "AP_Proximity_RPLidarA2.h"
#include <AP_SerialManager/AP_SerialManager.h>
#include <ctype.h>
#include <stdio.h>


#define RP_DEBUG_LEVEL 0

#if RP_DEBUG_LEVEL
  #include <GCS_MAVLink/GCS.h>
  #define Debug(level, fmt, args ...)  do { if (level <= RP_DEBUG_LEVEL) { gcs().send_text(MAV_SEVERITY_INFO, fmt, ## args); } } while (0)
#else
  #define Debug(level, fmt, args ...)
#endif

#define COMM_ACTIVITY_TIMEOUT_MS        200
#define RESET_RPA2_WAIT_MS              2                   // Acording to RPLidar A2 Documentation
#define RESYNC_TIMEOUT                  5000

// Commands
//-----------------------------------------

// Commands without payload and response
#define RPLIDAR_PREAMBLE               0xA5
#define RPLIDAR_CMD_STOP               0x25
#define RPLIDAR_CMD_SCAN               0x20
#define RPLIDAR_CMD_FORCE_SCAN         0x21
#define RPLIDAR_CMD_RESET              0x40

// Commands without payload but have response
#define RPLIDAR_CMD_GET_DEVICE_INFO    0x50
#define RPLIDAR_CMD_GET_DEVICE_HEALTH  0x52

// Commands with payload and have response
#define RPLIDAR_CMD_EXPRESS_SCAN       0x82

extern const AP_HAL::HAL& hal;

/*
   The constructor also initialises the proximity sensor. Note that this
   constructor is not called until detect() returns true, so we
   already know that we should setup the proximity sensor
 */
AP_Proximity_RPLidarA2::AP_Proximity_RPLidarA2(
    AP_Proximity &_frontend,
    AP_Proximity::Proximity_State &_state) :
    AP_Proximity_Backend(_frontend, _state)
{
    const AP_SerialManager &serial_manager = AP::serialmanager();
    _uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0);
    if (_uart != nullptr) {
        _uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar360, 0));
    }
    _cnt = 0;
    _cnt_resetted = 0 ;
    _sync_error = 0 ;
    _byte_count = 0;
    _rplidar_mech_error_cnt = 0;
    _rplidar_comm_error_cnt = 0;
    _rplidarOk = false;
    _sector_initialised = false;
    _initialised = false;
    _rp_state = rp_unknown;

}

// detect if a RPLidarA2 proximity sensor is connected by looking for a configured serial port
bool AP_Proximity_RPLidarA2::detect()
{
    return AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0) != nullptr;
}

// update the _rp_state of the sensor
void AP_Proximity_RPLidarA2::update(void)
{
    if (_uart == nullptr) {
        return;
    }

    // initialise sensor if necessary
    if (!_initialised) {
        _initialised = initialise();    //returns true if everything initialized properly
    }

    // if LIDAR in known state
    if (_initialised) {
        get_readings();
    }

    // check for timeout and set health status
    if ((_last_distance_received_ms == 0) || (AP_HAL::millis() - _last_distance_received_ms > COMM_ACTIVITY_TIMEOUT_MS)) {
        set_status(AP_Proximity::Status::NoData);
        Debug(1, "LIDAR NO DATA");
    } else {
        set_status(AP_Proximity::Status::Good);
    }
}

// get maximum distance (in meters) of sensor
float AP_Proximity_RPLidarA2::distance_max() const
{
    return 16.0f;  //16m max range RPLIDAR2, if you want to support the 8m version this is the only line to change
}

// get minimum distance (in meters) of sensor
float AP_Proximity_RPLidarA2::distance_min() const
{
    return 0.20f;  //20cm min range RPLIDAR2
}

bool AP_Proximity_RPLidarA2::initialise()
{
    // initialise sectors
    if (!_sector_initialised) {
        init_sectors();
    }
    if (!_initialised)  {         
        if (_uart == nullptr) {
            return false;
        }

        // Give time to the rplidar to reset
        if (_rp_state == rp_resetted) {
            if (AP_HAL::millis() - _last_reset_ms < RESET_RPA2_WAIT_MS) {
                return false;
            }
        }

        // in case we are already in rp_health we don't ask again for send_request_for_health
        if (_rp_state != rp_health) {
            _response_type == ResponseType_None;
            send_request_for_health();   // request health
            _last_request_ms = AP_HAL::millis();
            Debug(2, "             CURRENT STATE: %d ", _rp_state);
        }
        
        uint32_t nbytes = _uart->available();
        
        // if no readings yet, exit without initialising.
        // I am not sure if it is really needed as if nbytes = 0 then the code doesn't go through the while function.
        if (nbytes = 0) {
            _rplidar_comm_error_cnt++;
            if ((_rplidar_comm_error_cnt >= 20) || ((AP_HAL::millis() - _last_request_ms)) > COMM_ACTIVITY_TIMEOUT_MS) {
                set_status(AP_Proximity::Status::NoData);
                _rplidar_comm_error_cnt = 0;
            }
            return false;
        }

        while (nbytes-- > 0) {
            uint8_t c = _uart->read();
            Debug(2, "UART READ %x <%c>", c, c); //show HEX values
            if (c == RPLIDAR_PREAMBLE || _descriptor_data) {
                _descriptor_data = true;
                _descriptor[_byte_count] = c;
                _byte_count++;
                // descriptor packet has 7 byte in total
                if (_byte_count == sizeof(_descriptor)) {
                    Debug(2, "LIDAR DESCRIPTOR CATCHED");
                    _response_type = ResponseType_Descriptor;
                    // identify the payload data after the descriptor
                    parse_response_descriptor();
                    _byte_count = 0;
                    _descriptor_data = false;
                }
            }
            if (_response_type == ResponseType_Health) {
                Debug(3, "READ PAYLOAD");
                payload[_byte_count] = c;
                _byte_count++;

                if (_byte_count == _payload_length) {
                    Debug(2, "LIDAR MEASUREMENT CATCHED");
                    parse_response_data();
                    _byte_count = 0;
                    if (_rplidarOk = true) {
                        _rplidar_comm_error_cnt = 0;
                        _rplidar_mech_error_cnt = 0;
                        set_scan_mode();
                        set_status(AP_Proximity::Status::Good);
                        _initialised = true;
                    } else {
                        reset_rplidar();
                        _rplidar_mech_error_cnt++;
                        if (_rplidar_mech_error_cnt >= 4) {
                            _rplidar_mech_error_cnt = 0;
                            set_status(AP_Proximity::Status::MechFailure);
                        }
                    }

                }
            }
           
   
        }
    }

    return _initialised;
}

void AP_Proximity_RPLidarA2::reset_rplidar()
{
    if (_uart == nullptr) {
        return;
    }
    uint8_t tx_buffer[2] = {RPLIDAR_PREAMBLE, RPLIDAR_CMD_RESET};
    _uart->write(tx_buffer, 2);
    _resetted = true;   ///< be aware of extra 63 bytes coming after reset containing FW information
                                /// Documentation says that reset has no response, but I have not checked it.
    Debug(1, "LIDAR reset");
    _last_reset_ms =  AP_HAL::millis();
    _rp_state = rp_resetted;
    _initialised = false;
}

// initialise sector angles using user defined ignore areas, left same as SF40C
void AP_Proximity_RPLidarA2::init_sectors()
{
    // use defaults if no ignore areas defined
    const uint8_t ignore_area_count = get_ignore_area_count();
    if (ignore_area_count == 0) {
        _sector_initialised = true;
        return;
    }

    uint8_t sector = 0;
    for (uint8_t i=0; i<ignore_area_count; i++) {

        // get ignore area info
        uint16_t ign_area_angle;
        uint8_t ign_area_width;
        if (get_ignore_area(i, ign_area_angle, ign_area_width)) {

            // calculate how many degrees of space we have between this end of this ignore area and the start of the end
            int16_t start_angle, end_angle;
            get_next_ignore_start_or_end(1, ign_area_angle, start_angle);
            get_next_ignore_start_or_end(0, start_angle, end_angle);
            int16_t degrees_to_fill = wrap_360(end_angle - start_angle);

            // divide up the area into sectors
            while ((degrees_to_fill > 0) && (sector < PROXIMITY_SECTORS_MAX)) {
                uint16_t sector_size;
                if (degrees_to_fill >= 90) {
                    // set sector to maximum of 45 degrees
                    sector_size = 45;
                } else if (degrees_to_fill > 45) {
                    // use half the remaining area to optimise size of this sector and the next
                    sector_size = degrees_to_fill / 2.0f;
                } else  {
                    // 45 degrees or less are left so put it all into the next sector
                    sector_size = degrees_to_fill;
                }
                // record the sector middle and width
                _sector_middle_deg[sector] = wrap_360(start_angle + sector_size / 2.0f);
                _sector_width_deg[sector] = sector_size;

                // move onto next sector
                start_angle += sector_size;
                sector++;
                degrees_to_fill -= sector_size;
            }
        }
    }

    // set num sectors
    _num_sectors = sector;

    // re-initialise boundary because sector locations have changed
    init_boundary();

    // record success
    _sector_initialised = true;
}

// set Lidar into SCAN mode
void AP_Proximity_RPLidarA2::set_scan_mode()
{
    if (_uart == nullptr) {
        return;
    }
    uint8_t tx_buffer[2] = {RPLIDAR_PREAMBLE, RPLIDAR_CMD_SCAN};
    _uart->write(tx_buffer, 2);
    _last_request_ms = AP_HAL::millis();
    Debug(1, "LIDAR SCAN MODE ACTIVATED");
    _rp_state = rp_responding;
}

// send request for sensor health
void AP_Proximity_RPLidarA2::send_request_for_health()                                    //not called yet
{
    if (_uart == nullptr) {
        return;
    }
    uint8_t tx_buffer[2] = {RPLIDAR_PREAMBLE, RPLIDAR_CMD_GET_DEVICE_HEALTH};
    _uart->write(tx_buffer, 2);
    _last_request_ms = AP_HAL::millis();
    _rp_state = rp_health;
}

void AP_Proximity_RPLidarA2::get_readings()
{
    if (_uart == nullptr) {
        return;
    }
    Debug(2, "             CURRENT STATE: %d ", _rp_state);
    //   uint32_t nbytes = _uart->available(); meter dentro del switch


    STATE:
    switch(_rp_state){
        // we should not be here, as reset doesn't give any answer
        // just in case we give time to reset and then initialised = false to initialise again
        case rp_resetted:
            if (AP_HAL::milis() - _last_reset_ms < RESET_RPA2_WAIT_MS) {
                break;
            } else {
                _initialised = false;
            }
            break;

        case rp_responding:

            Debug(2, "RESPONDING");
            uint32_t nbytes = _uart->available();
            while (nbytes-- > 0) { 
                uint8_t c = _uart->read();
                if (c == RPLIDAR_PREAMBLE || _descriptor_data) {
                    _descriptor_data = true;
                    _descriptor[_byte_count] = c;
                    _byte_count++;
                    // descriptor packet has 7 byte in total
                    if (_byte_count == sizeof(_descriptor)) {
                        Debug(2,"LIDAR DESCRIPTOR CATCHED");
                        _response_type = ResponseType_Descriptor;
                        // identify the payload data after the descriptor
                        parse_response_descriptor();
                        _byte_count = 0;
                    }
                } else {
                    _rp_state = rp_unknown;
                }
            }
            break;

        case rp_measurements:
            uint32_t nbytes = _uart->available();
            while (nbytes-- > 0) {
                uint8_t c = _uart->read();
                if (_sync_error) {
                    // out of 5-byte sync mask -> catch new revolution
                    Debug(1, "       OUT OF SYNC");
                    // on first revolution bit 1 = 1, bit 2 = 0 of the first byte
                    if ((c & 0x03) == 0x01) {
                        _sync_error = 0;
                        Debug(1, "                  RESYNC");
                    }
                    else {
                        if (AP_HAL::millis() - _last_distance_received_ms > RESYNC_TIMEOUT) {
                            reset_rplidar();
                        }
                        break;
                    }
                }
                Debug(3, "READ PAYLOAD");
                payload[_byte_count] = c;
                _byte_count++;

                if (_byte_count == _payload_length) {
                    Debug(2, "LIDAR MEASUREMENT CATCHED");
                    parse_response_data();
                    _byte_count = 0;
                }
            }
            break;

            case rp_health:
                // We should not be here, as health state is called when initialising.
                Debug(1, "state: HEALTH");
                _initialised = false;
                break;

            case rp_unknown:
                Debug(1, "state: UNKNOWN");
                if (c == RPLIDAR_PREAMBLE) {
                    _rp_state = rp_responding;
                    goto STATE;
                    break;
                }
                _cnt++;
                if (_cnt>10) {
                    reset_rplidar();
                    _rp_state = rp_resetted;
                    _cnt=0;
                }
                break;

            default:
                Debug(1, "UNKNOWN LIDAR STATE");
                break;
        }
    }
}

void AP_Proximity_RPLidarA2::parse_response_descriptor()
{
    // check if descriptor packet is valid
    if (_descriptor[0] == RPLIDAR_PREAMBLE && _descriptor[1] == 0x5A) {

        if (_descriptor[2] == 0x05 && _descriptor[3] == 0x00 && _descriptor[4] == 0x00 && _descriptor[5] == 0x40 && _descriptor[6] == 0x81) {
            // payload is SCAN measurement data
            _payload_length = sizeof(payload.sensor_scan);
            static_assert(sizeof(payload.sensor_scan) == 5, "Unexpected payload.sensor_scan data structure size");
            _response_type = ResponseType_SCAN;
            Debug(2, "Measurement response detected");
            _last_distance_received_ms = AP_HAL::millis();
            _rp_state = rp_measurements;
        }
        if (_descriptor[2] == 0x03 && _descriptor[3] == 0x00 && _descriptor[4] == 0x00 && _descriptor[5] == 0x00 && _descriptor[6] == 0x06) {
            // payload is health data
            _payload_length = sizeof(payload.sensor_health);
            static_assert(sizeof(payload.sensor_health) == 3, "Unexpected payload.sensor_health data structure size");
            _response_type = ResponseType_Health;
            _last_distance_received_ms = AP_HAL::millis();
            _rp_state= rp_health;
        }
        return;
    }
    Debug(1, "Invalid response descriptor");
    _rp_state = rp_unknown;
}

void AP_Proximity_RPLidarA2::parse_response_data()
{
    switch (_response_type){
        case ResponseType_SCAN:
            Debug(2, "UART %02x %02x%02x %02x%02x", payload[0], payload[2], payload[1], payload[4], payload[3]); //show HEX values
            // check if valid SCAN packet: a valid packet starts with startbits which are complementary plus a checkbit in byte+1
            if ((payload.sensor_scan.startbit == !payload.sensor_scan.not_startbit) && payload.sensor_scan.checkbit) {
                const float angle_sign = (frontend.get_orientation(state.instance) == 1) ? -1.0f : 1.0f;
                const float angle_deg = wrap_360(payload.sensor_scan.angle_q6/64.0f * angle_sign + frontend.get_yaw_correction(state.instance));
                const float distance_m = (payload.sensor_scan.distance_q2/4000.0f);
#if RP_DEBUG_LEVEL >= 2
                const float quality = payload.sensor_scan.quality;
                Debug(2, "                                       D%02.2f A%03.1f Q%02d", distance_m, angle_deg, quality);
#endif
                _last_distance_received_ms = AP_HAL::millis();
                uint8_t sector;
                if (convert_angle_to_sector(angle_deg, sector)) {
                    if (distance_m > distance_min()) {
                        if (_last_sector == sector) {
                            if (_distance_m_last > distance_m) {
                                _distance_m_last = distance_m;
                                _angle_deg_last  = angle_deg;
                            }
                        } else {
                            // a new sector started, the previous one can be updated now
                            _angle[_last_sector] = _angle_deg_last;
                            _distance[_last_sector] = _distance_m_last;
                            _distance_valid[_last_sector] = true;
                            // update boundary used for avoidance
                            update_boundary_for_sector(_last_sector, true);
                            // initialize the new sector
                            _last_sector     = sector;
                            _distance_m_last = distance_m;
                            _angle_deg_last  = angle_deg;
                        }
                    } else {
                        _distance_valid[sector] = false;
                    }
                }
            } else {
                // not valid payload packet
                Debug(1, "Invalid Payload");
                _sync_error++;
            }
            break;

        case ResponseType_Health:
            // health issue if status is "2" ->HW error
            if (payload.sensor_health.status == 2) {
                Debug(1, "LIDAR Error");
                _rplidar_mech_error_cnt ++;
                _rplidarOk = false;

            } 
            if (payload.sensor_health.status < 2) {
                _rplidarOk = true;
            }
            break;

        default:
            // no valid payload packets recognized: return payload data=0
            Debug(1, "Unknown LIDAR packet");
            break;
    }
}

And also the corresponding changes to the header file.

[amilcarlucas]

Can you please raise a github PR with that ?

[andresrcdev]

Yes, but I am very bussy at work, so it won't be possible before weekend.
I think I will also need some help.

[Arduhawk]

What is the status on this? I am having troubles even running the pixhawk on a separate circuit and plugging power into the pixhawk after the lidar has spun up.

[acobo]

Hello all,
I'm having problems with a RPlidar A2M8 and Rover 4.1.5, half of the times the lidar is not detected and the "bad lidar health" message is shown. I guess it is due to this initialization problem.
Do anyone know the status of this proposed fix in the code?
I am reading posts suggesting that the lidar should be switched on before the pixhawk, other posts suggest the opposite solution, but in any case, it involves additional hardware to do that.
Thankyou in advance!
Adolfo.