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

增加proximity uavcan 

AP_Proximity,增加uavcan类型=9
AP_Proximity_UAVCAN,uavcan驱动
dsdl,增加range_sensor
apm_2208
Brown.Z 3 years ago
parent
16b69e95b1
commit
050d98eb0e
7 changed files with 364 additions and 8 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. 12
      ArduCopter/APM_Config.h
  2. 8
      libraries/AP_Proximity/AP_Proximity.cpp
  3. 1
      libraries/AP_Proximity/AP_Proximity.h
  4. 250
      libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp
  5. 72
      libraries/AP_Proximity/AP_Proximity_UAVCAN.h
  6. 2
      libraries/AP_UAVCAN/AP_UAVCAN.cpp
  7. 23
      libraries/AP_UAVCAN/dsdl/zrzk/equipment/range_sensor/26110.Proximity.uavcan

12
ArduCopter/APM_Config.h
Unescape View File

@ -51,11 +51,11 @@ @@ -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_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

8
libraries/AP_Proximity/AP_Proximity.cpp
Unescape View File

@ -26,6 +26,7 @@ @@ -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 <AP_Logger/AP_Logger.h>
@ -345,6 +346,13 @@ void AP_Proximity::detect_instance(uint8_t instance) @@ -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

1
libraries/AP_Proximity/AP_Proximity.h
Unescape View File

@ -51,6 +51,7 @@ public: @@ -51,6 +51,7 @@ public:
TRTOWEREVO = 6,
SF40C = 7,
SF45B = 8,
UAVCAN = 9,
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
SITL = 10,
AirSimSITL = 12,

250
libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp
Unescape View File

@ -0,0 +1,250 @@ @@ -0,0 +1,250 @@
#include <AP_HAL/AP_HAL.h>
#if HAL_CANMANAGER_ENABLED
#include <GCS_MAVLink/GCS.h>
#include "AP_Proximity_UAVCAN.h"
#include <AP_CANManager/AP_CANManager.h>
#include <AP_UAVCAN/AP_UAVCAN.h>
#include <zrzk/equipment/range_sensor/Proximity.hpp>
#define PRX_DEBUG 0
#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)
{
Prx_Debug("ap_uavcan:nullptr" );
return;
}
Prx_Debug("ap_uavcan:subscribe_msgs");
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;
}
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

72
libraries/AP_Proximity/AP_Proximity_UAVCAN.h
Unescape View File

@ -0,0 +1,72 @@ @@ -0,0 +1,72 @@
#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

2
libraries/AP_UAVCAN/AP_UAVCAN.cpp
Unescape View File

@ -62,6 +62,7 @@ @@ -62,6 +62,7 @@
#include "AP_UAVCAN_pool.h"
#include <zrzk/equipment/indication/ZrRGB.hpp>
#include <AP_Proximity/AP_Proximity_UAVCAN.h>
#define LED_DELAY_US 50000
@ -358,6 +359,7 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) @@ -358,6 +359,7 @@ 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<uavcan::equipment::actuator::ArrayCommand>(*_node);
act_out_array[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2));

23
libraries/AP_UAVCAN/dsdl/zrzk/equipment/range_sensor/26110.Proximity.uavcan
Unescape View File

@ -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
Write Preview
Loading…
Cancel
Save