AP_Proximity: Add DroneCAN backend

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 <AP_Logger/AP_Logger.h>
 
@@ -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)
 {

libraries/AP_Proximity/AP_Proximity.h

@@ -24,6 +24,9 @@
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include "AP_Proximity_Params.h"
 #include "AP_Proximity_Boundary_3D.h"
+#include <AP_Vehicle/AP_Vehicle_Type.h>
+
+#include <AP_HAL/Semaphores.h>
 
 #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 {

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 &current_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 &current_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 &current_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

libraries/AP_Proximity/AP_Proximity_Backend.h

@@ -18,6 +18,7 @@
 
 #if HAL_PROXIMITY_ENABLED
 #include <AP_Common/AP_Common.h>
+#include <AP_HAL/Semaphores.h>
 
 class AP_Proximity_Backend
 {
@@ -61,12 +62,21 @@ protected:
     // database helpers. All angles are in degrees
     static bool database_prepare_for_push(Vector3f &current_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 &current_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 &current_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 &params;            // parameters for this backend

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
 {

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

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 <http://www.gnu.org/licenses/>.
+ */
+
+#include "AP_Proximity_DroneCAN.h"
+
+#if AP_PROXIMITY_DRONECAN_ENABLED
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_CANManager/AP_CANManager.h>
+#include <AP_UAVCAN/AP_UAVCAN.h>
+#include <AP_BoardConfig/AP_BoardConfig.h>
+#include <GCS_MAVLink/GCS.h>
+
+#include <ardupilot/equipment/proximity_sensor/Proximity.hpp>
+
+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::ObstacleItem> 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<ardupilot::equipment::proximity_sensor::Proximity, MeasurementCb> *measurement_listener;
+    measurement_listener = new uavcan::Subscriber<ardupilot::equipment::proximity_sensor::Proximity, MeasurementCb>(*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

libraries/AP_Proximity/AP_Proximity_DroneCAN.h

@@ -0,0 +1,53 @@
+#pragma once
+
+#include "AP_Proximity_Backend.h"
+
+#include <AP_UAVCAN/AP_UAVCAN.h>
+
+#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<ObstacleItem> items;
+
+    AP_Proximity::Status _status;
+};
+
+#endif // AP_PROXIMITY_DRONECAN_ENABLED

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

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)
 };

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