Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: log Hires magnetic field on DroneCAN

This commit is contained in:
Andrew Tridgell 2024-03-03 09:04:32 +11:00
parent 9aade22c7e
commit 76343f5e6f
3 changed files with 43 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AP_Compass/AP_Compass_DroneCAN.cpp
View File

@ -22,6 +22,7 @@
#include <AP_CANManager/AP_CANManager.h>
#include <AP_DroneCAN/AP_DroneCAN.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_Logger/AP_Logger.h>
#include <SITL/SITL.h>
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);

3
libraries/AP_Compass/AP_Compass_DroneCAN.h
View File

@ -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();

4
libraries/AP_Compass/AP_Compass_config.h
View File

@ -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