diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index fbd8c7f8f1..b9dcf1f51a 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -15,6 +15,7 @@ #include "AP_Proximity.h" #include "AP_Proximity_LightWareSF40C.h" +#include "AP_Proximity_TeraRangerTower.h" #include "AP_Proximity_MAV.h" #include "AP_Proximity_SITL.h" @@ -27,7 +28,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,1:LightWareSF40C,2:MAVLink + // @Values: 0:None,1:LightWareSF40C,2:MAVLink,3:TeraRangerTower // @User: Standard AP_GROUPINFO("_TYPE", 1, AP_Proximity, _type[0], 0), @@ -282,6 +283,13 @@ void AP_Proximity::detect_instance(uint8_t instance) drivers[instance] = new AP_Proximity_MAV(*this, state[instance]); return; } + if (type == Proximity_Type_TRTOWER) { + if (AP_Proximity_TeraRangerTower::detect(serial_manager)) { + state[instance].instance = instance; + drivers[instance] = new AP_Proximity_TeraRangerTower(*this, state[instance], serial_manager); + return; + } + } #if CONFIG_HAL_BOARD == HAL_BOARD_SITL if (type == Proximity_Type_SITL) { state[instance].instance = instance; diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index 77171f5fde..232c29f999 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -36,10 +36,11 @@ public: // Proximity driver types enum Proximity_Type { - Proximity_Type_None = 0, - Proximity_Type_SF40C = 1, - Proximity_Type_MAV = 2, - Proximity_Type_SITL = 10, + Proximity_Type_None = 0, + Proximity_Type_SF40C = 1, + Proximity_Type_MAV = 2, + Proximity_Type_TRTOWER = 3, + Proximity_Type_SITL = 10, }; enum Proximity_Status { diff --git a/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp b/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp new file mode 100644 index 0000000000..cab6de0488 --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp @@ -0,0 +1,143 @@ +/* + 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 . + */ + +#include +#include "AP_Proximity_TeraRangerTower.h" +#include +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +/* + The constructor also initialises the proximity sensor. Note that this + constructor is not called until detect() returns true, so we + already know that we should setup the proximity sensor +*/ +AP_Proximity_TeraRangerTower::AP_Proximity_TeraRangerTower(AP_Proximity &_frontend, + AP_Proximity::Proximity_State &_state, + AP_SerialManager &serial_manager) : + AP_Proximity_Backend(_frontend, _state) +{ + uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0); + if (uart != nullptr) { + uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar360, 0)); + } +} + +// detect if a TeraRanger Tower proximity sensor is connected by looking for a configured serial port +bool AP_Proximity_TeraRangerTower::detect(AP_SerialManager &serial_manager) +{ + AP_HAL::UARTDriver *uart = nullptr; + uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0); + return uart != nullptr; +} + +// update the state of the sensor +void AP_Proximity_TeraRangerTower::update(void) +{ + if (uart == nullptr) { + return; + } + + // process incoming messages + read_sensor_data(); + + // check for timeout and set health status + if ((_last_distance_received_ms == 0) || (AP_HAL::millis() - _last_distance_received_ms > PROXIMITY_TRTOWER_TIMEOUT_MS)) { + set_status(AP_Proximity::Proximity_NoData); + } else { + set_status(AP_Proximity::Proximity_Good); + } +} + +// get maximum and minimum distances (in meters) of primary sensor +float AP_Proximity_TeraRangerTower::distance_max() const +{ + return 4.5f; +} +float AP_Proximity_TeraRangerTower::distance_min() const +{ + return 0.20f; +} + +// check for replies from sensor, returns true if at least one message was processed +bool AP_Proximity_TeraRangerTower::read_sensor_data() +{ + if (uart == nullptr) { + return false; + } + + uint16_t message_count = 0; + int16_t nbytes = uart->available(); + + while (nbytes-- > 0) { + char c = uart->read(); + if (c == 'T' ) { + buffer_count = 0; + } + + buffer[buffer_count++] = c; + + // we should always read 19 bytes THxxxxxxxxxxxxxxxxC + if (buffer_count >= 19){ + buffer_count = 0; + + // check if message has right CRC + if (crc_crc8(buffer, 18) == buffer[18]){ + uint16_t d1 = process_distance(buffer[2], buffer[3]); + uint16_t d2 = process_distance(buffer[4], buffer[5]); + uint16_t d3 = process_distance(buffer[6], buffer[7]); + uint16_t d4 = process_distance(buffer[8], buffer[9]); + uint16_t d5 = process_distance(buffer[10], buffer[11]); + uint16_t d6 = process_distance(buffer[12], buffer[13]); + uint16_t d7 = process_distance(buffer[14], buffer[15]); + uint16_t d8 = process_distance(buffer[16], buffer[17]); + + update_sector_data(0, d1); + update_sector_data(45, d8); + update_sector_data(90, d7); + update_sector_data(135, d6); + update_sector_data(180, d5); + update_sector_data(225, d4); + update_sector_data(270, d3); + update_sector_data(315, d2); + + message_count++; + } + } + } + return (message_count > 0); +} + +uint16_t AP_Proximity_TeraRangerTower::process_distance(uint8_t buf1, uint8_t buf2) +{ + return (buf1 << 8) + buf2; +} + +// process reply +void AP_Proximity_TeraRangerTower::update_sector_data(int16_t angle_deg, uint16_t distance_cm) +{ + uint8_t sector; + if (convert_angle_to_sector(angle_deg, sector)) { + _angle[sector] = angle_deg; + _distance[sector] = ((float) distance_cm) / 1000; + _distance_valid[sector] = distance_cm != 0xffff; + _last_distance_received_ms = AP_HAL::millis(); + // update boundary used for avoidance + update_boundary_for_sector(sector); + } +} diff --git a/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.h b/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.h new file mode 100644 index 0000000000..9d7dbe6ee9 --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.h @@ -0,0 +1,39 @@ +#pragma once + +#include "AP_Proximity.h" +#include "AP_Proximity_Backend.h" + +#define PROXIMITY_TRTOWER_TIMEOUT_MS 300 // requests timeout after 0.3 seconds + +class AP_Proximity_TeraRangerTower : public AP_Proximity_Backend +{ + +public: + // constructor + AP_Proximity_TeraRangerTower(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state, AP_SerialManager &serial_manager); + + // static detection function + static bool detect(AP_SerialManager &serial_manager); + + // update state + void update(void); + + // get maximum and minimum distances (in meters) of sensor + float distance_max() const; + float distance_min() const; + +private: + + // check and process replies from sensor + bool read_sensor_data(); + void update_sector_data(int16_t angle_deg, uint16_t distance_cm); + uint16_t process_distance(uint8_t buf1, uint8_t buf2); + + // reply related variables + AP_HAL::UARTDriver *uart = nullptr; + uint8_t buffer[20]; // buffer where to store data from serial + uint8_t buffer_count; + + // request related variables + uint32_t _last_distance_received_ms; // system time of last distance measurement received from sensor +};