Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

[mag] add a MAG_TO_IMU option to HMC58XX drivers (#1755)

Browse Source 
this allows to choose arbitrary orientation for external mounted hmc mag
and move data back to IMU frame before sending them over ABI.
This commit is contained in:
Gautier Hattenberger
2016-06-21 18:06:54 +02:00
committed by Felix Ruess
parent ede3a2d722
commit 017ce18977
2 changed files with 32 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/mag_hmc58xx.xml
+6
View File
@@ -5,6 +5,7 @@
<description>
HMC58xx magnetometer.
Module for standalone operation/logging of a HMC58xx magnetometer.
An arbitrary rotation between the sensor frame and the IMU frame can be compensated with a MAG_TO_IMU rotation (defined by three euler angles). The three angles must be defined to enable this correction. Otherwise it is assumed that the axis are aligned.
</description>
<configure name="MAG_HMC58XX_I2C_DEV" value="i2c1" description="I2C device to use (e.g. i2c1)"/>
<define name="MODULE_HMC58XX_SYNC_SEND" value="TRUE|FALSE" description="Send IMU_RAW message with each new measurement (default: FALSE)"/>
@@ -15,6 +16,11 @@
<define name="HMC58XX_CHAN_X" value="0|1|2" description="Channel id of x axis (default: 0)"/>
<define name="HMC58XX_CHAN_Y" value="0|1|2" description="Channel id of y axis (default: 1)"/>
<define name="HMC58XX_CHAN_Z" value="0|1|2" description="Channel id of z axis (default: 2)"/>
<section name="MAG_HMC" prefix="HMC58XX_">
<define name="MAG_TO_IMU_PHI" value="0.0" description="Rotation between sensor frame and IMU frame (phi angle)"/>
<define name="MAG_TO_IMU_THETA" value="0.0" description="Rotation between sensor frame and IMU frame (theta angle)"/>
<define name="MAG_TO_IMU_PSI" value="0.0" description="Rotation between sensor frame and IMU frame (psi angle)"/>
</section>
</doc>
<header>
<file name="mag_hmc58xx.h"/>
sw/airborne/modules/sensors/mag_hmc58xx.c
+26
View File
@@ -29,6 +29,7 @@
#include "mcu_periph/uart.h"
#include "pprzlink/messages.h"
#include "subsystems/datalink/downlink.h"
#include "generated/airframe.h"
#ifndef HMC58XX_CHAN_X
#define HMC58XX_CHAN_X 0
@@ -52,6 +53,13 @@
#if MODULE_HMC58XX_UPDATE_AHRS
#include "subsystems/imu.h"
#include "subsystems/abi.h"
#if defined HMC58XX_MAG_TO_IMU_PHI && defined HMC58XX_MAG_TO_IMU_THETA && defined HMC58XX_MAG_TO_IMU_PSI
#define USE_MAG_TO_IMU 1
static struct Int32RMat mag_to_imu; ///< rotation from mag to imu frame
#else
#define USE_MAG_TO_IMU 0
#endif
#endif
struct Hmc58xx mag_hmc58xx;
@@ -59,6 +67,15 @@ struct Hmc58xx mag_hmc58xx;
void mag_hmc58xx_module_init(void)
{
hmc58xx_init(&mag_hmc58xx, &(MAG_HMC58XX_I2C_DEV), HMC58XX_ADDR);
#if MODULE_HMC58XX_UPDATE_AHRS && USE_MAG_TO_IMU
struct Int32Eulers mag_to_imu_eulers = {
ANGLE_BFP_OF_REAL(HMC58XX_MAG_TO_IMU_PHI),
ANGLE_BFP_OF_REAL(HMC58XX_MAG_TO_IMU_THETA),
ANGLE_BFP_OF_REAL(HMC58XX_MAG_TO_IMU_PSI)
};
int32_rmat_of_eulers(&mag_to_imu, &mag_to_imu_eulers);
#endif
}
void mag_hmc58xx_module_periodic(void)
@@ -81,8 +98,17 @@ void mag_hmc58xx_module_event(void)
HMC58XX_CHAN_Y_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Y]),
HMC58XX_CHAN_Z_SIGN(int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Z])
};
// only rotate if needed
#if USE_MAG_TO_IMU
struct Int32Vect3 imu_mag;
// rotate data from mag frame to imu frame
int32_rmat_vmult(&imu_mag, &mag_to_imu, &mag);
// unscaled vector
VECT3_COPY(imu.mag_unscaled, imu_mag);
#else
// unscaled vector
VECT3_COPY(imu.mag_unscaled, mag);
#endif
// scale vector
imu_scale_mag(&imu);