mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
d86bab9c58
* remove unnecessary nullptr check, these are always called from an initialized AP_DroneCAN so if it's nullptr something has gone horrifically wrong * pass in driver index instead of repeatedly calling function to get it * simplify error handling; knowing exactly which allocation failed is not super helpful and one failing likely means subsequent ones will too, as it can only fail due to being out of memory
51 lines
1.6 KiB
C++
51 lines
1.6 KiB
C++
#pragma once
|
|
|
|
#include "AP_Compass.h"
|
|
|
|
#if AP_COMPASS_DRONECAN_ENABLED
|
|
|
|
#include "AP_Compass_Backend.h"
|
|
|
|
#include <AP_DroneCAN/AP_DroneCAN.h>
|
|
|
|
class AP_Compass_DroneCAN : public AP_Compass_Backend {
|
|
public:
|
|
AP_Compass_DroneCAN(AP_DroneCAN* ap_dronecan, uint32_t devid);
|
|
|
|
void read(void) override;
|
|
|
|
static bool 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);
|
|
#if AP_COMPASS_DRONECAN_HIRES_ENABLED
|
|
static void handle_magnetic_field_hires(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const dronecan_sensors_magnetometer_MagneticFieldStrengthHiRes &msg);
|
|
#endif
|
|
|
|
private:
|
|
bool init();
|
|
|
|
// callback for DroneCAN messages
|
|
void handle_mag_msg(const Vector3f &mag);
|
|
|
|
static AP_Compass_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t sensor_id);
|
|
|
|
uint8_t _instance;
|
|
|
|
uint32_t _devid;
|
|
|
|
// Module Detection Registry
|
|
static struct DetectedModules {
|
|
AP_DroneCAN* ap_dronecan;
|
|
uint8_t node_id;
|
|
uint8_t sensor_id;
|
|
AP_Compass_DroneCAN *driver;
|
|
uint32_t devid;
|
|
} _detected_modules[COMPASS_MAX_BACKEND];
|
|
|
|
static HAL_Semaphore _sem_registry;
|
|
};
|
|
|
|
#endif // AP_COMPASS_DRONECAN_ENABLED
|