AP_Proximity: Lightware SF45B driver

libraries/AP_Proximity/AP_Proximity.cpp

@@ -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;

libraries/AP_Proximity/AP_Proximity.h

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

libraries/AP_Proximity/AP_Proximity_LightWareSF45B.cpp

@@ -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;
+}

libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h

@@ -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;
+
+};

libraries/AP_Proximity/AP_Proximity_LightWareSerial.cpp

@@ -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;
+}

libraries/AP_Proximity/AP_Proximity_LightWareSerial.h

@@ -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;
+};