Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 08:13:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: united enumeration on startup, multiple CAN drivers, correct dev_id based on network and node ID

Browse Source
This commit is contained in:
Eugene Shamaev 2017-05-06 12:13:19 +03:00 committed by Francisco Ferreira
parent 6e6efa7e1b
commit aa1f6a7587
3 changed files with 101 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_Compass/AP_Compass.cpp
View File

@ -20,6 +20,7 @@
#include "AP_Compass_AK09916.h"
#include "AP_Compass_QMC5883L.h"
#if HAL_WITH_UAVCAN
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
#include "AP_Compass_UAVCAN.h"
#endif
#include "AP_Compass_MMC3416.h"
@ -520,6 +521,16 @@ void Compass::_detect_backends(void)
return;
#endif
#if HAL_WITH_UAVCAN
bool added;
do {
added = _add_backend(AP_Compass_UAVCAN::probe(*this), "UAVCAN", true);
if (_backend_count == COMPASS_MAX_BACKEND || _compass_count == COMPASS_MAX_INSTANCES) {
return;
}
} while (added);
#endif
#if HAL_COMPASS_DEFAULT == HAL_COMPASS_HIL
ADD_BACKEND(AP_Compass_HIL::detect(*this), nullptr, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_PX4 || HAL_COMPASS_DEFAULT == HAL_COMPASS_VRBRAIN
@ -719,18 +730,6 @@ void Compass::_detect_backends(void)
#error Unrecognised HAL_COMPASS_TYPE setting
#endif
#if HAL_WITH_UAVCAN
if ((AP_BoardConfig::get_can_enable() != 0) && (hal.can_mgr != nullptr))
{
if((_backend_count < COMPASS_MAX_BACKEND) && (_compass_count < COMPASS_MAX_INSTANCES))
{
printf("Creating AP_Compass_UAVCAN\n\r");
_backends[_backend_count] = new AP_Compass_UAVCAN(*this);
_backend_count++;
}
}
#endif
if (_backend_count == 0 ||
_compass_count == 0) {
hal.console->printf("No Compass backends available\n");

129
libraries/AP_Compass/AP_Compass_UAVCAN.cpp
View File

@ -25,12 +25,11 @@
#include <unistd.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
extern const AP_HAL::HAL& hal;
#define debug_mag_uavcan(level, fmt, args...) do { if ((level) <= AP_BoardConfig::get_can_debug()) { hal.console->printf(fmt, ##args); }} while (0)
// There is limitation to use only one UAVCAN magnetometer now.
#define debug_mag_uavcan(level, fmt, args...) do { if ((level) <= AP_BoardConfig_CAN::get_can_debug()) { printf(fmt, ##args); }} while (0)
/*
constructor - registers instance at top Compass driver
@ -38,59 +37,97 @@ extern const AP_HAL::HAL& hal;
AP_Compass_UAVCAN::AP_Compass_UAVCAN(Compass &compass):
AP_Compass_Backend(compass)
{
if (hal.can_mgr != nullptr) {
AP_UAVCAN *ap_uavcan = hal.can_mgr->get_UAVCAN();
if (ap_uavcan != nullptr) {
// Give time to receive some packets from CAN if baro sensor is present
// This way it will get calibrated correctly
_instance = register_compass();
hal.scheduler->delay(1000);
uint8_t listener_channel = ap_uavcan->register_mag_listener(this, 1);
struct DeviceStructure {
uint8_t bus_type : 3;
uint8_t bus: 5;
uint8_t address;
uint8_t devtype;
};
union DeviceId {
struct DeviceStructure devid_s;
uint32_t devid;
};
union DeviceId d;
d.devid_s.bus_type = 3;
d.devid_s.bus = 0;
d.devid_s.address = listener_channel;
d.devid_s.devtype = 0;
set_dev_id(_instance, d.devid);
set_external(_instance, true);
_sum.zero();
_count = 0;
accumulate();
debug_mag_uavcan(2, "AP_Compass_UAVCAN loaded\n\r");
}
}
_mag_baro = hal.util->new_semaphore();
}
AP_Compass_UAVCAN::~AP_Compass_UAVCAN()
{
if (hal.can_mgr != nullptr) {
AP_UAVCAN *ap_uavcan = hal.can_mgr->get_UAVCAN();
if (ap_uavcan != nullptr) {
ap_uavcan->remove_mag_listener(this);
if (_initialized)
{
if (hal.can_mgr[_manager] != nullptr) {
AP_UAVCAN *ap_uavcan = hal.can_mgr[_manager]->get_UAVCAN();
if (ap_uavcan != nullptr) {
ap_uavcan->remove_mag_listener(this);
debug_mag_uavcan(2, "AP_Compass_UAVCAN destructed\n\r");
debug_mag_uavcan(2, "AP_Compass_UAVCAN destructed\n\r");
}
}
}
}
AP_Compass_Backend *AP_Compass_UAVCAN::probe(Compass &compass)
{
AP_Compass_UAVCAN *sensor = nullptr;
if (AP_BoardConfig_CAN::get_can_num_ifaces() != 0) {
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
if (hal.can_mgr[i] != nullptr) {
AP_UAVCAN *uavcan = hal.can_mgr[i]->get_UAVCAN();
if (uavcan != nullptr) {
uint8_t freemag = uavcan->find_smallest_free_mag_node();
if (freemag != UINT8_MAX) {
sensor = new AP_Compass_UAVCAN(compass);
if (sensor->register_uavcan_compass(i, freemag)) {
debug_mag_uavcan(2, "AP_Compass_UAVCAN probed, drv: %d, node: %d\n\r", i, freemag);
return sensor;
} else {
delete sensor;
sensor = nullptr;
}
}
}
}
}
}
return sensor;
}
bool AP_Compass_UAVCAN::register_uavcan_compass(uint8_t mgr, uint8_t node)
{
if (hal.can_mgr[mgr] != nullptr) {
AP_UAVCAN *ap_uavcan = hal.can_mgr[mgr]->get_UAVCAN();
if (ap_uavcan != nullptr) {
_manager = mgr;
if (ap_uavcan->register_mag_listener_to_node(this, node)) {
_instance = register_compass();
struct DeviceStructure {
uint8_t bus_type : 3;
uint8_t bus: 5;
uint8_t address;
uint8_t devtype;
};
union DeviceId {
struct DeviceStructure devid_s;
uint32_t devid;
};
union DeviceId d;
d.devid_s.bus_type = 3;
d.devid_s.bus = mgr;
d.devid_s.address = node;
d.devid_s.devtype = 1;
set_dev_id(_instance, d.devid);
set_external(_instance, true);
_sum.zero();
_count = 0;
accumulate();
debug_mag_uavcan(2, "AP_Compass_UAVCAN loaded\n\r");
return true;
}
}
}
return false;
}
void AP_Compass_UAVCAN::read(void)
{
// avoid division by zero if we haven't received any mag reports

7
libraries/AP_Compass/AP_Compass_UAVCAN.h
View File

@ -12,6 +12,10 @@ public:
AP_Compass_UAVCAN(Compass &compass);
~AP_Compass_UAVCAN() override;
static AP_Compass_Backend *probe(Compass &compass);
bool register_uavcan_compass(uint8_t mgr, uint8_t node);
// This method is called from UAVCAN thread
void handle_mag_msg(Vector3f &mag);
@ -22,5 +26,8 @@ private:
uint32_t _count;
uint64_t _last_timestamp;
bool _initialized;
uint8_t _manager;
AP_HAL::Semaphore *_mag_baro;
};