binsir
9 changed files with 361 additions and 7 deletions
@ -0,0 +1,232 @@
@@ -0,0 +1,232 @@
|
||||
#include <AP_HAL/AP_HAL.h> |
||||
|
||||
#if HAL_WITH_UAVCAN |
||||
|
||||
#include "AP_Proximity_UAVCAN.h" |
||||
#include <GCS_MAVLink/GCS.h> |
||||
#include <AP_BoardConfig/AP_BoardConfig_CAN.h> |
||||
#include <AP_UAVCAN/AP_UAVCAN.h> |
||||
|
||||
#include <zrzk/equipment/range_sensor/Proximity.hpp> |
||||
#define PRX_DEBUG 1 |
||||
#if PRX_DEBUG |
||||
#include <GCS_MAVLink/GCS.h> |
||||
# 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<zrzk::equipment::range_sensor::Proximity, ProximityCb> *proximity_listener; |
||||
proximity_listener = new uavcan::Subscriber<zrzk::equipment::range_sensor::Proximity, ProximityCb>(*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 |
||||
@ -0,0 +1,72 @@
@@ -0,0 +1,72 @@
|
||||
#pragma once |
||||
|
||||
#include "AP_Proximity_Backend.h" |
||||
|
||||
#if HAL_WITH_UAVCAN |
||||
#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
|
||||
@ -0,0 +1,23 @@
@@ -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 |
||||
|
||||
Loading…
Reference in new issue