diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index b779d46ef7..44f0177f35 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -51,11 +51,11 @@ // Put your variable definitions into the UserVariables.h file (or another file name and then change the #define below). //#define USERHOOK_VARIABLES "UserVariables.h" // Put your custom code into the UserCode.cpp with function names matching those listed below and ensure the appropriate #define below is uncommented below -//#define USERHOOK_INIT userhook_init(); // for code to be run once at startup -//#define USERHOOK_FASTLOOP userhook_FastLoop(); // for code to be run at 100hz -//#define USERHOOK_50HZLOOP userhook_50Hz(); // for code to be run at 50hz -//#define USERHOOK_MEDIUMLOOP userhook_MediumLoop(); // for code to be run at 10hz -//#define USERHOOK_SLOWLOOP userhook_SlowLoop(); // for code to be run at 3.3hz -//#define USERHOOK_SUPERSLOWLOOP userhook_SuperSlowLoop(); // for code to be run at 1hz -//#define USERHOOK_AUXSWITCH ENABLED // for code to handle user aux switches +#define USERHOOK_INIT userhook_init(); // for code to be run once at startup +#define USERHOOK_FASTLOOP userhook_FastLoop(); // for code to be run at 100hz +#define USERHOOK_50HZLOOP userhook_50Hz(); // for code to be run at 50hz +#define USERHOOK_MEDIUMLOOP userhook_MediumLoop(); // for code to be run at 10hz +#define USERHOOK_SLOWLOOP userhook_SlowLoop(); // for code to be run at 3.3hz +#define USERHOOK_SUPERSLOWLOOP userhook_SuperSlowLoop(); // for code to be run at 1hz +// #define USERHOOK_AUXSWITCH ENABLED // for code to handle user aux switches //#define USER_PARAMS_ENABLED ENABLED // to enable user parameters diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index 174c386eb0..03c944d2ad 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_UAVCAN.h" #include @@ -345,6 +346,13 @@ void AP_Proximity::detect_instance(uint8_t instance) return; } break; + case Type::UAVCAN: +#if HAL_CANMANAGER_ENABLED + state[instance].instance = instance; + drivers[instance] = AP_Proximity_UAVCAN::probe(*this, state[instance]); + return; +#endif + break; case Type::CYGBOT_D1: #if AP_PROXIMITY_CYGBOT_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index d095ba9093..316fc0136f 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -51,6 +51,7 @@ public: TRTOWEREVO = 6, SF40C = 7, SF45B = 8, + UAVCAN = 9, #if CONFIG_HAL_BOARD == HAL_BOARD_SITL SITL = 10, AirSimSITL = 12, diff --git a/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp b/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp new file mode 100644 index 0000000000..26b4916cf6 --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp @@ -0,0 +1,250 @@ +#include + +#if HAL_CANMANAGER_ENABLED + +#include +#include "AP_Proximity_UAVCAN.h" +#include +#include + +#include +#define PRX_DEBUG 0 +#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) + { + Prx_Debug("ap_uavcan:nullptr" ); + return; + } + + Prx_Debug("ap_uavcan:subscribe_msgs"); + 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; + } + Prx_Debug("get_uavcan:%d",node_id); + 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 + + update_sector_data(0, _interim_data.d1_cm); // d1 + update_sector_data(45, _interim_data.d2_cm); // d8 + update_sector_data(315, _interim_data.d3_cm); // d7 + + 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) { + Prx_Debug("----nullptr : ap:%d,id:%d ----",(int)_ap_uavcan,_node_id); + + 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,addr:%d",(int)_ap_uavcan,_node_id,cb.msg->d0,cb.msg->sensor_id); + 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) +{ + // Get location on 3-D boundary based on angle to the object + const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle_deg); + if ((distance_cm != 0xffff) && !ignore_reading(angle_deg, distance_cm * 0.01f, false)) { + boundary.set_face_attributes(face, angle_deg, ((float) distance_cm) / 100); + // update OA database + database_push(angle_deg, ((float) distance_cm) / 100); + } else { + boundary.reset_face(face); + } + _last_distance_received_ms = AP_HAL::millis(); + + + /* + 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()); + 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..1b073e7ced --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_UAVCAN.h @@ -0,0 +1,72 @@ +#pragma once + +#include "AP_Proximity_Backend.h" + +#if HAL_CANMANAGER_ENABLED +#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 7bf18d5be5..cfd60f4f71 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -62,6 +62,7 @@ #include "AP_UAVCAN_pool.h" #include +#include #define LED_DELAY_US 50000 @@ -358,7 +359,8 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) #if HAL_EFI_ENABLED AP_EFI_DroneCAN::subscribe_msgs(this); #endif - + 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)); act_out_array[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest); 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..17d6271af0 --- /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