ardupilot/libraries/AP_Compass/AP_Compass_UAVCAN.cpp

173 lines
4.8 KiB
C++

/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#if HAL_WITH_UAVCAN
#include "AP_Compass_UAVCAN.h"
#if HAL_OS_POSIX_IO
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#endif
#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_CAN::get_can_debug()) { printf(fmt, ##args); }} while (0)
/*
constructor - registers instance at top Compass driver
*/
AP_Compass_UAVCAN::AP_Compass_UAVCAN(Compass &compass):
AP_Compass_Backend(compass)
{
_mag_baro = hal.util->new_semaphore();
}
AP_Compass_UAVCAN::~AP_Compass_UAVCAN()
{
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");
}
}
}
}
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
if (_count == 0) {
return;
}
if (_mag_baro->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
_sum /= _count;
publish_filtered_field(_sum, _instance);
_sum.zero();
_count = 0;
_mag_baro->give();
}
}
void AP_Compass_UAVCAN::handle_mag_msg(Vector3f &mag)
{
Vector3f raw_field = mag * 1000.0;
// rotate raw_field from sensor frame to body frame
rotate_field(raw_field, _instance);
// publish raw_field (uncorrected point sample) for calibration use
publish_raw_field(raw_field, _instance);
// correct raw_field for known errors
correct_field(raw_field, _instance);
if (_mag_baro->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
// accumulate into averaging filter
_sum += raw_field;
_count++;
_mag_baro->give();
}
}
#endif