From 88a5548f1dcd310f00acbe05ca8389edc1063f3a Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Wed, 28 Sep 2022 01:14:39 +0530 Subject: [PATCH 1/4] 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 26d60b08d04eb..2a1f9d95bd386 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 2171190649a87..d521d87359ce3 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 @@ 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 { diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.cpp b/libraries/AP_Proximity/AP_Proximity_Backend.cpp index cd18fcdd29681..afd0d1a21976f 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 42f092c3747da..6ed390d26b09b 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 @@ class AP_Proximity_Backend // 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 9fd2a7ae4eb4a..336d7e787a37e 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 4f22db5a31863..7002204e0ed8c 100644 --- a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h +++ b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h @@ -121,6 +121,9 @@ class AP_Proximity_Boundary_3D 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 0000000000000..dd94bca0746af --- /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 0000000000000..3a1eab9a40407 --- /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 af045c2b1baca..1baf22d567452 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 f5df84fc3b0c8..54a663970d685 100644 --- a/libraries/AP_Proximity/AP_Proximity_Params.h +++ b/libraries/AP_Proximity/AP_Proximity_Params.h @@ -23,4 +23,5 @@ class AP_Proximity_Params { 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 9db47c8f7305d..c9dd130ab90ff 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; } From 6dcfa232cfb220afaef406531d82cd9e561c2560 Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Wed, 28 Sep 2022 01:15:07 +0530 Subject: [PATCH 2/4] AP_Periph: Add proximity support --- Tools/AP_Periph/AP_Periph.cpp | 11 ++++++ Tools/AP_Periph/AP_Periph.h | 6 +++ Tools/AP_Periph/Parameters.cpp | 34 +++++++++++++++++ Tools/AP_Periph/Parameters.h | 11 ++++++ Tools/AP_Periph/can.cpp | 67 ++++++++++++++++++++++++++++++++++ Tools/AP_Periph/wscript | 1 + 6 files changed, 130 insertions(+) diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index 0e2efab410131..434eaa7d7d333 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/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 diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 184506c8b4c5a..9c3f2ad02ae58 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -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(); diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 21700de273cd3..fa30f6a9ae46d 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/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 }; diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index d0a2f30b25221..e6013dea1893c 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/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; diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 6e8a95685fe59..c407e6313eb08 100644 --- a/Tools/AP_Periph/can.cpp +++ b/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 Date: Wed, 28 Sep 2022 01:15:45 +0530 Subject: [PATCH 3/4] hwdef: Add Proximity hwdef for L431 --- .../hwdef/MatekL431-Proximity/hwdef-bl.dat | 3 +++ .../hwdef/MatekL431-Proximity/hwdef.dat | 14 ++++++++++++++ 2 files changed, 17 insertions(+) create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Proximity/hwdef-bl.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Proximity/hwdef.dat diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Proximity/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Proximity/hwdef-bl.dat new file mode 100644 index 0000000000000..9b1ad2a20f80d --- /dev/null +++ b/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" diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Proximity/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Proximity/hwdef.dat new file mode 100644 index 0000000000000..723712cf4e3c0 --- /dev/null +++ b/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 + From b888112fd378c6f251083f61c9126243ea1ac086 Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Wed, 14 Dec 2022 13:36:07 +0530 Subject: [PATCH 4/4] AP_UAVCAN: Add proximity support --- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 0020150936854..9dbe0c8a4972e 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -61,6 +61,7 @@ #include #include #include "AP_UAVCAN_pool.h" +#include #define LED_DELAY_US 50000 @@ -356,6 +357,10 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) AP_EFI_DroneCAN::subscribe_msgs(this); #endif +#if AP_PROXIMITY_DRONECAN_ENABLED + AP_Proximity_DroneCAN::subscribe_msgs(this); +#endif + act_out_array[driver_index] = new uavcan::Publisher(*_node); act_out_array[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2)); act_out_array[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);