diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index e11d6dd1dc..daaeaa31a4 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -347,7 +347,7 @@ void AP_Proximity::detect_instance(uint8_t instance) #endif 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; } } diff --git a/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp b/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp index 81c813204b..15d0d4468d 100644 --- a/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp +++ b/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp @@ -7,50 +7,25 @@ #include #include -#include +#include -extern const AP_HAL::HAL& hal; -AP_Proximity_UAVCAN::DetectedModules AP_Proximity_UAVCAN::_detected_modules[] = {0}; -UC_REGISTRY_BINDER(ProximityCb, zrzk::equipment::range_sensor::Proximity); +extern const AP_HAL::HAL &hal; -#define debug_baro_uavcan(level_debug, can_driver, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(can_driver)) { printf(fmt, ##args); }} while (0) +#define debug_proximity_uavcan(level_debug, can_driver, fmt, args...) \ + do \ + { \ + if ((level_debug) <= AP::can().get_debug_level_driver(can_driver)) \ + { \ + printf(fmt, ##args); \ + } \ + } while (0) -//HAL_Semaphore AP_Airspeed_UAVCAN::_sem_registry; +HAL_Semaphore AP_Proximity_UAVCAN::_sem_registry; +UC_REGISTRY_BINDER(ProximityCb, zrzk::equipment::range_sensor::Proximity); +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) +AP_Proximity_UAVCAN::AP_Proximity_UAVCAN(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state) : AP_Proximity_Backend(_frontend, _state) { - AP_Proximity_UAVCAN *backend = nullptr; - for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++) - { - if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_uavcan != nullptr) - { - backend = new AP_Proximity_UAVCAN(_frontend, _state); - if (backend == nullptr) - { - debug_baro_uavcan(2, - _detected_modules[i].ap_uavcan->get_driver_index(), - "Failed register UAVCAN Proximity Node %d on Bus %d\n", - _detected_modules[i].node_id, - _detected_modules[i].ap_uavcan->get_driver_index()); - } - else - { - _detected_modules[i].driver = backend; - backend->_ap_uavcan = _detected_modules[i].ap_uavcan; - backend->_node_id = _detected_modules[i].node_id; - backend->register_sensor(); - debug_baro_uavcan(2, - _detected_modules[i].ap_uavcan->get_driver_index(), - "Registered UAVCAN Proximity Node %d on Bus %d\n", - _detected_modules[i].node_id, - _detected_modules[i].ap_uavcan->get_driver_index()); - } - break; - } - } } void AP_Proximity_UAVCAN::subscribe_msgs(AP_UAVCAN *ap_uavcan) @@ -65,7 +40,7 @@ void AP_Proximity_UAVCAN::subscribe_msgs(AP_UAVCAN *ap_uavcan) 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)); + 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"); @@ -73,10 +48,13 @@ void AP_Proximity_UAVCAN::subscribe_msgs(AP_UAVCAN *ap_uavcan) } } -bool AP_Proximity_UAVCAN::detect(AP_Proximity &_frontend, - AP_Proximity::Proximity_State &_state) +AP_Proximity_Backend *AP_Proximity_UAVCAN::probe(AP_Proximity &_frontend, + AP_Proximity::Proximity_State &_state) { + WITH_SEMAPHORE(_sem_registry); + AP_Proximity_UAVCAN *backend = nullptr; + for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++) { if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_uavcan != nullptr) @@ -84,23 +62,20 @@ bool AP_Proximity_UAVCAN::detect(AP_Proximity &_frontend, backend = new AP_Proximity_UAVCAN(_frontend, _state); if (backend == nullptr) { - debug_baro_uavcan(2, - _detected_modules[i].ap_uavcan->get_driver_index(), - "Failed register UAVCAN Proximity Node %d on Bus %d\n", - _detected_modules[i].node_id, - _detected_modules[i].ap_uavcan->get_driver_index()); + debug_proximity_uavcan(2, + _detected_modules[i].ap_uavcan->get_driver_index(), + "Failed register UAVCAN Proximity Node %d on Bus %d\n", + _detected_modules[i].node_id, + _detected_modules[i].ap_uavcan->get_driver_index()); } else { _detected_modules[i].driver = backend; - backend->_ap_uavcan = _detected_modules[i].ap_uavcan; - backend->_node_id = _detected_modules[i].node_id; - // backend->register_sensor(); - debug_baro_uavcan(2, - _detected_modules[i].ap_uavcan->get_driver_index(), - "Registered UAVCAN Proximity Node %d on Bus %d\n", - _detected_modules[i].node_id, - _detected_modules[i].ap_uavcan->get_driver_index()); + debug_proximity_uavcan(2, + _detected_modules[i].ap_uavcan->get_driver_index(), + "Registered UAVCAN Proximity Node %d on Bus %d\n", + _detected_modules[i].node_id, + _detected_modules[i].ap_uavcan->get_driver_index()); } break; } @@ -108,44 +83,46 @@ bool AP_Proximity_UAVCAN::detect(AP_Proximity &_frontend, return backend; } -AP_Proximity_UAVCAN *AP_Proximity_UAVCAN::get_uavcan_backend(AP_UAVCAN *ap_uavcan, uint8_t node_id, bool create_new) +AP_Proximity_UAVCAN *AP_Proximity_UAVCAN::get_uavcan_backend(AP_UAVCAN *ap_uavcan) { if (ap_uavcan == nullptr) { return nullptr; } - for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++) { + 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) { + _detected_modules[i].ap_uavcan == ap_uavcan) + { return _detected_modules[i].driver; } } - - if (create_new) { - bool already_detected = false; - //Check if there's an empty spot for possible registeration - 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) { - //Already Detected - already_detected = true; - break; - } + + bool detected = false; + for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++) + { + if (_detected_modules[i].ap_uavcan == ap_uavcan) + { + // detected + detected = true; + break; } - if (!already_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; - } + } + + 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; + break; } } } return nullptr; - } float AP_Proximity_UAVCAN::distance_max() const @@ -160,21 +137,21 @@ float AP_Proximity_UAVCAN::distance_min() const void AP_Proximity_UAVCAN::update(void) { - //WITH_SEMAPHORE(_sem_registry); - update_sector_data(0, d1_cm); // d1 - update_sector_data(45, d8_cm); // d8 - update_sector_data(90, d7_cm); // d7 - update_sector_data(135, d6_cm); // d6 - update_sector_data(180, d5_cm); // d5 - update_sector_data(225, d4_cm); // d4 - update_sector_data(270, d3_cm); // d3 - update_sector_data(315, d2_cm); // d2 - - gcs().send_text(MAV_SEVERITY_INFO,"update data\r\n"); + WITH_SEMAPHORE(_sem_proximity); + + update_sector_data(0, _interim_data.d1_cm); // d1 + update_sector_data(45, _interim_data.d8_cm); // d8 + update_sector_data(90, _interim_data.d7_cm); // d7 + update_sector_data(135, _interim_data.d6_cm); // d6 + update_sector_data(180, _interim_data.d5_cm); // d5 + update_sector_data(225, _interim_data.d4_cm); // d4 + update_sector_data(270, _interim_data.d3_cm); // d3 + update_sector_data(315, _interim_data.d2_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"); + // gcs().send_text(MAV_SEVERITY_INFO, "NoData\r\n"); } else { @@ -182,24 +159,45 @@ void AP_Proximity_UAVCAN::update(void) } } -void AP_Proximity_UAVCAN::handle_proximity_data(AP_UAVCAN *ap_uavcan, uint8_t node_id, const ProximityCb &cb) +void AP_Proximity_UAVCAN::handle_proximity_data_trampoline(AP_UAVCAN *ap_uavcan, uint8_t node_id, const ProximityCb &cb) { - // WITH_SEMAPHORE(_sem_registry); - AP_Proximity_UAVCAN *driver = get_uavcan_backend(ap_uavcan, node_id, true); + WITH_SEMAPHORE(_sem_registry); + + AP_Proximity_UAVCAN *driver = get_uavcan_backend(ap_uavcan); if (driver == nullptr) { - gcs().send_text(MAV_SEVERITY_INFO, "driver == nullptr\r\n"); return; } - // WITH_SEMAPHORE(driver->_sem_proximity); - driver->d1_cm = cb.msg->d0; - driver->d2_cm = cb.msg->d45; - driver->d3_cm = cb.msg->d90; - driver->d5_cm = cb.msg->d90; - driver->d6_cm = cb.msg->d90; - driver->d7_cm = cb.msg->d90; - driver->d8_cm = cb.msg->d90; - gcs().send_text(MAV_SEVERITY_INFO, "get_proximity_data\r\n"); + driver->handle_proximity_data(cb); +} + +void AP_Proximity_UAVCAN::handle_proximity_data(const ProximityCb &cb) +{ + + WITH_SEMAPHORE(_sem_proximity); + + _interim_data.d1_cm = handle_data_valid(cb.msg->d0); + _interim_data.d2_cm = handle_data_valid(cb.msg->d315); + _interim_data.d3_cm = handle_data_valid(cb.msg->d315); + _interim_data.d5_cm = handle_data_valid(cb.msg->d180); + _interim_data.d6_cm = handle_data_valid(cb.msg->d135); + _interim_data.d7_cm = handle_data_valid(cb.msg->d90); + _interim_data.d8_cm = handle_data_valid(cb.msg->d45); + + _last_distance_received_ms = AP_HAL::millis(); + // _last_sample_time_ms = AP_HAL::millis(); +} + +uint16_t AP_Proximity_UAVCAN::handle_data_valid(uint16_t data) +{ + if (data == 0xFFFF) + { + return 0; + } + else + { + return data / 10; + } } void AP_Proximity_UAVCAN::update_sector_data(int16_t angle_deg, uint16_t distance_cm) @@ -208,7 +206,7 @@ void AP_Proximity_UAVCAN::update_sector_data(int16_t angle_deg, uint16_t distanc _angle[sector] = angle_deg; _distance[sector] = ((float)distance_cm) / 100; _distance_valid[sector] = distance_cm != 0xffff; - _last_distance_received_ms = AP_HAL::millis(); + //_last_distance_received_ms = AP_HAL::millis(); // update boundary used for avoidance update_boundary_for_sector(sector, true); } diff --git a/libraries/AP_Proximity/AP_Proximity_UAVCAN.h b/libraries/AP_Proximity/AP_Proximity_UAVCAN.h index 661e8e2c71..12801ecafd 100644 --- a/libraries/AP_Proximity/AP_Proximity_UAVCAN.h +++ b/libraries/AP_Proximity/AP_Proximity_UAVCAN.h @@ -6,34 +6,39 @@ #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_UAVCAN *get_uavcan_backend(AP_UAVCAN *ap_uavcan, uint8_t node_id, bool create_new); - static bool detect(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state); - static void handle_proximity_data(AP_UAVCAN *ap_uavcan, uint8_t node_id, const ProximityCb &cb); - //uint8_t _instance; - AP_Proximity::Proximity_State _status; - AP_UAVCAN *_ap_uavcan; - uint8_t _node_id; - - inline void register_sensor() - { - // instance = frontend.register_sensor(); - } - // HAL_Semaphore _sem_proximity; - //bool new_data; - //MAV_DISTANCE_SENSOR _sensor_type; - uint32_t _last_distance_received_ms; + 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); + // 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; @@ -42,13 +47,19 @@ public: uint16_t d6_cm; uint16_t d7_cm; uint16_t d8_cm; - void update_sector_data(int16_t angle_deg, uint16_t distance_cm); - - static struct DetectedModules - { - AP_UAVCAN *ap_uavcan; - uint8_t node_id; - AP_Proximity_UAVCAN *driver; - } _detected_modules[PROXIMITY_MAX_INSTANCES]; + } _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; }; #endif //HAL_WITH_UAVCAN \ No newline at end of file