Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: Lightware SF45B driver

This commit is contained in:
Randy Mackay 2020-09-29 22:30:24 +09:00 committed by Andrew Tridgell
parent f3b193d6f8
commit e081d83185
6 changed files with 505 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_Proximity/AP_Proximity.cpp
View File

@ -21,6 +21,7 @@
#include "AP_Proximity_RangeFinder.h"
#include "AP_Proximity_MAV.h"
#include "AP_Proximity_LightWareSF40C.h"
#include "AP_Proximity_LightWareSF45B.h"
#include "AP_Proximity_SITL.h"
#include "AP_Proximity_MorseSITL.h"
#include "AP_Proximity_AirSimSITL.h"
@ -34,7 +35,7 @@ const AP_Param::GroupInfo AP_Proximity::var_info[] = {
// @Param: _TYPE
// @DisplayName: Proximity type
// @Description: What type of proximity sensor is connected
// @Values: 0:None,7:LightwareSF40c,1:LightWareSF40C-legacy,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,10:SITL,11:MorseSITL,12:AirSimSITL
// @Values: 0:None,7:LightwareSF40c,1:LightWareSF40C-legacy,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,8:LightwareSF45B,10:SITL,11:MorseSITL,12:AirSimSITL
// @RebootRequired: True
// @User: Standard
AP_GROUPINFO("_TYPE", 1, AP_Proximity, _type[0], 0),
@ -154,7 +155,7 @@ const AP_Param::GroupInfo AP_Proximity::var_info[] = {
// @Param: 2_TYPE
// @DisplayName: Second Proximity type
// @Description: What type of proximity sensor is connected
// @Values: 0:None,7:LightwareSF40c,1:LightWareSF40C-legacy,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,10:SITL,11:MorseSITL,12:AirSimSITL
// @Values: 0:None,7:LightwareSF40c,1:LightWareSF40C-legacy,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,8:LightwareSF45B,10:SITL,11:MorseSITL,12:AirSimSITL
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("2_TYPE", 16, AP_Proximity, _type[1], 0),
@ -327,6 +328,14 @@ void AP_Proximity::detect_instance(uint8_t instance)
}
break;
case Type::SF45B:
if (AP_Proximity_LightWareSF45B::detect()) {
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_LightWareSF45B(*this, state[instance]);
return;
}
break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case Type::SITL:
state[instance].instance = instance;

1
libraries/AP_Proximity/AP_Proximity.h
View File

@ -47,6 +47,7 @@ public:
RPLidarA2 = 5,
TRTOWEREVO = 6,
SF40C = 7,
SF45B = 8,
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
SITL = 10,
MorseSITL = 11,

201
libraries/AP_Proximity/AP_Proximity_LightWareSF45B.cpp Normal file
View File

@ -0,0 +1,201 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
The Lightware SF45B serial interface is described on this wiki page
http://support.lightware.co.za/sf45/#/commands
*/
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/utility/sparse-endian.h>
#include "AP_Proximity_LightWareSF45B.h"
extern const AP_HAL::HAL& hal;
static const uint32_t PROXIMITY_SF45B_TIMEOUT_MS = 200;
static const uint32_t PROXIMITY_SF45B_REINIT_INTERVAL_MS = 5000; // re-initialise sensor after this many milliseconds
static const float PROXIMITY_SF45B_COMBINE_READINGS_DEG = 5.0f; // combine readings from within this many degrees to improve efficiency
static const uint32_t PROXIMITY_SF45B_STREAM_DISTANCE_DATA_CM = 5;
static const uint8_t PROXIMITY_SF45B_DESIRED_UPDATE_RATE = 8; // 1:48hz, 2:55hz, 3:64hz, 4:77hz, 5:97hz, 6:129hz, 7:194hz, 8:388hz
static const uint32_t PROXIMITY_SF45B_DESIRED_FIELDS = ((uint32_t)1 << 0 | (uint32_t)1 << 8); // first return (unfiltered), yaw angle
static const uint16_t PROXIMITY_SF45B_DESIRED_FIELD_COUNT = 2; // DISTANCE_DATA_CM message should contain two fields
// update the state of the sensor
void AP_Proximity_LightWareSF45B::update(void)
{
if (_uart == nullptr) {
return;
}
// initialise sensor if necessary
initialise();
// process incoming messages
process_replies();
// check for timeout and set health status
if ((_last_distance_received_ms == 0) || ((AP_HAL::millis() - _last_distance_received_ms) > PROXIMITY_SF45B_TIMEOUT_MS)) {
set_status(AP_Proximity::Status::NoData);
} else {
set_status(AP_Proximity::Status::Good);
}
}
// initialise sensor
void AP_Proximity_LightWareSF45B::initialise()
{
// check sensor is configured correctly
_init_complete = (_sensor_state.stream_data_type == PROXIMITY_SF45B_STREAM_DISTANCE_DATA_CM) &&
(_sensor_state.update_rate == PROXIMITY_SF45B_DESIRED_UPDATE_RATE) &&
(_sensor_state.streaming_fields == PROXIMITY_SF45B_DESIRED_FIELDS);
// exit if initialisation requests have been sent within the last few seconds
uint32_t now_ms = AP_HAL::millis();
if ((now_ms - _last_init_ms) < PROXIMITY_SF45B_REINIT_INTERVAL_MS) {
return;
}
_last_init_ms = now_ms;
// request stream rate and contents
request_stream_start();
// initialise boundary
init_boundary();
}
// request start of streaming of distances
void AP_Proximity_LightWareSF45B::request_stream_start()
{
// request output rate
send_message((uint8_t)MessageID::UPDATE_RATE, true, &PROXIMITY_SF45B_DESIRED_UPDATE_RATE, sizeof(PROXIMITY_SF45B_DESIRED_UPDATE_RATE));
// request first return (unfiltered), and yaw angle
send_message((uint8_t)MessageID::DISTANCE_OUTPUT, true, (const uint8_t*)&PROXIMITY_SF45B_DESIRED_FIELDS, sizeof(PROXIMITY_SF45B_DESIRED_FIELDS));
// request start streaming of DISTANCE_DATA_CM messages
send_message((uint8_t)MessageID::STREAM, true, (const uint8_t*)&PROXIMITY_SF45B_STREAM_DISTANCE_DATA_CM, sizeof(PROXIMITY_SF45B_STREAM_DISTANCE_DATA_CM));
}
// check for replies from sensor
void AP_Proximity_LightWareSF45B::process_replies()
{
if (_uart == nullptr) {
return;
}
// process up to 1K of characters per iteration
uint32_t nbytes = MIN(_uart->available(), 1024U);
while (nbytes-- > 0) {
const int16_t r = _uart->read();
if ((r < 0) || (r > 0xFF)) {
continue;
}
if (parse_byte((uint8_t)r)) {
process_message();
}
}
}
// process the latest message held in the _msg structure
void AP_Proximity_LightWareSF45B::process_message()
{
// process payload
switch ((MessageID)_msg.msgid) {
case MessageID::DISTANCE_OUTPUT:
if (_payload_recv == sizeof(uint32_t)) {
_sensor_state.streaming_fields = UINT32_VALUE(_msg.payload[3], _msg.payload[2], _msg.payload[1], _msg.payload[0]);
}
break;
case MessageID::STREAM:
if (_payload_recv == sizeof(uint32_t)) {
_sensor_state.stream_data_type = UINT32_VALUE(_msg.payload[3], _msg.payload[2], _msg.payload[1], _msg.payload[0]);
}
break;
case MessageID::UPDATE_RATE:
if (_payload_recv == 1) {
_sensor_state.update_rate = _msg.payload[0];
}
break;
case MessageID::DISTANCE_DATA_CM: {
// ignore distance messages until initialisation is complete
if (!_init_complete || (_payload_recv != (PROXIMITY_SF45B_DESIRED_FIELD_COUNT * 2))) {
break;
}
_last_distance_received_ms = AP_HAL::millis();
const float distance_m = _distance_filt.apply((int16_t)UINT16_VALUE(_msg.payload[1], _msg.payload[0])) * 0.01f;
const float angle_deg = correct_angle_for_orientation((int16_t)UINT16_VALUE(_msg.payload[3], _msg.payload[2]) * 0.01f);
// if distance is from a new sector then update distance, angle and boundary for previous sector
const uint8_t sector = convert_angle_to_sector(angle_deg);
if (sector != _sector) {
if (_sector != UINT8_MAX) {
_angle[_sector] = _sector_angle;
_distance[_sector] = _sector_distance;
_distance_valid[_sector] = _sector_distance_valid;
update_boundary_for_sector(_sector, false);
}
// init for new sector
_sector = sector;
_sector_angle = 0;
_sector_distance = INT16_MAX;
_sector_distance_valid = false;
}
// if distance is from a new minisector then update obstacle database using angle and distance from previous minisector
const uint8_t minisector = convert_angle_to_minisector(angle_deg);
if (minisector != _minisector) {
if ((_minisector != UINT8_MAX) && _minisector_distance_valid) {
database_push(_minisector_angle, _minisector_distance);
}
// init mini sector
_minisector = minisector;
_minisector_angle = 0;
_minisector_distance = INT16_MAX;
_minisector_distance_valid = false;
}
// check reading is valid
if (!ignore_reading(angle_deg) && (distance_m >= distance_min()) && (distance_m <= distance_max())) {
// update shortest distance for this sector
if (distance_m < _sector_distance) {
_sector_angle = angle_deg;
_sector_distance = distance_m;
_sector_distance_valid = true;
}
// update shortest distance for this mini sector
if (distance_m < _minisector_distance) {
_minisector_angle = angle_deg;
_minisector_distance = distance_m;
_minisector_distance_valid = true;
}
}
break;
}
default:
// ignore unsupported messages
break;
}
}
// convert an angle (in degrees) to a mini sector number
uint8_t AP_Proximity_LightWareSF45B::convert_angle_to_minisector(float angle_deg) const
{
return wrap_360(angle_deg + (PROXIMITY_SF45B_COMBINE_READINGS_DEG * 0.5f)) / PROXIMITY_SF45B_COMBINE_READINGS_DEG;
}

102
libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h Normal file
View File

@ -0,0 +1,102 @@
#pragma once
#include <Filter/Filter.h>
#include "AP_Proximity.h"
#include "AP_Proximity_LightWareSerial.h"
class AP_Proximity_LightWareSF45B : public AP_Proximity_LightWareSerial
{
public:
// constructor
AP_Proximity_LightWareSF45B(AP_Proximity &_frontend,
AP_Proximity::Proximity_State &_state) :
AP_Proximity_LightWareSerial(_frontend, _state) {}
uint16_t rxspace() const override {
return 1280;
};
// update state
void update(void) override;
// get maximum and minimum distances (in meters) of sensor
float distance_max() const override { return 50.0f; }
float distance_min() const override { return 0.20f; }
private:
// message ids
enum class MessageID : uint8_t {
PRODUCT_NAME = 0,
HARDWARE_VERSION = 1,
FIRMWARE_VERSION = 2,
SERIAL_NUMBER = 3,
TEXT_MESSAGE = 7,
USER_DATA = 9,
TOKEN = 10,
SAVE_PARAMETERS = 12,
RESET = 14,
STAGE_FIRMWARE = 16,
COMMIT_FIRMWARE = 17,
DISTANCE_OUTPUT = 27,
STREAM = 30,
DISTANCE_DATA_CM = 44,
DISTANCE_DATA_MM = 45,
LASER_FIRING = 50,
TEMPERATURE = 57,
UPDATE_RATE = 66,
NOISE = 74,
ZERO_OFFSET = 75,
LOST_SIGNAL_COUNTER = 76,
BAUD_RATE = 79,
I2C_ADDRESS = 80,
STEPPER_STATUS = 93,
SCAN_ON_STARTUP = 94,
SCAN_ENABLE = 96,
SCAN_POSITION = 97,
SCAN_LOW_ANGLE = 98,
SCAN_HIGH_ANGLE = 99
};
// initialise sensor
void initialise();
// request start of streaming of distances
void request_stream_start();
// check and process replies from sensor
void process_replies();
// process the latest message held in the msg structure
void process_message();
// convert an angle (in degrees) to a mini sector number
uint8_t convert_angle_to_minisector(float angle_deg) const;
// internal variables
uint32_t _last_init_ms; // system time of last re-initialisation
uint32_t _last_distance_received_ms; // system time of last distance measurement received from sensor
bool _init_complete; // true once sensor initialisation is complete
ModeFilterInt16_Size5 _distance_filt{2};// mode filter to reduce glitches
// sector (45 degrees) angles and distances (used to build mini fence for simple avoidance)
uint8_t _sector = UINT8_MAX; // sector number (from 0 to 7) of most recently received distance
float _sector_distance; // shortest distance (in meters) in sector
float _sector_angle; // angle (in degrees) of shortest distance in sector
bool _sector_distance_valid; // true if sector has at least one valid distance
// mini sector (5 degrees) angles and distances (used to populate obstacle database for path planning)
uint8_t _minisector = UINT8_MAX; // mini sector number (from 0 to 71) of most recently received distance
float _minisector_distance; // shortest distance (in meters) in mini sector
float _minisector_angle; // angle (in degrees) of shortest distance in mini sector
bool _minisector_distance_valid; // true if mini sector has at least one valid distance
// state of sensor
struct {
uint8_t update_rate; // sensor reported update rate enum from UPDATE_RATE message
uint32_t streaming_fields; // sensor reported bitmask of fields sent in DISTANCE_DATA_CM message
uint32_t stream_data_type; // sensor reported stream value. 5 if DISTANCE_DATA_CM messages are being streamed
} _sensor_state;
};

140
libraries/AP_Proximity/AP_Proximity_LightWareSerial.cpp Normal file
View File

@ -0,0 +1,140 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/utility/sparse-endian.h>
#include <AP_Math/crc.h>
#include "AP_Proximity_LightWareSerial.h"
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;
#define PROXIMITY_LIGHTWARE_HEADER 0xAA
// send message to sensor
void AP_Proximity_LightWareSerial::send_message(uint8_t msgid, bool write, const uint8_t *payload, uint16_t payload_len)
{
if ((_uart == nullptr) || (payload_len > PROXIMITY_LIGHTWARE_PAYLOAD_LEN_MAX)) {
return;
}
// check for sufficient space in outgoing buffer
if (_uart->txspace() < payload_len + 6U) {
return;
}
// write header
_uart->write((uint8_t)PROXIMITY_LIGHTWARE_HEADER);
uint16_t crc = crc_xmodem_update(0, PROXIMITY_LIGHTWARE_HEADER);
// write flags including payload length
const uint16_t flags = ((payload_len+1) << 6) | (write ? 0x01 : 0);
_uart->write(LOWBYTE(flags));
crc = crc_xmodem_update(crc, LOWBYTE(flags));
_uart->write(HIGHBYTE(flags));
crc = crc_xmodem_update(crc, HIGHBYTE(flags));
// msgid
_uart->write(msgid);
crc = crc_xmodem_update(crc, msgid);
// payload
if ((payload_len > 0) && (payload != nullptr)) {
for (uint16_t i = 0; i < payload_len; i++) {
_uart->write(payload[i]);
crc = crc_xmodem_update(crc, payload[i]);
}
}
// checksum
_uart->write(LOWBYTE(crc));
_uart->write(HIGHBYTE(crc));
}
// process one byte received on serial port
// returns true if a complete message has been received
// state is stored in _msg structure
bool AP_Proximity_LightWareSerial::parse_byte(uint8_t b)
{
// check that payload buffer is large enough
static_assert(ARRAY_SIZE(_msg.payload) == PROXIMITY_LIGHTWARE_PAYLOAD_LEN_MAX, "AP_Proximity_LightWareSerial: check _msg.payload array size");
// process byte depending upon current state
switch (_parse_state) {
case ParseState::HEADER:
if (b == PROXIMITY_LIGHTWARE_HEADER) {
_crc_expected = crc_xmodem_update(0, b);
_parse_state = ParseState::FLAGS_L;
}
break;
case ParseState::FLAGS_L:
_msg.flags_low = b;
_crc_expected = crc_xmodem_update(_crc_expected, b);
_parse_state = ParseState::FLAGS_H;
break;
case ParseState::FLAGS_H:
_msg.flags_high = b;
_crc_expected = crc_xmodem_update(_crc_expected, b);
_msg.payload_len = UINT16_VALUE(_msg.flags_high, _msg.flags_low) >> 6;
if ((_msg.payload_len == 0) || (_msg.payload_len > PROXIMITY_LIGHTWARE_PAYLOAD_LEN_MAX)) {
// invalid payload length, abandon message
_parse_state = ParseState::HEADER;
} else {
_parse_state = ParseState::MSG_ID;
}
break;
case ParseState::MSG_ID:
_msg.msgid = b;
_crc_expected = crc_xmodem_update(_crc_expected, b);
if (_msg.payload_len > 1) {
_parse_state = ParseState::PAYLOAD;
} else {
_parse_state = ParseState::CRC_L;
}
_payload_recv = 0;
break;
case ParseState::PAYLOAD:
if (_payload_recv < (_msg.payload_len - 1)) {
_msg.payload[_payload_recv] = b;
_payload_recv++;
_crc_expected = crc_xmodem_update(_crc_expected, b);
}
if (_payload_recv >= (_msg.payload_len - 1)) {
_parse_state = ParseState::CRC_L;
}
break;
case ParseState::CRC_L:
_msg.crc_low = b;
_parse_state = ParseState::CRC_H;
break;
case ParseState::CRC_H:
_parse_state = ParseState::HEADER;
_msg.crc_high = b;
if (_crc_expected == UINT16_VALUE(_msg.crc_high, _msg.crc_low)) {
return true;
}
break;
}
return false;
}

50
libraries/AP_Proximity/AP_Proximity_LightWareSerial.h Normal file
View File

@ -0,0 +1,50 @@
#pragma once
#include "AP_Proximity.h"
#include "AP_Proximity_Backend_Serial.h"
#define PROXIMITY_LIGHTWARE_PAYLOAD_LEN_MAX 256 // maximum payload size we can accept (in some configurations sensor may send as large as 1023)
class AP_Proximity_LightWareSerial : public AP_Proximity_Backend_Serial
{
public:
// constructor
using AP_Proximity_Backend_Serial::AP_Proximity_Backend_Serial;
protected:
// initialise sensor
void initialise();
// send message to sensor
void send_message(uint8_t msgid, bool write, const uint8_t *payload, uint16_t payload_len);
// process one byte received on serial port
// returns true if a complete message has been received
// state is stored in _msg structure
bool parse_byte(uint8_t b);
enum class ParseState {
HEADER = 0,
FLAGS_L,
FLAGS_H,
MSG_ID,
PAYLOAD,
CRC_L,
CRC_H
} _parse_state; // state of incoming message processing
uint16_t _payload_recv; // number of message's payload bytes received so far
uint16_t _crc_expected; // latest message's expected crc
// structure holding latest message contents
struct {
uint8_t flags_low; // flags low byte
uint8_t flags_high; // flags high byte
uint16_t payload_len; // latest message payload length (1+ bytes in payload)
uint8_t payload[PROXIMITY_LIGHTWARE_PAYLOAD_LEN_MAX]; // payload
uint8_t msgid; // latest message's message id
uint8_t crc_low; // crc low byte
uint8_t crc_high; // crc high byte
} _msg;
};