You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
72 lines
1.8 KiB
72 lines
1.8 KiB
#pragma once |
|
|
|
#include "AP_Proximity_Backend.h" |
|
|
|
#if HAL_CANMANAGER_ENABLED |
|
#include <AP_UAVCAN/AP_UAVCAN.h> |
|
|
|
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
|