From ea1fb96e7b2c5903225ed13a46ab39ca0485ae29 Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Wed, 28 Sep 2022 01:14:39 +0530 Subject: [PATCH] AP_Proximity: Add DroneCAN backend --- libraries/AP_Proximity/AP_Proximity.cpp | 11 + libraries/AP_Proximity/AP_Proximity.h | 14 ++ .../AP_Proximity/AP_Proximity_Backend.cpp | 12 +- libraries/AP_Proximity/AP_Proximity_Backend.h | 12 +- .../AP_Proximity/AP_Proximity_Boundary_3D.cpp | 18 +- .../AP_Proximity/AP_Proximity_Boundary_3D.h | 3 + .../AP_Proximity/AP_Proximity_DroneCAN.cpp | 210 ++++++++++++++++++ .../AP_Proximity/AP_Proximity_DroneCAN.h | 53 +++++ .../AP_Proximity/AP_Proximity_Params.cpp | 10 +- libraries/AP_Proximity/AP_Proximity_Params.h | 1 + libraries/AP_Proximity/AP_Proximity_Utils.cpp | 2 + 11 files changed, 341 insertions(+), 5 deletions(-) create mode 100644 libraries/AP_Proximity/AP_Proximity_DroneCAN.cpp create mode 100644 libraries/AP_Proximity/AP_Proximity_DroneCAN.h diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index 26d60b08d0..2a1f9d95bd 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -26,6 +26,7 @@ #include "AP_Proximity_SITL.h" #include "AP_Proximity_AirSimSITL.h" #include "AP_Proximity_Cygbot_D1.h" +#include "AP_Proximity_DroneCAN.h" #include @@ -179,7 +180,11 @@ void AP_Proximity::init() drivers[instance] = new AP_Proximity_Cygbot_D1(*this, state[instance], params[instance], serial_instance); serial_instance++; } + break; # endif + + case Type::DroneCAN: + num_instances = instance+1; break; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL @@ -354,6 +359,12 @@ bool AP_Proximity::get_object_angle_and_distance(uint8_t object_number, float& a return boundary.get_horizontal_object_angle_and_distance(object_number, angle_deg, distance); } +// get obstacle pitch and angle for a particular obstacle num +bool AP_Proximity::get_obstacle_info(uint8_t obstacle_num, float &angle_deg, float &pitch, float &distance) const +{ + return boundary.get_obstacle_info(obstacle_num, angle_deg, pitch, distance); +} + // handle mavlink messages void AP_Proximity::handle_msg(const mavlink_message_t &msg) { diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index 2171190649..d521d87359 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -24,6 +24,9 @@ #include #include "AP_Proximity_Params.h" #include "AP_Proximity_Boundary_3D.h" +#include + +#include #define PROXIMITY_MAX_INSTANCES 3 // Maximum number of proximity sensor instances available on this platform #define PROXIMITY_SENSOR_ID_START 10 @@ -34,6 +37,7 @@ class AP_Proximity { public: friend class AP_Proximity_Backend; + friend class AP_Proximity_DroneCAN; AP_Proximity(); @@ -56,6 +60,7 @@ public: AirSimSITL = 12, #endif CYGBOT_D1 = 13, + DroneCAN = 14, }; enum class Status { @@ -115,6 +120,9 @@ public: uint8_t get_object_count() const; bool get_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const; + // get obstacle pitch and angle for a particular obstacle num + bool get_obstacle_info(uint8_t obstacle_num, float &angle_deg, float &pitch, float &distance) const; + // // mavlink related methods // @@ -158,6 +166,11 @@ public: // Check if Obstacle defined by body-frame yaw and pitch is near ground bool check_obstacle_near_ground(float pitch, float yaw, float distance) const; + // get proximity address (for AP_Periph CAN) + uint8_t get_address(uint8_t id) const { + return id >= PROXIMITY_MAX_INSTANCES? 0 : uint8_t(params[id].address.get()); + } + protected: // parameters for backends @@ -188,6 +201,7 @@ private: uint32_t last_downward_update_ms; // last update ms } _rangefinder_state; + HAL_Semaphore detect_sem; }; namespace AP { diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.cpp b/libraries/AP_Proximity/AP_Proximity_Backend.cpp index cd18fcdd29..afd0d1a219 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Backend.cpp @@ -29,6 +29,7 @@ AP_Proximity_Backend::AP_Proximity_Backend(AP_Proximity& _frontend, AP_Proximity state(_state), params(_params) { + _backend_type = (AP_Proximity::Type )_params.type.get(); } static_assert(PROXIMITY_MAX_DIRECTION <= 8, @@ -87,6 +88,7 @@ bool AP_Proximity_Backend::ignore_reading(float pitch, float yaw, float distance // returns true if database is ready to be pushed to and all cached data is ready bool AP_Proximity_Backend::database_prepare_for_push(Vector3f ¤t_pos, Matrix3f &body_to_ned) { +#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph) AP_OADatabase *oaDb = AP::oadatabase(); if (oaDb == nullptr || !oaDb->healthy()) { return false; @@ -99,16 +101,19 @@ bool AP_Proximity_Backend::database_prepare_for_push(Vector3f ¤t_pos, Matr body_to_ned = AP::ahrs().get_rotation_body_to_ned(); return true; +#else + return false; +#endif } // update Object Avoidance database with Earth-frame point -void AP_Proximity_Backend::database_push(float angle, float distance) +void AP_Proximity_Backend::database_push(float angle, float pitch, float distance) { Vector3f current_pos; Matrix3f body_to_ned; if (database_prepare_for_push(current_pos, body_to_ned)) { - database_push(angle, distance, AP_HAL::millis(), current_pos, body_to_ned); + database_push(angle, pitch, distance, AP_HAL::millis(), current_pos, body_to_ned); } } @@ -116,6 +121,8 @@ void AP_Proximity_Backend::database_push(float angle, float distance) // pitch can be optionally provided if needed void AP_Proximity_Backend::database_push(float angle, float pitch, float distance, uint32_t timestamp_ms, const Vector3f ¤t_pos, const Matrix3f &body_to_ned) { + +#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph) AP_OADatabase *oaDb = AP::oadatabase(); if (oaDb == nullptr || !oaDb->healthy()) { return; @@ -135,6 +142,7 @@ void AP_Proximity_Backend::database_push(float angle, float pitch, float distanc temp_pos.z = temp_pos.z * -1.0f; oaDb->queue_push(temp_pos, timestamp_ms, distance); +#endif } #endif // HAL_PROXIMITY_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.h b/libraries/AP_Proximity/AP_Proximity_Backend.h index 42f092c374..6ed390d26b 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.h +++ b/libraries/AP_Proximity/AP_Proximity_Backend.h @@ -18,6 +18,7 @@ #if HAL_PROXIMITY_ENABLED #include +#include class AP_Proximity_Backend { @@ -61,12 +62,21 @@ protected: // database helpers. All angles are in degrees static bool database_prepare_for_push(Vector3f ¤t_pos, Matrix3f &body_to_ned); // Note: "angle" refers to yaw (in body frame) towards the obstacle - static void database_push(float angle, float distance); + static void database_push(float angle, float pitch, float distance); + static void database_push(float angle, float distance) { + database_push(angle, 0.0f, distance); + } + static void database_push(float angle, float distance, uint32_t timestamp_ms, const Vector3f ¤t_pos, const Matrix3f &body_to_ned) { database_push(angle, 0.0f, distance, timestamp_ms, current_pos, body_to_ned); }; static void database_push(float angle, float pitch, float distance, uint32_t timestamp_ms, const Vector3f ¤t_pos, const Matrix3f &body_to_ned); + // semaphore for access to shared frontend data + HAL_Semaphore _sem; + + AP_Proximity::Type _backend_type; + AP_Proximity &frontend; AP_Proximity::Proximity_State &state; // reference to this instances state AP_Proximity_Params ¶ms; // parameters for this backend diff --git a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp index 9fd2a7ae4e..336d7e787a 100644 --- a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp @@ -238,7 +238,6 @@ bool AP_Proximity_Boundary_3D::get_distance(const Face &face, float &distance) c if (!face.valid()) { return false; } - if (_distance_valid[face.layer][face.sector]) { distance = _distance[face.layer][face.sector]; return true; @@ -380,6 +379,23 @@ bool AP_Proximity_Boundary_3D::get_horizontal_object_angle_and_distance(uint8_t return false; } +// get an obstacle info for AP_Periph +// returns false if no angle or distance could be returned for some reason +bool AP_Proximity_Boundary_3D::get_obstacle_info(uint8_t obstacle_num, float &angle_deg, float &pitch_deg, float &distance) const +{ + // obstacle num is just "flattened layers, and sectors" + const uint8_t layer = obstacle_num / PROXIMITY_NUM_SECTORS; + const uint8_t sector = obstacle_num % PROXIMITY_NUM_SECTORS; + if (_distance_valid[layer][sector]) { + angle_deg = _angle[layer][sector]; + pitch_deg = _pitch[layer][sector]; + distance = _filtered_distance[layer][sector].get(); + return true; + } + + return false; +} + // Return filtered distance for the passed in face bool AP_Proximity_Boundary_3D::get_filtered_distance(const Face &face, float &distance) const { diff --git a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h index 4f22db5a31..7002204e0e 100644 --- a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h +++ b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h @@ -121,6 +121,9 @@ public: uint8_t get_horizontal_object_count() const; bool get_horizontal_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const; + // get obstacle info for AP_Periph + bool get_obstacle_info(uint8_t obstacle_num, float &angle_deg, float &pitch_deg, float &distance) const; + // get number of layers uint8_t get_num_layers() const { return PROXIMITY_NUM_LAYERS; } diff --git a/libraries/AP_Proximity/AP_Proximity_DroneCAN.cpp b/libraries/AP_Proximity/AP_Proximity_DroneCAN.cpp new file mode 100644 index 0000000000..dd94bca074 --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_DroneCAN.cpp @@ -0,0 +1,210 @@ +/* + 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 "AP_Proximity_DroneCAN.h" + +#if AP_PROXIMITY_DRONECAN_ENABLED + +#include +#include +#include +#include +#include + +#include + +extern const AP_HAL::HAL& hal; + +//DroneCAN Frontend Registry Binder ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY +UC_REGISTRY_BINDER(MeasurementCb, ardupilot::equipment::proximity_sensor::Proximity); + +ObjectBuffer_TS AP_Proximity_DroneCAN::items(50); + +#define PROXIMITY_TIMEOUT_MS 500 // distance messages must arrive within this many milliseconds + + +//links the Proximity DroneCAN message to this backend +void AP_Proximity_DroneCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan) +{ + if (ap_uavcan == nullptr) { + return; + } + + auto* node = ap_uavcan->get_node(); + + uavcan::Subscriber *measurement_listener; + measurement_listener = new uavcan::Subscriber(*node); + if (measurement_listener == nullptr) { + AP_BoardConfig::allocation_error("DroneCAN_PRX"); + } + + // Register method to handle incoming Proximity measurement + const int measurement_listener_res = measurement_listener->start(MeasurementCb(ap_uavcan, &handle_measurement)); + if (measurement_listener_res < 0) { + AP_BoardConfig::allocation_error("DroneCAN Proximity subscriber start problem\n\r"); + return; + } +} + +//Method to find the backend relating to the node id +AP_Proximity_DroneCAN* AP_Proximity_DroneCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t address, bool create_new) +{ + if (ap_uavcan == nullptr) { + return nullptr; + } + + AP_Proximity *prx = AP::proximity(); + if (prx == nullptr) { + return nullptr; + } + + AP_Proximity_DroneCAN* driver = nullptr; + //Scan through the proximity type params to find DroneCAN with matching address. + for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++) { + if ((AP_Proximity::Type)prx->params[i].type.get() == AP_Proximity::Type::DroneCAN && + prx->params[i].address == address) { + driver = (AP_Proximity_DroneCAN*)prx->drivers[i]; + } + //Double check if the driver was initialised as DroneCAN Type + if (driver != nullptr && (driver->_backend_type == AP_Proximity::Type::DroneCAN)) { + if (driver->_ap_uavcan == ap_uavcan && + driver->_node_id == node_id) { + return driver; + } else { + //we found a possible duplicate addressed sensor + //we return nothing in such scenario + return nullptr; + } + } + } + + if (create_new) { + for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++) { + if ((AP_Proximity::Type)prx->params[i].type.get() == AP_Proximity::Type::DroneCAN && + prx->params[i].address == address) { + WITH_SEMAPHORE(prx->detect_sem); + if (prx->drivers[i] != nullptr) { + //we probably initialised this driver as something else, reboot is required for setting + //it up as DroneCAN type + return nullptr; + } + prx->drivers[i] = new AP_Proximity_DroneCAN(*prx, prx->state[i], prx->params[i]); + driver = (AP_Proximity_DroneCAN*)prx->drivers[i]; + if (driver == nullptr) { + break; + } + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Prx[%u]: added DroneCAN node %u addr %u", + unsigned(i), unsigned(node_id), unsigned(address)); + + if (is_zero(prx->params[i].max_m) && is_zero(prx->params[i].min_m)) { + // GCS reporting will be incorrect if min/max are not set + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Configure PRX%u_MIN and PRX%u_MAX", + unsigned(i), unsigned(i)); + } + //Assign node id and respective dronecan driver, for identification + if (driver->_ap_uavcan == nullptr) { + driver->_ap_uavcan = ap_uavcan; + driver->_node_id = node_id; + break; + } + } + } + } + + return driver; +} + + +// update the state of the sensor +void AP_Proximity_DroneCAN::update(void) +{ + // check for timeout and set health status + if ((_last_update_ms == 0 || (AP_HAL::millis() - _last_update_ms > PROXIMITY_TIMEOUT_MS))) { + set_status(AP_Proximity::Status::NoData); + } else { + set_status(_status); + } + + if (_status == AP_Proximity::Status::Good) { + ObstacleItem object_item; + WITH_SEMAPHORE(_sem); + while (items.pop(object_item)) { + const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(object_item.pitch_deg, object_item.yaw_deg); + if (!is_zero(object_item.distance_m) && !ignore_reading(object_item.pitch_deg, object_item.yaw_deg, object_item.distance_m, false)) { + // update boundary used for avoidance + frontend.boundary.set_face_attributes(face, object_item.pitch_deg, object_item.yaw_deg, object_item.distance_m, state.instance); + // update OA database + database_push(object_item.pitch_deg, object_item.yaw_deg, object_item.distance_m); + } + } + } +} + +// get maximum and minimum distances (in meters) +float AP_Proximity_DroneCAN::distance_max() const +{ + if (is_zero(params.max_m)) { + // GCS will not report correct correct value if max isn't set properly + // This is a arbitrary value to prevent the above issue + return 100.0f; + } + return params.max_m; +} + +float AP_Proximity_DroneCAN::distance_min() const +{ + return params.min_m; +} + +//Proximity message handler +void AP_Proximity_DroneCAN::handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MeasurementCb &cb) +{ + //fetch the matching DroneCAN driver, node id and sensor id backend instance + AP_Proximity_DroneCAN* driver = get_uavcan_backend(ap_uavcan, node_id, cb.msg->sensor_id, true); + if (driver == nullptr) { + return; + } + WITH_SEMAPHORE(driver->_sem); + switch (cb.msg->reading_type) { + case ardupilot::equipment::proximity_sensor::Proximity::READING_TYPE_GOOD: { + //update the states in backend instance + driver->_last_update_ms = AP_HAL::millis(); + driver->_status = AP_Proximity::Status::Good; + const ObstacleItem item = {cb.msg->yaw, cb.msg->pitch, cb.msg->distance}; + + if (driver->items.space()) { + // ignore reading if no place to put it in the queue + driver->items.push(item); + } + break; + } + //Additional states supported by Proximity message + case ardupilot::equipment::proximity_sensor::Proximity::READING_TYPE_NOT_CONNECTED: { + driver->_last_update_ms = AP_HAL::millis(); + driver->_status = AP_Proximity::Status::NotConnected; + break; + } + case ardupilot::equipment::proximity_sensor::Proximity::READING_TYPE_NO_DATA: { + driver->_last_update_ms = AP_HAL::millis(); + driver->_status = AP_Proximity::Status::NoData; + break; + } + default: + break; + } +} + + +#endif // AP_PROXIMITY_DRONECAN_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_DroneCAN.h b/libraries/AP_Proximity/AP_Proximity_DroneCAN.h new file mode 100644 index 0000000000..3a1eab9a40 --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_DroneCAN.h @@ -0,0 +1,53 @@ +#pragma once + +#include "AP_Proximity_Backend.h" + +#include + +#ifndef AP_PROXIMITY_DRONECAN_ENABLED +#define AP_PROXIMITY_DRONECAN_ENABLED (HAL_CANMANAGER_ENABLED && HAL_PROXIMITY_ENABLED) +#endif + +#if AP_PROXIMITY_DRONECAN_ENABLED +class MeasurementCb; + +class AP_Proximity_DroneCAN : public AP_Proximity_Backend +{ +public: + // constructor + using AP_Proximity_Backend::AP_Proximity_Backend; + + // update state + void update(void) override; + + // get maximum and minimum distances (in meters) of sensor + float distance_max() const override; + float distance_min() const override; + + + static AP_Proximity_DroneCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t address, bool create_new); + + + static void subscribe_msgs(AP_UAVCAN* ap_uavcan); + + static void handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MeasurementCb &cb); + +private: + + uint32_t _last_update_ms; // system time of last message received + + AP_UAVCAN* _ap_uavcan; + uint8_t _node_id; + + struct ObstacleItem { + float yaw_deg; + float pitch_deg; + float distance_m; + }; + + static ObjectBuffer_TS items; + + AP_Proximity::Status _status; +}; + +#endif // AP_PROXIMITY_DRONECAN_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_Params.cpp b/libraries/AP_Proximity/AP_Proximity_Params.cpp index af045c2b1b..1baf22d567 100644 --- a/libraries/AP_Proximity/AP_Proximity_Params.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Params.cpp @@ -8,7 +8,7 @@ const AP_Param::GroupInfo AP_Proximity_Params::var_info[] = { // @Param: _TYPE // @DisplayName: Proximity type // @Description: What type of proximity sensor is connected - // @Values: 0:None,7:LightwareSF40c,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,8:LightwareSF45B,10:SITL,12:AirSimSITL,13:CygbotD1 + // @Values: 0:None,7:LightwareSF40c,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,8:LightwareSF45B,10:SITL,12:AirSimSITL,13:CygbotD1, 14:DroneCAN // @RebootRequired: True // @User: Standard AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Proximity_Params, type, 0, AP_PARAM_FLAG_ENABLE), @@ -108,6 +108,14 @@ const AP_Param::GroupInfo AP_Proximity_Params::var_info[] = { // @User: Advanced AP_GROUPINFO("_MAX", 17, AP_Proximity_Params, max_m, 0.0f), + // @Param: _ADDR + // @DisplayName: Bus address of sensor + // @Description: The bus address of the sensor, where applicable. Used for the I2C and DroneCAN sensors to allow for multiple sensors on different addresses. + // @Range: 0 127 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("_ADDR", 25, AP_Proximity_Params, address, 0), + AP_GROUPEND }; diff --git a/libraries/AP_Proximity/AP_Proximity_Params.h b/libraries/AP_Proximity/AP_Proximity_Params.h index f5df84fc3b..54a663970d 100644 --- a/libraries/AP_Proximity/AP_Proximity_Params.h +++ b/libraries/AP_Proximity/AP_Proximity_Params.h @@ -23,4 +23,5 @@ public: AP_Int8 ignore_width_deg[PROXIMITY_MAX_IGNORE]; // width of beam (in degrees) that should be ignored AP_Float max_m; // maximum range in meters AP_Float min_m; // minimum range in meters + AP_Int8 address; // proximity address (for AP_Periph CAN) }; diff --git a/libraries/AP_Proximity/AP_Proximity_Utils.cpp b/libraries/AP_Proximity/AP_Proximity_Utils.cpp index 9db47c8f73..c9dd130ab9 100644 --- a/libraries/AP_Proximity/AP_Proximity_Utils.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Utils.cpp @@ -54,6 +54,7 @@ bool AP_Proximity::get_rangefinder_alt(float &alt_m) const // Check if Obstacle defined by body-frame yaw and pitch is near ground bool AP_Proximity::check_obstacle_near_ground(float pitch, float yaw, float distance) const { +#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph) if (!_ign_gnd_enable) { return false; } @@ -83,6 +84,7 @@ bool AP_Proximity::check_obstacle_near_ground(float pitch, float yaw, float dist return true; } } +#endif return false; }