zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Proximity: Lightware SF45B driver

zr-v5.1
Randy Mackay 4 years ago committed by Andrew Tridgell
parent
f3b193d6f8
commit
e081d83185
6 changed files with 505 additions and 2 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 13
      libraries/AP_Proximity/AP_Proximity.cpp
  2. 1
      libraries/AP_Proximity/AP_Proximity.h
  3. 201
      libraries/AP_Proximity/AP_Proximity_LightWareSF45B.cpp
  4. 102
      libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h
  5. 140
      libraries/AP_Proximity/AP_Proximity_LightWareSerial.cpp
  6. 50
      libraries/AP_Proximity/AP_Proximity_LightWareSerial.h

13
libraries/AP_Proximity/AP_Proximity.cpp
Unescape View File

@ -21,6 +21,7 @@ @@ -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[] = { @@ -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[] = { @@ -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) @@ -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
Unescape View File

@ -47,6 +47,7 @@ public: @@ -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
Unescape View File

@ -0,0 +1,201 @@ @@ -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
Unescape View File

@ -0,0 +1,102 @@ @@ -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
Unescape View File

@ -0,0 +1,140 @@ @@ -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
Unescape View File

@ -0,0 +1,50 @@ @@ -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;
};
Write Preview
Loading…
Cancel
Save