Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include "AP_Compass.h"
       4             : 
       5             : #if AP_COMPASS_DRONECAN_ENABLED
       6             : 
       7             : #include "AP_Compass_Backend.h"
       8             : 
       9             : #include <AP_DroneCAN/AP_DroneCAN.h>
      10             : 
      11             : class AP_Compass_DroneCAN : public AP_Compass_Backend {
      12             : public:
      13             :     AP_Compass_DroneCAN(AP_DroneCAN* ap_dronecan, uint32_t devid);
      14             : 
      15             :     static bool subscribe_msgs(AP_DroneCAN* ap_dronecan);
      16             :     static AP_Compass_Backend* probe(uint8_t index);
      17           0 :     static uint32_t get_detected_devid(uint8_t index) { return _detected_modules[index].devid; }
      18             :     static void handle_magnetic_field(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_ahrs_MagneticFieldStrength& msg);
      19             :     static void handle_magnetic_field_2(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_ahrs_MagneticFieldStrength2 &msg);
      20             : #if AP_COMPASS_DRONECAN_HIRES_ENABLED
      21             :     static void handle_magnetic_field_hires(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const dronecan_sensors_magnetometer_MagneticFieldStrengthHiRes &msg);
      22             : #endif
      23             : 
      24             : private:
      25             :     bool init();
      26             : 
      27             :     // callback for DroneCAN messages
      28             :     void handle_mag_msg(const Vector3f &mag);
      29             : 
      30             :     static AP_Compass_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t sensor_id);
      31             : 
      32             :     uint8_t  _instance;
      33             : 
      34             :     uint32_t _devid;
      35             : 
      36             :     // Module Detection Registry
      37             :     static struct DetectedModules {
      38             :         AP_DroneCAN* ap_dronecan;
      39             :         uint8_t node_id;
      40             :         uint8_t sensor_id;
      41             :         AP_Compass_DroneCAN *driver;
      42             :         uint32_t devid;
      43             :     } _detected_modules[MAX_CONNECTED_MAGS];
      44             : 
      45             :     static HAL_Semaphore _sem_registry;
      46             : };
      47             : 
      48             : #endif  // AP_COMPASS_DRONECAN_ENABLED