Add AP_Periph support for Proximity sensors

Tools/AP_Periph/AP_Periph.cpp

@@ -214,6 +214,17 @@ void AP_Periph_FW::init()
     }
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_PRX
+    if (proximity.get_type(0) != AP_Proximity::Type::None && g.proximity_port >= 0) {
+        auto *uart = hal.serial(g.proximity_port);
+        if (uart != nullptr) {
+            uart->begin(g.proximity_baud);
+            serial_manager.set_protocol_and_baud(g.proximity_port, AP_SerialManager::SerialProtocol_Lidar360, g.proximity_baud);
+            proximity.init();
+        }
+    }
+#endif
+
 #ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
     pwm_hardpoint_init();
 #endif

Tools/AP_Periph/AP_Periph.h

@@ -11,6 +11,7 @@
 #include <AP_BattMonitor/AP_BattMonitor.h>
 #include <AP_Airspeed/AP_Airspeed.h>
 #include <AP_RangeFinder/AP_RangeFinder.h>
+#include <AP_Proximity/AP_Proximity.h>
 #include <AP_EFI/AP_EFI.h>
 #include <AP_MSP/AP_MSP.h>
 #include <AP_MSP/msp.h>
@@ -90,6 +91,7 @@ class AP_Periph_FW {
     void can_airspeed_update();
     void can_rangefinder_update();
     void can_battery_update();
+    void can_proximity_update();
 
     void load_parameters();
     void prepare_reboot();
@@ -188,6 +190,10 @@ class AP_Periph_FW {
     uint32_t last_sample_ms;
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_PRX
+    AP_Proximity proximity;
+#endif
+
 #ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
     void pwm_irq_handler(uint8_t pin, bool pin_state, uint32_t timestamp);
     void pwm_hardpoint_init();

Tools/AP_Periph/Parameters.cpp

@@ -490,6 +490,40 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GOBJECT(efi, "EFI", AP_EFI),
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_PRX
+    // @Param: PRX_BAUDRATE
+    // @DisplayName: Proximity Sensor serial baudrate
+    // @Description: Proximity Sensor serial baudrate.
+    // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200,230:230400,256:256000,460:460800,500:500000,921:921600,1500:1500000
+    // @Increment: 1
+    // @User: Standard
+    // @RebootRequired: True
+    GSCALAR(proximity_baud, "PRX_BAUDRATE", HAL_PERIPH_RANGEFINDER_BAUDRATE_DEFAULT),
+
+    // @Param: PRX_PORT
+    // @DisplayName: Proximity Sensor Serial Port
+    // @Description: This is the serial port number where SERIALx_PROTOCOL will be set to Proximity Sensor.
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    // @RebootRequired: True
+    GSCALAR(proximity_port, "PRX_PORT", AP_PERIPH_RANGEFINDER_PORT_DEFAULT),
+
+    // @Param: PRX_MAX_RATE
+    // @DisplayName: Proximity Sensor max rate
+    // @Description: This is the maximum rate we send Proximity Sensor data in Hz. Zero means no limit
+    // @Units: Hz
+    // @Range: 0 200
+    // @Increment: 1
+    // @User: Advanced
+    GSCALAR(proximity_max_rate, "PRX_MAX_RATE", 50),
+
+    // Proximity driver
+    // @Group: PRX
+    // @Path: ../libraries/AP_RangeFinder/AP_Proximity.cpp
+    GOBJECT(proximity, "PRX", AP_Proximity),
+#endif
+
     AP_VAREND
 };
 

Tools/AP_Periph/Parameters.h

@@ -63,6 +63,10 @@ class Parameters {
         k_param_can_slcan_cport,
         k_param_temperature_sensor,
         k_param_esc_command_timeout_ms,
+        k_param_proximity,
+        k_param_proximity_baud,
+        k_param_proximity_port,
+        k_param_proximity_max_rate,
     };
 
     AP_Int16 format_version;
@@ -96,6 +100,13 @@ class Parameters {
     AP_Int16 rangefinder_max_rate;
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_PRX
+    AP_Int32 proximity_baud;
+    AP_Int8 proximity_port;
+    AP_Int16 proximity_max_rate;
+#endif
+
+
 #ifdef HAL_PERIPH_ENABLE_ADSB
     AP_Int32 adsb_baudrate;
     AP_Int8 adsb_port;

Tools/AP_Periph/can.cpp

@@ -1734,6 +1734,7 @@ void AP_Periph_FW::can_update()
         can_baro_update();
         can_airspeed_update();
         can_rangefinder_update();
+        can_proximity_update();
     #if defined(HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY) || defined (HAL_PERIPH_ENABLE_NOTIFY)
         can_buzzer_update();
     #endif
@@ -2333,6 +2334,72 @@ void AP_Periph_FW::can_rangefinder_update(void)
 }
 
 
+void AP_Periph_FW::can_proximity_update()
+{
+#ifdef HAL_PERIPH_ENABLE_PRX
+    if (proximity.get_type(0) == AP_Proximity::Type::None) {
+        return;
+    }
+
+    uint32_t now = AP_HAL::native_millis();
+    static uint32_t last_update_ms;
+    if (g.proximity_max_rate > 0 &&
+        now - last_update_ms < 1000/g.proximity_max_rate) {
+        // limit to max rate
+        return;
+    }
+    last_update_ms = now;
+    proximity.update();
+    AP_Proximity::Status status = proximity.get_status();
+    if (status <= AP_Proximity::Status::NoData) {
+        // don't send any data
+        return;
+    }
+
+    ardupilot_equipment_proximity_sensor_Proximity pkt {};
+
+    const uint8_t obstacle_count = proximity.get_obstacle_count();
+
+    // if no objects return
+    if (obstacle_count == 0) {
+        return;
+    }
+
+    // calculate maximum roll, pitch values from objects
+    for (uint8_t i=0; i<obstacle_count; i++) {
+        if (!proximity.get_obstacle_info(i, pkt.yaw, pkt.pitch, pkt.distance)) {
+            // not a valid obstacle
+            continue;
+        }
+
+        pkt.sensor_id = proximity.get_address(0);
+
+        switch (status) {
+        case AP_Proximity::Status::NotConnected:
+            pkt.reading_type = ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_READING_TYPE_NOT_CONNECTED;
+            break;
+        case AP_Proximity::Status::Good:
+            pkt.reading_type = ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_READING_TYPE_GOOD;
+            break;
+        case AP_Proximity::Status::NoData:
+        default:
+            pkt.reading_type = ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_READING_TYPE_NO_DATA;
+            break;
+        }
+
+        uint8_t buffer[ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_MAX_SIZE] {};
+        uint16_t total_size = ardupilot_equipment_proximity_sensor_Proximity_encode(&pkt, buffer, !periph.canfdout());
+
+        canard_broadcast(ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_SIGNATURE,
+                        ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_ID,
+                        CANARD_TRANSFER_PRIORITY_LOW,
+                        &buffer[0],
+                        total_size);
+
+    }
+#endif
+}
+
 #ifdef HAL_PERIPH_ENABLE_ADSB
 /*
   map an ADSB_VEHICLE MAVLink message to a UAVCAN TrafficReport message

Tools/AP_Periph/wscript

@@ -67,6 +67,7 @@ def build(bld):
                      'AP_Stats',
                      'AP_EFI',
                      'AP_CheckFirmware',
+                     'AP_Proximity',
                      ]
     bld.ap_stlib(
         name= 'AP_Periph_libs',

libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Proximity/hwdef-bl.dat

@@ -0,0 +1,3 @@
+include ../MatekL431/hwdef-bl.inc
+
+define CAN_APP_NODE_NAME "org.ardupilot.MatekL431-Proximity"

libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Proximity/hwdef.dat

@@ -0,0 +1,14 @@
+include ../MatekL431/hwdef.inc
+
+define CAN_APP_NODE_NAME "org.ardupilot.MatekL431-Proximity"
+
+define HAL_USE_ADC FALSE
+define STM32_ADC_USE_ADC1 FALSE
+define HAL_DISABLE_ADC_DRIVER TRUE
+
+# support all proximity types
+define HAL_PROXIMITY_ENABLED 1
+define HAL_PERIPH_ENABLE_PRX
+
+define AP_PERIPH_PRX_PORT_DEFAULT 2
+

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 @@ class AP_Proximity
         AirSimSITL = 12,
 #endif
         CYGBOT_D1 = 13,
+        DroneCAN = 14,
     };
 
     enum class Status {
@@ -115,6 +120,9 @@ class AP_Proximity
     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 @@ class AP_Proximity
     // 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 @@ class AP_Proximity
         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,23 +101,28 @@ 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);
     }
 }
 
 // update Object Avoidance database with Earth-frame point
 // 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 @@ class AP_Proximity_Backend
     // 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