AP_Proximity: Add Cygbot D1

libraries/AP_Proximity/AP_Proximity.cpp

@@ -26,6 +26,7 @@
 #include "AP_Proximity_LightWareSF45B.h"
 #include "AP_Proximity_SITL.h"
 #include "AP_Proximity_AirSimSITL.h"
+#include "AP_Proximity_Cygbot_D1.h"
 
 extern const AP_HAL::HAL &hal;
 
@@ -36,7 +37,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,8:LightwareSF45B,10:SITL,12:AirSimSITL
+    // @Values: 0:None,7:LightwareSF40c,1:LightWareSF40C-legacy,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,8:LightwareSF45B,10:SITL,12:AirSimSITL,13:CygbotD1
     // @RebootRequired: True
     // @User: Standard
     AP_GROUPINFO_FLAGS("_TYPE",   1, AP_Proximity, _type[0], 0, AP_PARAM_FLAG_ENABLE),
@@ -335,6 +336,16 @@ void AP_Proximity::detect_instance(uint8_t instance)
         }
         break;
 
+    case Type::CYGBOT_D1:
+#if AP_PROXIMITY_CYGBOT_ENABLED
+    if (AP_Proximity_Cygbot_D1::detect()) {
+        state[instance].instance = instance;
+        drivers[instance] = new AP_Proximity_Cygbot_D1(*this, state[instance]);
+        return;
+    }
+# endif
+    break;
+
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     case Type::SITL:
         state[instance].instance = instance;
@@ -345,6 +356,7 @@ void AP_Proximity::detect_instance(uint8_t instance)
         state[instance].instance = instance;
         drivers[instance] = new AP_Proximity_AirSimSITL(*this, state[instance]);
         return;
+
 #endif
     }
 }

libraries/AP_Proximity/AP_Proximity.h

@@ -60,6 +60,7 @@ public:
         SITL    = 10,
         AirSimSITL = 12,
 #endif
+        CYGBOT_D1 = 13,
     };
 
     enum class Status {

libraries/AP_Proximity/AP_Proximity_Cygbot_D1.cpp

@@ -0,0 +1,185 @@
+#include "AP_Proximity_Cygbot_D1.h"
+
+#if HAL_PROXIMITY_ENABLED && AP_PROXIMITY_CYGBOT_ENABLED
+
+// update the state of the sensor
+void AP_Proximity_Cygbot_D1::update()
+{
+    if (!_initialized) {
+        send_sensor_start();
+        _temp_boundary.reset();
+        _initialized = true;
+        _last_init_ms = AP_HAL::millis();
+    }
+
+    if ((AP_HAL::millis() - _last_init_ms) < CYGBOT_INIT_TIMEOUT_MS) {
+        // just initialized
+        set_status(AP_Proximity::Status::NoData);
+        return;
+    }
+
+    // read data
+    read_sensor_data();
+
+    if (AP_HAL::millis() - _last_distance_received_ms < CYGBOT_TIMEOUT_MS) {
+        set_status(AP_Proximity::Status::Good);
+    } else {
+        // long time since we received any valid sensor data
+        // try sending the sensor the "send data" message
+        _initialized = false;
+        set_status(AP_Proximity::Status::NoData);
+    }
+}
+
+// send message to the sensor to start streaming 2-D data
+void AP_Proximity_Cygbot_D1::send_sensor_start()
+{
+    // this message corresponds to "start message"
+    const uint8_t packet_start_2d[8] = { CYGBOT_PACKET_HEADER_0, CYGBOT_PACKET_HEADER_1, CYGBOT_PACKET_HEADER_2, 0x02, 0x00, 0x01, 0x00, 0x03 };
+    _uart->write(packet_start_2d, 8);
+}
+
+void AP_Proximity_Cygbot_D1::read_sensor_data()
+{
+    uint32_t nbytes = _uart->available();
+    while (nbytes-- > 0) {
+        uint8_t byte = _uart->read();
+        if (!parse_byte(byte)) {
+            // reset
+            reset();
+        }
+    }
+}
+
+// parse one byte from the sensor. Return false on error.
+// Message format is: header1 + header2 + header3 + length1 + length2 + PayloadCommand + checksum
+bool AP_Proximity_Cygbot_D1::parse_byte(uint8_t data)
+{
+    switch (_parse_state) {
+    case Header1:
+        if (data == CYGBOT_PACKET_HEADER_0) {
+            _parse_state = Header2;
+            return true;
+        }
+
+        return false;
+
+    case Header2:
+        if (data == CYGBOT_PACKET_HEADER_1) {
+            _parse_state = Header3;
+            return true;
+        }
+        return false;
+
+    case Header3:
+        if (data == CYGBOT_PACKET_HEADER_2) {
+            _parse_state = Length1;
+            return true;
+        }
+        return false;
+
+    case Length1:
+        _msg.payload_len_flags_low = data;
+        _parse_state = Length2;
+        return true;
+
+    case Length2:
+        _msg.payload_len_flags_high = data;
+        _msg.payload_len = UINT16_VALUE(data, _msg.payload_len_flags_low);
+        if (_msg.payload_len > CYGBOT_MAX_MSG_SIZE) {
+            return false;
+        }
+        _parse_state = Payload_Header;
+        return true;
+
+    case Payload_Header:
+        if (data == CYGBOT_PAYLOAD_HEADER) {
+            _parse_state = Payload_Data;
+            _msg.payload_counter = 0;
+            _msg.payload[_msg.payload_counter] = data;
+            _msg.payload_counter ++;
+            return true;
+        }
+        return false;
+
+    case Payload_Data:
+        if (_msg.payload_counter < (_msg.payload_len - 1)) {
+            _msg.payload[_msg.payload_counter] = data;
+            _msg.payload_counter++;
+            return true;
+        }
+        _parse_state = CheckSum;
+        return true;
+
+    case CheckSum: {
+        // To-DO: FIX CHECKSUM below. It does not pass correctly
+        // const uint8_t checksum_num = calc_checksum(_msg.payload, _msg.payload_len);
+        // if (data != checksum_num) {
+        // 	return false;
+        // }
+        // checksum is valid, parse payload
+        _last_distance_received_ms = AP_HAL::millis();
+        parse_payload();
+        _temp_boundary.update_3D_boundary(boundary);
+        reset();
+        return true;
+    }
+    break;
+
+    default:
+        return false;
+    }
+
+    return false;
+}
+
+// parse payload, to pick out distances, and feed them to the correct faces
+void AP_Proximity_Cygbot_D1::parse_payload()
+{
+    // current horizontal angle in the payload
+    float sampled_angle = CYGBOT_2D_START_ANGLE;
+
+    for (uint16_t i = 1; i < _msg.payload_len - 1; i += 2) {
+        const uint16_t distance_mm = UINT16_VALUE(_msg.payload[i], _msg.payload[i+1]);
+        float distance_m = distance_mm * 0.001f;
+        if (distance_m > distance_min() && distance_m < distance_max()) {
+            if (ignore_reading(sampled_angle, distance_m)) {
+                // ignore this angle
+                sampled_angle += CYGBOT_2D_ANGLE_STEP;
+                continue;
+            }
+            // convert angle to face
+            const AP_Proximity_Boundary_3D::Face face = boundary.get_face(sampled_angle);
+            // push face to temp boundary
+            _temp_boundary.add_distance(face, sampled_angle, distance_m);
+            // push to OA_DB
+            database_push(sampled_angle, distance_m);
+        }
+        // increment sampled angle
+        sampled_angle += CYGBOT_2D_ANGLE_STEP;
+    }
+}
+
+// Checksum
+uint8_t AP_Proximity_Cygbot_D1::calc_checksum(uint8_t *buff, int buffSize)
+{
+    uint8_t check_sum_num = 0x00;
+    check_sum_num ^= _msg.payload_len_flags_low;
+    check_sum_num ^= _msg.payload_len_flags_high;
+
+    for (uint16_t i = 0; i < buffSize - 1; i++) {
+        check_sum_num ^= buff[i];
+    }
+    return check_sum_num;
+}
+
+// reset all variables and flags
+void AP_Proximity_Cygbot_D1::reset()
+{
+    _parse_state = Header1;
+    _msg.payload_counter = 0;
+    _msg.payload_len = 0;
+    _temp_boundary.reset();
+}
+
+#endif // HAL_PROXIMITY_ENABLED

libraries/AP_Proximity/AP_Proximity_Cygbot_D1.h

@@ -0,0 +1,87 @@
+#pragma once
+
+#include "AP_Proximity.h"
+
+#ifndef AP_PROXIMITY_CYGBOT_ENABLED
+#define AP_PROXIMITY_CYGBOT_ENABLED HAL_PROXIMITY_ENABLED
+#endif
+
+#if (HAL_PROXIMITY_ENABLED && AP_PROXIMITY_CYGBOT_ENABLED)
+#include "AP_Proximity_Backend_Serial.h"
+
+#define CYGBOT_MAX_MSG_SIZE            350
+#define CYGBOT_PACKET_HEADER_0         0x5A
+#define CYGBOT_PACKET_HEADER_1         0x77
+#define CYGBOT_PACKET_HEADER_2         0xFF
+#define CYGBOT_PAYLOAD_HEADER 		   0x01
+#define CYGBOT_2D_START_ANGLE 		   -60.0f  // Starting 2-D horizontal angle of distances received in payload
+#define CYGBOT_2D_ANGLE_STEP		   0.75f   // Angle step size of each distance received. Starts from CYGBOT_2D_START_ANGLE
+
+#define CYGBOT_TIMEOUT_MS              500    // Driver will report "unhealthy" if valid sensor readings not received within this many ms
+#define CYGBOT_INIT_TIMEOUT_MS         1000   // Timeout this many ms after init
+#define CYGBOT_MAX_RANGE_M             7.0f   // max range of the sensor in meters
+#define CYGBOT_MIN_RANGE_M             0.2f   // min range of the sensor in meters
+
+class AP_Proximity_Cygbot_D1 : public AP_Proximity_Backend_Serial
+{
+
+public:
+
+    using AP_Proximity_Backend_Serial::AP_Proximity_Backend_Serial;
+
+    // update the state of the sensor
+    void update(void) override;
+
+    // get maximum and minimum distances (in meters) of sensor
+    float distance_max() const override { return CYGBOT_MAX_RANGE_M; }
+    float distance_min() const override { return CYGBOT_MIN_RANGE_M; }
+
+private:
+
+    // send message to the sensor to start streaming 2-D data
+    void send_sensor_start();
+
+    // read bytes from the sensor
+    void read_sensor_data();
+
+    // parse one byte from the sensor. Return false on error.
+    bool parse_byte(uint8_t data);
+
+    // parse payload, to pick out distances, and feed them to the correct faces
+    void parse_payload();
+
+    // Checksum
+    uint8_t calc_checksum(uint8_t *buff, int buffSize);
+
+    // reset all variables and flags
+    void reset();
+
+    // expected bytes from the sensor
+    enum PacketList {
+        Header1 = 0,
+        Header2,
+        Header3,
+        Length1,
+        Length2,
+        Payload_Header,
+        Payload_Data,
+        CheckSum
+    } _parse_state;
+
+    struct {
+        uint8_t payload_len_flags_low;          // low byte for payload size
+        uint8_t payload_len_flags_high;         // high byte for payload size
+        uint16_t payload_len;                   // latest message expected payload length
+        uint16_t payload_counter;              // counter of the number of payload bytes received
+        uint8_t payload[CYGBOT_MAX_MSG_SIZE];   // payload
+    } _msg;
+
+    bool _initialized;
+    uint32_t _last_init_ms;                 // system time of last sensor init
+    uint32_t _last_distance_received_ms;    // system time of last distance measurement received from sensor
+
+    AP_Proximity_Temp_Boundary _temp_boundary; // temporary boundary to store incoming payload
+
+};
+
+#endif // HAL_PROXIMITY_ENABLED