Open rmackay9 opened 4 years ago
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.
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.
Can you please raise a github PR with that ?
Yes, but I am very bussy at work, so it won't be possible before weekend. I think I will also need some help.
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.
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.
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