diff --git a/libraries/AP_Compass/AP_Compass_DroneCAN.cpp b/libraries/AP_Compass/AP_Compass_DroneCAN.cpp index 8563dd480b..3f29ec5706 100644 --- a/libraries/AP_Compass/AP_Compass_DroneCAN.cpp +++ b/libraries/AP_Compass/AP_Compass_DroneCAN.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include extern const AP_HAL::HAL& hal; @@ -48,6 +49,12 @@ void AP_Compass_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan) if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field_2, ap_dronecan->get_driver_index()) == nullptr) { AP_BoardConfig::allocation_error("mag2_sub"); } + +#if AP_COMPASS_DRONECAN_HIRES_ENABLED + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field_hires, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("mag3_sub"); + } +#endif } AP_Compass_Backend* AP_Compass_DroneCAN::probe(uint8_t index) @@ -198,6 +205,35 @@ void AP_Compass_DroneCAN::handle_magnetic_field_2(AP_DroneCAN *ap_dronecan, cons } } +#if AP_COMPASS_DRONECAN_HIRES_ENABLED +/* + just log hires magnetic field data for magnetic surveying + */ +void AP_Compass_DroneCAN::handle_magnetic_field_hires(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, + const dronecan_sensors_magnetometer_MagneticFieldStrengthHiRes &msg) +{ +// @LoggerMessage: MAGH +// @Description: Magnetometer high resolution data +// @Field: TimeUS: Time since system startup +// @Field: Node: CAN node +// @Field: Sensor: sensor ID on node +// @Field: Bus: CAN bus +// @Field: Mx: X axis field +// @Field: My: y axis field +// @Field: Mz: z axis field + + // just log it for now + AP::logger().WriteStreaming("MAGH", "TimeUS,Node,Sensor,Bus,Mx,My,Mz", "s#-----", "F-----", "QBBBfff", + transfer.timestamp_usec, + transfer.source_node_id, + ap_dronecan->get_driver_index(), + msg.sensor_id, + msg.magnetic_field_ga[0]*1000, + msg.magnetic_field_ga[1]*1000, + msg.magnetic_field_ga[2]*1000); +} +#endif + void AP_Compass_DroneCAN::read(void) { drain_accumulated_samples(_instance); diff --git a/libraries/AP_Compass/AP_Compass_DroneCAN.h b/libraries/AP_Compass/AP_Compass_DroneCAN.h index f72d799579..7a50f41406 100644 --- a/libraries/AP_Compass/AP_Compass_DroneCAN.h +++ b/libraries/AP_Compass/AP_Compass_DroneCAN.h @@ -19,6 +19,9 @@ public: 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(); diff --git a/libraries/AP_Compass/AP_Compass_config.h b/libraries/AP_Compass/AP_Compass_config.h index f032d0f9f8..c6c236db17 100644 --- a/libraries/AP_Compass/AP_Compass_config.h +++ b/libraries/AP_Compass/AP_Compass_config.h @@ -46,6 +46,10 @@ #define AP_COMPASS_DRONECAN_ENABLED (AP_COMPASS_BACKEND_DEFAULT_ENABLED && HAL_ENABLE_DRONECAN_DRIVERS) #endif +#ifndef AP_COMPASS_DRONECAN_HIRES_ENABLED +#define AP_COMPASS_DRONECAN_HIRES_ENABLED 0 +#endif + // i2c-based compasses: #ifndef AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED #define AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED AP_COMPASS_BACKEND_DEFAULT_ENABLED