diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index c1f5396d1c..9296fd21a4 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/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 } } diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index ac64e4c685..b9268553ad 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -60,6 +60,7 @@ public: SITL = 10, AirSimSITL = 12, #endif + CYGBOT_D1 = 13, }; enum class Status { diff --git a/libraries/AP_Proximity/AP_Proximity_Cygbot_D1.cpp b/libraries/AP_Proximity/AP_Proximity_Cygbot_D1.cpp new file mode 100644 index 0000000000..e4e0a376a8 --- /dev/null +++ b/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 diff --git a/libraries/AP_Proximity/AP_Proximity_Cygbot_D1.h b/libraries/AP_Proximity/AP_Proximity_Cygbot_D1.h new file mode 100644 index 0000000000..3e844732f9 --- /dev/null +++ b/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