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

Merge branch 'zr_rover4.1' of gitee.com:xmzrzk/zr-v4 into zr_rover4.2

zr_rover4.2
zbr 4 years ago
parent
4ee35bd819 9c93cb7620
commit
537e9bde52
3 changed files with 43 additions and 37 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 11
      libraries/AP_Proximity/AP_Proximity.cpp
  2. 67
      libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp
  3. 2
      libraries/AP_Proximity/AP_Proximity_UAVCAN.h

11
libraries/AP_Proximity/AP_Proximity.cpp
Unescape View File

@ -328,7 +328,12 @@ void AP_Proximity::detect_instance(uint8_t instance)
return; return;
} }
break; break;
#if HAL_WITH_UAVCAN
case Type::UAVCAN:
state[instance].instance = instance;
drivers[instance] = AP_Proximity_UAVCAN::probe(*this, state[instance]);
return;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case Type::SITL: case Type::SITL:
state[instance].instance = instance; state[instance].instance = instance;
@ -345,10 +350,6 @@ void AP_Proximity::detect_instance(uint8_t instance)
drivers[instance] = new AP_Proximity_AirSimSITL(*this, state[instance]); drivers[instance] = new AP_Proximity_AirSimSITL(*this, state[instance]);
return; return;
#endif #endif
case Type::UAVCAN:
state[instance].instance = instance;
drivers[instance] = AP_Proximity_UAVCAN::probe(*this, state[instance]);
return;
} }
} }

67
libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp
Unescape View File

@ -24,7 +24,9 @@ HAL_Semaphore AP_Proximity_UAVCAN::_sem_registry;
UC_REGISTRY_BINDER(ProximityCb, zrzk::equipment::range_sensor::Proximity); UC_REGISTRY_BINDER(ProximityCb, zrzk::equipment::range_sensor::Proximity);
AP_Proximity_UAVCAN::DetectedModules AP_Proximity_UAVCAN::_detected_modules[] = {0}; 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)
{ {
} }
@ -83,7 +85,7 @@ AP_Proximity_Backend *AP_Proximity_UAVCAN::probe(AP_Proximity &_frontend,
return backend; return backend;
} }
AP_Proximity_UAVCAN *AP_Proximity_UAVCAN::get_uavcan_backend(AP_UAVCAN *ap_uavcan) AP_Proximity_UAVCAN *AP_Proximity_UAVCAN::get_uavcan_backend(AP_UAVCAN *ap_uavcan,uint8_t node_id)
{ {
if (ap_uavcan == nullptr) if (ap_uavcan == nullptr)
{ {
@ -93,7 +95,8 @@ AP_Proximity_UAVCAN *AP_Proximity_UAVCAN::get_uavcan_backend(AP_UAVCAN *ap_uavca
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 && if (_detected_modules[i].driver != nullptr &&
_detected_modules[i].ap_uavcan == ap_uavcan) _detected_modules[i].ap_uavcan == ap_uavcan&&
_detected_modules[i].node_id == node_id)
{ {
return _detected_modules[i].driver; return _detected_modules[i].driver;
} }
@ -102,7 +105,8 @@ AP_Proximity_UAVCAN *AP_Proximity_UAVCAN::get_uavcan_backend(AP_UAVCAN *ap_uavca
bool detected = false; bool detected = false;
for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++) for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++)
{ {
if (_detected_modules[i].ap_uavcan == ap_uavcan) if (_detected_modules[i].ap_uavcan == ap_uavcan &&
_detected_modules[i].node_id == node_id)
{ {
// detected // detected
detected = true; detected = true;
@ -117,6 +121,7 @@ AP_Proximity_UAVCAN *AP_Proximity_UAVCAN::get_uavcan_backend(AP_UAVCAN *ap_uavca
if (_detected_modules[i].ap_uavcan == nullptr) if (_detected_modules[i].ap_uavcan == nullptr)
{ {
_detected_modules[i].ap_uavcan = ap_uavcan; _detected_modules[i].ap_uavcan = ap_uavcan;
_detected_modules[i].node_id = node_id;
break; break;
} }
} }
@ -140,13 +145,13 @@ void AP_Proximity_UAVCAN::update(void)
WITH_SEMAPHORE(_sem_proximity); WITH_SEMAPHORE(_sem_proximity);
update_sector_data(0, _interim_data.d1_cm); // d1 update_sector_data(0, _interim_data.d1_cm); // d1
update_sector_data(45, _interim_data.d8_cm); // d8 update_sector_data(45, _interim_data.d2_cm); // d8
update_sector_data(90, _interim_data.d7_cm); // d7 update_sector_data(90, _interim_data.d3_cm); // d7
update_sector_data(135, _interim_data.d6_cm); // d6 update_sector_data(135, _interim_data.d4_cm); // d6
update_sector_data(180, _interim_data.d5_cm); // d5 update_sector_data(180, _interim_data.d5_cm); // d5
update_sector_data(225, _interim_data.d4_cm); // d4 update_sector_data(225, _interim_data.d6_cm); // d4
update_sector_data(270, _interim_data.d3_cm); // d3 update_sector_data(270, _interim_data.d7_cm); // d3
update_sector_data(315, _interim_data.d2_cm); // d2 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)) if ((_last_distance_received_ms == 0) || (AP_HAL::millis() - _last_distance_received_ms > PROXIMITY_UAVCAN_TIMEOUT_MS))
{ {
@ -163,7 +168,7 @@ void AP_Proximity_UAVCAN::handle_proximity_data_trampoline(AP_UAVCAN *ap_uavcan,
{ {
WITH_SEMAPHORE(_sem_registry); WITH_SEMAPHORE(_sem_registry);
AP_Proximity_UAVCAN *driver = get_uavcan_backend(ap_uavcan); AP_Proximity_UAVCAN *driver = get_uavcan_backend(ap_uavcan,node_id);
if (driver == nullptr) if (driver == nullptr)
{ {
return; return;
@ -176,35 +181,35 @@ void AP_Proximity_UAVCAN::handle_proximity_data(const ProximityCb &cb)
WITH_SEMAPHORE(_sem_proximity); WITH_SEMAPHORE(_sem_proximity);
_interim_data.d1_cm = handle_data_valid(cb.msg->d0); _interim_data.d1_cm = cb.msg->d0;
_interim_data.d2_cm = handle_data_valid(cb.msg->d315); _interim_data.d2_cm = cb.msg->d45;
_interim_data.d3_cm = handle_data_valid(cb.msg->d315); _interim_data.d3_cm = cb.msg->d90;
_interim_data.d5_cm = handle_data_valid(cb.msg->d180); _interim_data.d4_cm = cb.msg->d135;
_interim_data.d6_cm = handle_data_valid(cb.msg->d135); _interim_data.d5_cm = cb.msg->d180;
_interim_data.d7_cm = handle_data_valid(cb.msg->d90); _interim_data.d6_cm = cb.msg->d225;
_interim_data.d8_cm = handle_data_valid(cb.msg->d45); _interim_data.d7_cm = cb.msg->d270;
_interim_data.d8_cm = cb.msg->d315;
_last_distance_received_ms = AP_HAL::millis(); _last_distance_received_ms = AP_HAL::millis();
// _last_sample_time_ms = AP_HAL::millis(); // _last_sample_time_ms = AP_HAL::millis();
} }
uint16_t AP_Proximity_UAVCAN::handle_data_valid(uint16_t data) // uint16_t AP_Proximity_UAVCAN::handle_data_valid(uint16_t data)
{ // {
if (data == 0xFFFF) // if (data == 0xFFFF)
{ // {
return 0; // return 0;
} // }
else // else
{ // {
return data / 10; // return data / 10;
} // }
} // }
void AP_Proximity_UAVCAN::update_sector_data(int16_t angle_deg, uint16_t distance_cm) void AP_Proximity_UAVCAN::update_sector_data(int16_t angle_deg, uint16_t distance_cm)
{ {
const uint8_t sector = convert_angle_to_sector(angle_deg); const uint8_t sector = convert_angle_to_sector(angle_deg);
_angle[sector] = angle_deg; _angle[sector] = angle_deg;
_distance[sector] = ((float)distance_cm) / 100; _distance[sector] = ((float)distance_cm) / 1000;
_distance_valid[sector] = distance_cm != 0xffff; _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 used for avoidance

2
libraries/AP_Proximity/AP_Proximity_UAVCAN.h
Unescape View File

@ -29,7 +29,7 @@ public:
//AP_Proximity::Proximity_State _status; //AP_Proximity::Proximity_State _status;
private: private:
static AP_Proximity_UAVCAN *get_uavcan_backend(AP_UAVCAN *ap_uavcan); static AP_Proximity_UAVCAN *get_uavcan_backend(AP_UAVCAN *ap_uavcan,uint8_t node_id);
// inline void register_sensor() // inline void register_sensor()
// { // {
// // instance = frontend.register_sensor(); // // instance = frontend.register_sensor();

Write Preview
Loading…
Cancel
Save