diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 5caab9762d..a9c9c60166 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -196,11 +196,11 @@ void Copter::init_ardupilot() barometer.set_log_baro_bit(MASK_LOG_IMU); barometer.calibrate(); - // initialise rangefinder - init_rangefinder(); + // // initialise rangefinder + // init_rangefinder(); - // init proximity sensor - init_proximity(); + // // init proximity sensor + // init_proximity(); #if BEACON_ENABLED == ENABLED // init beacons used for non-gps position estimation @@ -253,6 +253,11 @@ void Copter::init_ardupilot() enable_motor_output(); } + // initialise rangefinder + init_rangefinder(); + + // init proximity sensor + // init_proximity(); // disable safety if requested BoardConfig.init_safety(); diff --git a/ArduCopter/zr_flight.cpp b/ArduCopter/zr_flight.cpp index 126040be02..781bc577c3 100644 --- a/ArduCopter/zr_flight.cpp +++ b/ArduCopter/zr_flight.cpp @@ -84,6 +84,18 @@ void Copter::zr_SuperSlowLoop(){ } if(before_fly){ zr_set_battery_capacity(); + + + static uint32_t prx_init_time = AP_HAL::millis(); + static bool init_prx = false; + if (init_prx == false ) { + uint32_t tnow = AP_HAL::millis(); + if(tnow - prx_init_time > 5000){ + init_prx = true; + init_proximity(); + gcs().send_text(MAV_SEVERITY_INFO, "--- init prx:%dms -> %dms ---",prx_init_time,tnow); + } + } } } diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index 71bdf2019c..fe5760461f 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -162,7 +162,7 @@ #endif #ifndef HAL_WITH_UAVCAN -#define HAL_WITH_UAVCAN 0 +#define HAL_WITH_UAVCAN 1 #endif #ifndef HAL_RCINPUT_WITH_AP_RADIO diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index 2700050024..ae16df7455 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -25,6 +25,7 @@ #include "AP_Proximity_SITL.h" #include "AP_Proximity_MorseSITL.h" #include "AP_Proximity_AirSimSITL.h" +#include "AP_Proximity_UAVCAN.h" #include extern const AP_HAL::HAL &hal; @@ -336,7 +337,13 @@ void AP_Proximity::detect_instance(uint8_t instance) return; } break; - +#if HAL_WITH_UAVCAN + case Type::UAVCAN: + state[instance].instance = instance; + // drivers[instance] = new AP_Proximity_UAVCAN(*this, state[instance]); + drivers[instance] = AP_Proximity_UAVCAN::probe(*this, state[instance]); + return; +#endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case Type::SITL: state[instance].instance = instance; diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index 6ee3192ebb..cb5eecd09f 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -48,6 +48,7 @@ public: TRTOWEREVO = 6, SF40C = 7, MR72 = 8, + UAVCAN =9, #if CONFIG_HAL_BOARD == HAL_BOARD_SITL SITL = 10, MorseSITL = 11, diff --git a/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp b/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp new file mode 100644 index 0000000000..8cc86bff1f --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp @@ -0,0 +1,232 @@ +#include + +#if HAL_WITH_UAVCAN + +#include "AP_Proximity_UAVCAN.h" +#include +#include +#include + +#include +#define PRX_DEBUG 1 +#if PRX_DEBUG +#include + # define Prx_Debug(fmt, args ...) gcs().send_text(MAV_SEVERITY_INFO, "prx-%d: " fmt "", __LINE__, ## args) +#else + # define Prx_Debug(fmt, args ...) +#endif + +extern const AP_HAL::HAL &hal; + +#define debug_proximity_uavcan(level_debug, can_driver, fmt, args...) \ + do \ + { \ + if ((level_debug) <= AP::can().get_debug_level_driver(can_driver)) \ + { \ + hal.console->printf(fmt, ##args); \ + } \ + } while (0) + +HAL_Semaphore AP_Proximity_UAVCAN::_sem_registry; +UC_REGISTRY_BINDER(ProximityCb, zrzk::equipment::range_sensor::Proximity); + +uint8_t AP_Proximity_UAVCAN::_node_id = 0; +AP_Proximity_UAVCAN* AP_Proximity_UAVCAN::_driver = nullptr; +AP_UAVCAN* AP_Proximity_UAVCAN::_ap_uavcan = nullptr; + +AP_Proximity_UAVCAN::DetectedModules AP_Proximity_UAVCAN::_detected_modules[] = {0}; + +AP_Proximity_UAVCAN::AP_Proximity_UAVCAN(AP_Proximity &_frontend, + AP_Proximity::Proximity_State &_state) : + AP_Proximity_Backend(_frontend, _state) +{ + _driver = this; +} + +void AP_Proximity_UAVCAN::subscribe_msgs(AP_UAVCAN *ap_uavcan) +{ + if (ap_uavcan == nullptr) + { + return; + } + + auto *node = ap_uavcan->get_node(); + + uavcan::Subscriber *proximity_listener; + proximity_listener = new uavcan::Subscriber(*node); + // Register method to handle incoming RangeFinder measurement + const int proximity_listener_res = proximity_listener->start(ProximityCb(ap_uavcan, &handle_proximity_data_trampoline)); + if (proximity_listener_res < 0) + { + AP_HAL::panic("UAVCAN RangeFinder subscriber start problem\n\r"); + return; + } +} + +AP_Proximity_Backend *AP_Proximity_UAVCAN::probe(AP_Proximity &_frontend, + AP_Proximity::Proximity_State &_state) +{ + WITH_SEMAPHORE(_sem_registry); + + AP_Proximity_UAVCAN *backend = nullptr; + Prx_Debug("---------- AP_Proximity_UAVCAN ----------"); + backend = new AP_Proximity_UAVCAN(_frontend, _state); + if (backend == nullptr) + { + Prx_Debug("backend:%d",(int)backend); + }else{ + + Prx_Debug("faild,backend"); + } + + Prx_Debug("---------- AP_Proximity_UAVCAN ----------\n"); + return backend; +} + +AP_Proximity_UAVCAN *AP_Proximity_UAVCAN::get_uavcan_backend(AP_UAVCAN *ap_uavcan,uint8_t node_id) +{ + if (ap_uavcan == nullptr) + { + return nullptr; + } + + for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++) + { + if (_detected_modules[i].driver != nullptr && + _detected_modules[i].ap_uavcan == ap_uavcan&& + _detected_modules[i].node_id == node_id) + { + return _detected_modules[i].driver; + } + } + + bool detected = false; + for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++) + { + if (_detected_modules[i].ap_uavcan == ap_uavcan && + _detected_modules[i].node_id == node_id) + { + // detected + detected = true; + break; + } + } + + if (!detected) + { + for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++) + { + if (_detected_modules[i].ap_uavcan == nullptr) + { + _detected_modules[i].ap_uavcan = ap_uavcan; + _detected_modules[i].node_id = node_id; + break; + } + } + } + + return nullptr; +} + +float AP_Proximity_UAVCAN::distance_max() const +{ + return 40.0f; +} + +float AP_Proximity_UAVCAN::distance_min() const +{ + return 0.20f; +} + +void AP_Proximity_UAVCAN::update(void) +{ + WITH_SEMAPHORE(_sem_proximity); + + update_sector_data(0, _interim_data.d1_cm); // d1 + update_sector_data(45, _interim_data.d2_cm); // d8 + update_sector_data(90, _interim_data.d3_cm); // d7 + update_sector_data(135, _interim_data.d4_cm); // d6 + update_sector_data(180, _interim_data.d5_cm); // d5 + update_sector_data(225, _interim_data.d6_cm); // d4 + update_sector_data(270, _interim_data.d7_cm); // d3 + update_sector_data(315, _interim_data.d8_cm); // d2 + + if ((_last_distance_received_ms == 0) || (AP_HAL::millis() - _last_distance_received_ms > PROXIMITY_UAVCAN_TIMEOUT_MS)) + { + set_status(AP_Proximity::Status::NoData); + // gcs().send_text(MAV_SEVERITY_INFO, "NoData\r\n"); + } + else + { + set_status(AP_Proximity::Status::Good); + } +} + +void AP_Proximity_UAVCAN::handle_proximity_data_trampoline(AP_UAVCAN *ap_uavcan, uint8_t node_id, const ProximityCb &cb) +{ + + if (_driver == nullptr) { + return; + } + + if (_ap_uavcan == nullptr) { + _ap_uavcan = ap_uavcan; + _node_id = node_id; + Prx_Debug("---- uavcan: ap:%d,id:%d ----",(int)_ap_uavcan,_node_id); + } + // Prx_Debug("handle_proximity: ap:%d,id:%d ,%d",(int)_ap_uavcan,_node_id,_driver->_interim_data.d1_cm); + if (_ap_uavcan == ap_uavcan && _node_id == node_id) { + WITH_SEMAPHORE(_sem_registry); + _driver->_interim_data.d1_cm = cb.msg->d0; + _driver->_interim_data.d2_cm = cb.msg->d45; + _driver->_interim_data.d3_cm = cb.msg->d90; + _driver->_interim_data.d4_cm = cb.msg->d135; + _driver->_interim_data.d5_cm = cb.msg->d180; + _driver->_interim_data.d6_cm = cb.msg->d225; + _driver->_interim_data.d7_cm = cb.msg->d270; + _driver->_interim_data.d8_cm = cb.msg->d315; + _driver->_last_distance_received_ms = AP_HAL::millis(); + } + /* + AP_Proximity_UAVCAN *driver = get_uavcan_backend(ap_uavcan,node_id); + if (driver == nullptr) + { + return; + } + driver->handle_proximity_data(cb); + */ +} + +void AP_Proximity_UAVCAN::handle_proximity_data(const ProximityCb &cb) +{ + + WITH_SEMAPHORE(_sem_proximity); + + _interim_data.d1_cm = cb.msg->d0; + _interim_data.d2_cm = cb.msg->d45; + _interim_data.d3_cm = cb.msg->d90; + _interim_data.d4_cm = cb.msg->d135; + _interim_data.d5_cm = cb.msg->d180; + _interim_data.d6_cm = cb.msg->d225; + _interim_data.d7_cm = cb.msg->d270; + _interim_data.d8_cm = cb.msg->d315; + _last_distance_received_ms = AP_HAL::millis(); + // _last_sample_time_ms = AP_HAL::millis(); +} + +void AP_Proximity_UAVCAN::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) / 100; + _distance_valid[sector] = (_distance[sector] > distance_min() && _distance[sector] < distance_max()); + // Prx_Debug("proximity:[%d],%.2f",sector,_distance[sector]); + //_last_distance_received_ms = AP_HAL::millis(); + // update boundary used for avoidance + update_boundary_for_sector(sector, true); + } +} + +#endif diff --git a/libraries/AP_Proximity/AP_Proximity_UAVCAN.h b/libraries/AP_Proximity/AP_Proximity_UAVCAN.h new file mode 100644 index 0000000000..d81f99b069 --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_UAVCAN.h @@ -0,0 +1,72 @@ +#pragma once + +#include "AP_Proximity_Backend.h" + +#if HAL_WITH_UAVCAN +#include + +class ProximityCb; + +#define PROXIMITY_UAVCAN_TIMEOUT_MS 300 +class AP_Proximity_UAVCAN : public AP_Proximity_Backend +{ +public: + AP_Proximity_UAVCAN(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_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 void subscribe_msgs(AP_UAVCAN *ap_uavcan); + + static AP_Proximity_Backend *probe(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state); + + static void handle_proximity_data_trampoline(AP_UAVCAN *ap_uavcan, uint8_t node_id, const ProximityCb &cb); + + //uint8_t _instance; + //AP_Proximity::Proximity_State _status; + +private: + static AP_Proximity_UAVCAN *get_uavcan_backend(AP_UAVCAN *ap_uavcan,uint8_t node_id); + + static AP_Proximity_UAVCAN* _driver; + // inline void register_sensor() + // { + // // instance = frontend.register_sensor(); + // } + + uint32_t _last_distance_received_ms; + void handle_proximity_data(const ProximityCb &cb); + struct Proximity_Data + { + uint16_t d1_cm; + uint16_t d2_cm; + uint16_t d3_cm; + uint16_t d4_cm; + uint16_t d5_cm; + uint16_t d6_cm; + uint16_t d7_cm; + uint16_t d8_cm; + } _interim_data; + void update_sector_data(int16_t angle_deg, uint16_t distance_cm); + + uint16_t handle_data_valid(uint16_t data); + + static struct DetectedModules + { + AP_UAVCAN *ap_uavcan; + uint8_t node_id; + AP_Proximity_UAVCAN *driver; + } _detected_modules[PROXIMITY_MAX_INSTANCES]; + + static HAL_Semaphore _sem_registry; + HAL_Semaphore _sem_proximity; + + static AP_UAVCAN* _ap_uavcan; + static uint8_t _node_id; + + +}; +#endif //HAL_WITH_UAVCAN \ No newline at end of file diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 7291f1aa56..4d4f9a6292 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -77,6 +77,7 @@ #include #include #include +#include #define LED_DELAY_US 50000 @@ -249,7 +250,7 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) _node->setNodeID(self_node_id); char ndname[20]; - snprintf(ndname, sizeof(ndname), "org.ardupilot:%u", driver_index); + snprintf(ndname, sizeof(ndname), "zrzk.rover:%u", driver_index); uavcan::NodeStatusProvider::NodeName name(ndname); _node->setName(name); @@ -298,6 +299,7 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) AP_Airspeed_UAVCAN::subscribe_msgs(this); AP_OpticalFlow_HereFlow::subscribe_msgs(this); AP_RangeFinder_UAVCAN::subscribe_msgs(this); + AP_Proximity_UAVCAN::subscribe_msgs(this); act_out_array[driver_index] = new uavcan::Publisher(*_node); act_out_array[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2)); diff --git a/libraries/AP_UAVCAN/dsdl/zrzk/equipment/range_sensor/26110.Proximity.uavcan b/libraries/AP_UAVCAN/dsdl/zrzk/equipment/range_sensor/26110.Proximity.uavcan new file mode 100644 index 0000000000..9bbc1370c9 --- /dev/null +++ b/libraries/AP_UAVCAN/dsdl/zrzk/equipment/range_sensor/26110.Proximity.uavcan @@ -0,0 +1,23 @@ +uint8 sensor_id + +uint5 SENSOR_TYPE_UNDEFINED = 0 +uint5 SENSOR_TYPE_SONAR = 1 +uint5 SENSOR_TYPE_LIDAR = 2 +uint5 SENSOR_TYPE_RADAR = 3 +uint5 sensor_type + +uint3 READING_TYPE_UNDEFINED = 0 # Range is unknown +uint3 READING_TYPE_VALID_RANGE = 1 # Range field contains valid distance +uint3 READING_TYPE_TOO_CLOSE = 2 # Range field contains min range for the sensor +uint3 READING_TYPE_TOO_FAR = 3 # Range field contains max range for the sensor +uint3 reading_type + +uint16 d0 # Meters distance_cm +uint16 d45 +uint16 d90 +uint16 d135 +uint16 d180 +uint16 d225 +uint16 d270 +uint16 d315 + \ No newline at end of file