mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-22 08:38:36 -04:00
AP_Compass: rename UAVCAN drivers to DroneCAN
This commit is contained in:
Andrew Tridgell
parent
63e67db516
commit
2e8ea6ac47
208
libraries/AP_Compass/AP_Compass_DroneCAN.cpp
Normal file
208
libraries/AP_Compass/AP_Compass_DroneCAN.cpp
Normal file
@ -0,0 +1,208 @@
|
||||
/*
|
||||
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_Compass_UAVCAN.h"
|
||||
|
||||
#if AP_COMPASS_UAVCAN_ENABLED
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#include <AP_CANManager/AP_CANManager.h>
|
||||
#include <AP_DroneCAN/AP_DroneCAN.h>
|
||||
#include <AP_BoardConfig/AP_BoardConfig.h>
|
||||
#include <SITL/SITL.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
#define LOG_TAG "COMPASS"
|
||||
|
||||
AP_Compass_UAVCAN::DetectedModules AP_Compass_UAVCAN::_detected_modules[];
|
||||
HAL_Semaphore AP_Compass_UAVCAN::_sem_registry;
|
||||
|
||||
AP_Compass_UAVCAN::AP_Compass_UAVCAN(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t sensor_id, uint32_t devid)
|
||||
: _ap_dronecan(ap_dronecan)
|
||||
, _node_id(node_id)
|
||||
, _sensor_id(sensor_id)
|
||||
, _devid(devid)
|
||||
{
|
||||
}
|
||||
|
||||
void AP_Compass_UAVCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
|
||||
{
|
||||
if (ap_dronecan == nullptr) {
|
||||
return;
|
||||
}
|
||||
if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field, ap_dronecan->get_driver_index()) == nullptr) {
|
||||
AP_BoardConfig::allocation_error("mag_sub");
|
||||
}
|
||||
|
||||
if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field_2, ap_dronecan->get_driver_index()) == nullptr) {
|
||||
AP_BoardConfig::allocation_error("mag2_sub");
|
||||
}
|
||||
}
|
||||
|
||||
AP_Compass_Backend* AP_Compass_UAVCAN::probe(uint8_t index)
|
||||
{
|
||||
AP_Compass_UAVCAN* driver = nullptr;
|
||||
if (!_detected_modules[index].driver && _detected_modules[index].ap_dronecan) {
|
||||
WITH_SEMAPHORE(_sem_registry);
|
||||
// Register new Compass mode to a backend
|
||||
driver = new AP_Compass_UAVCAN(_detected_modules[index].ap_dronecan, _detected_modules[index].node_id, _detected_modules[index].sensor_id, _detected_modules[index].devid);
|
||||
if (driver) {
|
||||
if (!driver->init()) {
|
||||
delete driver;
|
||||
return nullptr;
|
||||
}
|
||||
_detected_modules[index].driver = driver;
|
||||
AP::can().log_text(AP_CANManager::LOG_INFO,
|
||||
LOG_TAG,
|
||||
"Found Mag Node %d on Bus %d Sensor ID %d\n",
|
||||
_detected_modules[index].node_id,
|
||||
_detected_modules[index].ap_dronecan->get_driver_index(),
|
||||
_detected_modules[index].sensor_id);
|
||||
#if AP_TEST_DRONECAN_DRIVERS
|
||||
// Scroll through the registered compasses, and set the offsets
|
||||
if (driver->_compass.get_offsets(index).is_zero()) {
|
||||
driver->_compass.set_offsets(index, AP::sitl()->mag_ofs[index]);
|
||||
}
|
||||
|
||||
// we want to simulate a calibrated compass by default, so set
|
||||
// scale to 1
|
||||
AP_Param::set_default_by_name("COMPASS_SCALE", 1);
|
||||
AP_Param::set_default_by_name("COMPASS_SCALE2", 1);
|
||||
AP_Param::set_default_by_name("COMPASS_SCALE3", 1);
|
||||
driver->save_dev_id(index);
|
||||
driver->set_rotation(index, ROTATION_NONE);
|
||||
|
||||
// make first compass external
|
||||
driver->set_external(index, true);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
return driver;
|
||||
}
|
||||
|
||||
bool AP_Compass_UAVCAN::init()
|
||||
{
|
||||
// Adding 1 is necessary to allow backward compatibilty, where this field was set as 1 by default
|
||||
if (!register_compass(_devid, _instance)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
set_dev_id(_instance, _devid);
|
||||
set_external(_instance, true);
|
||||
|
||||
AP::can().log_text(AP_CANManager::LOG_INFO, LOG_TAG, "AP_Compass_UAVCAN loaded\n\r");
|
||||
return true;
|
||||
}
|
||||
|
||||
AP_Compass_UAVCAN* AP_Compass_UAVCAN::get_uavcan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t sensor_id)
|
||||
{
|
||||
if (ap_dronecan == nullptr) {
|
||||
return nullptr;
|
||||
}
|
||||
for (uint8_t i=0; i<COMPASS_MAX_BACKEND; i++) {
|
||||
if (_detected_modules[i].driver &&
|
||||
_detected_modules[i].ap_dronecan == ap_dronecan &&
|
||||
_detected_modules[i].node_id == node_id &&
|
||||
_detected_modules[i].sensor_id == sensor_id) {
|
||||
return _detected_modules[i].driver;
|
||||
}
|
||||
}
|
||||
|
||||
bool already_detected = false;
|
||||
// Check if there's an empty spot for possible registeration
|
||||
for (uint8_t i = 0; i < COMPASS_MAX_BACKEND; i++) {
|
||||
if (_detected_modules[i].ap_dronecan == ap_dronecan &&
|
||||
_detected_modules[i].node_id == node_id &&
|
||||
_detected_modules[i].sensor_id == sensor_id) {
|
||||
// Already Detected
|
||||
already_detected = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!already_detected) {
|
||||
for (uint8_t i = 0; i < COMPASS_MAX_BACKEND; i++) {
|
||||
if (nullptr == _detected_modules[i].ap_dronecan) {
|
||||
_detected_modules[i].ap_dronecan = ap_dronecan;
|
||||
_detected_modules[i].node_id = node_id;
|
||||
_detected_modules[i].sensor_id = sensor_id;
|
||||
_detected_modules[i].devid = AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_UAVCAN,
|
||||
ap_dronecan->get_driver_index(),
|
||||
node_id,
|
||||
sensor_id + 1); // we use sensor_id as devtype
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
struct DetectedModules tempslot;
|
||||
// Sort based on the node_id, larger values first
|
||||
// we do this, so that we have repeatable compass
|
||||
// registration, especially in cases of extraneous
|
||||
// CAN compass is connected.
|
||||
for (uint8_t i = 1; i < COMPASS_MAX_BACKEND; i++) {
|
||||
for (uint8_t j = i; j > 0; j--) {
|
||||
if (_detected_modules[j].node_id > _detected_modules[j-1].node_id) {
|
||||
tempslot = _detected_modules[j];
|
||||
_detected_modules[j] = _detected_modules[j-1];
|
||||
_detected_modules[j-1] = tempslot;
|
||||
}
|
||||
}
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
void AP_Compass_UAVCAN::handle_mag_msg(const Vector3f &mag)
|
||||
{
|
||||
Vector3f raw_field = mag * 1000.0;
|
||||
|
||||
accumulate_sample(raw_field, _instance);
|
||||
}
|
||||
|
||||
void AP_Compass_UAVCAN::handle_magnetic_field(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_ahrs_MagneticFieldStrength& msg)
|
||||
{
|
||||
WITH_SEMAPHORE(_sem_registry);
|
||||
|
||||
Vector3f mag_vector;
|
||||
AP_Compass_UAVCAN* driver = get_uavcan_backend(ap_dronecan, transfer.source_node_id, 0);
|
||||
if (driver != nullptr) {
|
||||
mag_vector[0] = msg.magnetic_field_ga[0];
|
||||
mag_vector[1] = msg.magnetic_field_ga[1];
|
||||
mag_vector[2] = msg.magnetic_field_ga[2];
|
||||
driver->handle_mag_msg(mag_vector);
|
||||
}
|
||||
}
|
||||
|
||||
void AP_Compass_UAVCAN::handle_magnetic_field_2(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_ahrs_MagneticFieldStrength2 &msg)
|
||||
{
|
||||
WITH_SEMAPHORE(_sem_registry);
|
||||
|
||||
Vector3f mag_vector;
|
||||
uint8_t sensor_id = msg.sensor_id;
|
||||
AP_Compass_UAVCAN* driver = get_uavcan_backend(ap_dronecan, transfer.source_node_id, sensor_id);
|
||||
if (driver != nullptr) {
|
||||
mag_vector[0] = msg.magnetic_field_ga[0];
|
||||
mag_vector[1] = msg.magnetic_field_ga[1];
|
||||
mag_vector[2] = msg.magnetic_field_ga[2];
|
||||
driver->handle_mag_msg(mag_vector);
|
||||
}
|
||||
}
|
||||
|
||||
void AP_Compass_UAVCAN::read(void)
|
||||
{
|
||||
drain_accumulated_samples(_instance);
|
||||
}
|
||||
#endif // AP_COMPASS_UAVCAN_ENABLED
|
||||
50
libraries/AP_Compass/AP_Compass_DroneCAN.h
Normal file
50
libraries/AP_Compass/AP_Compass_DroneCAN.h
Normal file
@ -0,0 +1,50 @@
|
||||
#pragma once
|
||||
|
||||
#include "AP_Compass.h"
|
||||
|
||||
#if AP_COMPASS_UAVCAN_ENABLED
|
||||
|
||||
#include "AP_Compass_Backend.h"
|
||||
|
||||
#include <AP_DroneCAN/AP_DroneCAN.h>
|
||||
|
||||
class AP_Compass_UAVCAN : public AP_Compass_Backend {
|
||||
public:
|
||||
AP_Compass_UAVCAN(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t sensor_id, uint32_t devid);
|
||||
|
||||
void read(void) override;
|
||||
|
||||
static void subscribe_msgs(AP_DroneCAN* ap_dronecan);
|
||||
static AP_Compass_Backend* probe(uint8_t index);
|
||||
static uint32_t get_detected_devid(uint8_t index) { return _detected_modules[index].devid; }
|
||||
static void handle_magnetic_field(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_ahrs_MagneticFieldStrength& msg);
|
||||
static void handle_magnetic_field_2(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_ahrs_MagneticFieldStrength2 &msg);
|
||||
|
||||
private:
|
||||
bool init();
|
||||
|
||||
// callback for DroneCAN messages
|
||||
void handle_mag_msg(const Vector3f &mag);
|
||||
|
||||
static AP_Compass_UAVCAN* get_uavcan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t sensor_id);
|
||||
|
||||
uint8_t _instance;
|
||||
|
||||
AP_DroneCAN* _ap_dronecan;
|
||||
uint8_t _node_id;
|
||||
uint8_t _sensor_id;
|
||||
uint32_t _devid;
|
||||
|
||||
// Module Detection Registry
|
||||
static struct DetectedModules {
|
||||
AP_DroneCAN* ap_dronecan;
|
||||
uint8_t node_id;
|
||||
uint8_t sensor_id;
|
||||
AP_Compass_UAVCAN *driver;
|
||||
uint32_t devid;
|
||||
} _detected_modules[COMPASS_MAX_BACKEND];
|
||||
|
||||
static HAL_Semaphore _sem_registry;
|
||||
};
|
||||
|
||||
#endif // AP_COMPASS_UAVCAN_ENABLED
|
||||
Loading…
Reference in New Issue
Block a user