zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

proximity uavcan 数据正常

zr_rover4.2
binsir 4 years ago
parent
51377db209
commit
b096f5ad9a
3 changed files with 145 additions and 136 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      libraries/AP_Proximity/AP_Proximity.cpp
  2. 202
      libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp
  3. 75
      libraries/AP_Proximity/AP_Proximity_UAVCAN.h

2
libraries/AP_Proximity/AP_Proximity.cpp
Unescape View File

@ -347,7 +347,7 @@ void AP_Proximity::detect_instance(uint8_t instance) @@ -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;
}
}

202
libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp
Unescape View File

@ -7,50 +7,25 @@ @@ -7,50 +7,25 @@
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
#include <AP_UAVCAN/AP_UAVCAN.h>
#include<zrzk/equipment/range_sensor/Proximity.hpp>
#include <zrzk/equipment/range_sensor/Proximity.hpp>
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) @@ -65,7 +40,7 @@ void AP_Proximity_UAVCAN::subscribe_msgs(AP_UAVCAN *ap_uavcan)
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));
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) @@ -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, @@ -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, @@ -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 @@ -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) @@ -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 @@ -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);
}

75
libraries/AP_Proximity/AP_Proximity_UAVCAN.h
Unescape View File

@ -6,34 +6,39 @@ @@ -6,34 +6,39 @@
#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_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: @@ -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
Write Preview
Loading…
Cancel
Save