2016-09-13 00:24:41 -03:00
|
|
|
#pragma once
|
|
|
|
|
2019-11-10 22:47:38 -04:00
|
|
|
#include "AP_RangeFinder.h"
|
|
|
|
#include "AP_RangeFinder_Backend_Serial.h"
|
2016-09-13 00:24:41 -03:00
|
|
|
|
2016-11-04 22:19:32 -03:00
|
|
|
// defines
|
2016-09-13 00:24:41 -03:00
|
|
|
#define LEDDARONE_DEFAULT_ADDRESS 0x01
|
2016-11-04 22:19:32 -03:00
|
|
|
#define LEDDARONE_MODOBUS_FUNCTION_CODE 0x04
|
|
|
|
#define LEDDARONE_MODOBUS_FUNCTION_REGISTER_ADDRESS 20
|
|
|
|
#define LEDDARONE_MODOBUS_FUNCTION_READ_NUMBER 10
|
|
|
|
|
|
|
|
#define LEDDARONE_SERIAL_PORT_MAX 250
|
|
|
|
#define LEDDARONE_READ_BUFFER_SIZE 25
|
|
|
|
|
|
|
|
#define LEDDARONE_DETECTIONS_MAX 3
|
|
|
|
#define LEDDARONE_DETECTION_DATA_NUMBER_INDEX 10
|
|
|
|
#define LEDDARONE_DETECTION_DATA_INDEX_OFFSET 11
|
|
|
|
#define LEDDARONE_DETECTION_DATA_OFFSET 4
|
2016-09-13 00:24:41 -03:00
|
|
|
|
2016-10-08 08:58:05 -03:00
|
|
|
// LeddarOne status
|
|
|
|
enum LeddarOne_Status {
|
2016-11-06 06:02:14 -04:00
|
|
|
LEDDARONE_STATE_OK = 0,
|
|
|
|
LEDDARONE_STATE_READING_BUFFER = 1,
|
|
|
|
LEDDARONE_STATE_ERR_BAD_CRC = -1,
|
|
|
|
LEDDARONE_STATE_ERR_NO_RESPONSES = -2,
|
|
|
|
LEDDARONE_STATE_ERR_BAD_RESPONSE = -3,
|
|
|
|
LEDDARONE_STATE_ERR_SHORT_RESPONSE = -4,
|
|
|
|
LEDDARONE_STATE_ERR_SERIAL_PORT = -5,
|
|
|
|
LEDDARONE_STATE_ERR_NUMBER_DETECTIONS = -6
|
2016-10-08 08:58:05 -03:00
|
|
|
};
|
2016-09-13 00:24:41 -03:00
|
|
|
|
2016-11-04 11:15:31 -03:00
|
|
|
// LeddarOne Modbus status
|
|
|
|
enum LeddarOne_ModbusStatus {
|
2016-11-06 06:02:14 -04:00
|
|
|
LEDDARONE_MODBUS_STATE_INIT = 0,
|
|
|
|
LEDDARONE_MODBUS_STATE_PRE_SEND_REQUEST,
|
|
|
|
LEDDARONE_MODBUS_STATE_SENT_REQUEST,
|
|
|
|
LEDDARONE_MODBUS_STATE_AVAILABLE
|
2016-11-04 11:15:31 -03:00
|
|
|
};
|
|
|
|
|
2019-11-01 03:28:22 -03:00
|
|
|
class AP_RangeFinder_LeddarOne : public AP_RangeFinder_Backend_Serial
|
2016-09-13 00:24:41 -03:00
|
|
|
{
|
|
|
|
|
|
|
|
public:
|
|
|
|
|
2019-11-01 03:28:22 -03:00
|
|
|
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
|
2016-09-13 00:24:41 -03:00
|
|
|
|
2017-08-08 04:32:53 -03:00
|
|
|
protected:
|
|
|
|
|
|
|
|
virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
|
|
|
|
return MAV_DISTANCE_SENSOR_LASER;
|
|
|
|
}
|
|
|
|
|
2016-09-13 00:24:41 -03:00
|
|
|
private:
|
|
|
|
// get a reading
|
2019-11-01 07:59:50 -03:00
|
|
|
bool get_reading(uint16_t &reading_cm) override;
|
2016-09-13 00:24:41 -03:00
|
|
|
|
|
|
|
// CRC16
|
|
|
|
bool CRC16(uint8_t *aBuffer, uint8_t aLength, bool aCheck);
|
|
|
|
|
|
|
|
// parse a response message from ModBus
|
2016-10-10 05:14:39 -03:00
|
|
|
LeddarOne_Status parse_response(uint8_t &number_detections);
|
2016-09-13 00:24:41 -03:00
|
|
|
|
2016-11-04 11:15:31 -03:00
|
|
|
uint32_t last_sending_request_ms;
|
2016-11-22 10:27:36 -04:00
|
|
|
uint32_t last_available_ms;
|
2016-09-13 00:24:41 -03:00
|
|
|
|
2016-09-19 06:45:56 -03:00
|
|
|
uint16_t detections[LEDDARONE_DETECTIONS_MAX];
|
|
|
|
uint32_t sum_distance;
|
2016-11-04 11:15:31 -03:00
|
|
|
|
2016-11-06 06:02:14 -04:00
|
|
|
LeddarOne_ModbusStatus modbus_status = LEDDARONE_MODBUS_STATE_INIT;
|
2016-11-04 22:19:32 -03:00
|
|
|
uint8_t read_buffer[LEDDARONE_READ_BUFFER_SIZE];
|
2016-11-04 11:15:31 -03:00
|
|
|
uint32_t read_len;
|
2016-11-21 07:45:34 -04:00
|
|
|
|
2016-11-22 17:40:37 -04:00
|
|
|
// Modbus send request buffer
|
|
|
|
// read input register (function code 0x04)
|
|
|
|
const uint8_t send_request_buffer[8] = {
|
|
|
|
LEDDARONE_DEFAULT_ADDRESS,
|
|
|
|
LEDDARONE_MODOBUS_FUNCTION_CODE,
|
|
|
|
0,
|
|
|
|
LEDDARONE_MODOBUS_FUNCTION_REGISTER_ADDRESS, // 20: Address of first register to read
|
|
|
|
0,
|
|
|
|
LEDDARONE_MODOBUS_FUNCTION_READ_NUMBER, // 10: The number of consecutive registers to read
|
|
|
|
0x30, // CRC Lo
|
|
|
|
0x09 // CRC Hi
|
|
|
|
};
|
2016-09-13 00:24:41 -03:00
|
|
|
};
|