Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
mag_hmc58xx.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
29 #include "mcu_periph/uart.h"
30 #include "pprzlink/messages.h"
32 #include "generated/airframe.h"
33 
34 #ifndef HMC58XX_CHAN_X
35 #define HMC58XX_CHAN_X 0
36 #endif
37 #ifndef HMC58XX_CHAN_Y
38 #define HMC58XX_CHAN_Y 1
39 #endif
40 #ifndef HMC58XX_CHAN_Z
41 #define HMC58XX_CHAN_Z 2
42 #endif
43 #ifndef HMC58XX_CHAN_X_SIGN
44 #define HMC58XX_CHAN_X_SIGN +
45 #endif
46 #ifndef HMC58XX_CHAN_Y_SIGN
47 #define HMC58XX_CHAN_Y_SIGN +
48 #endif
49 #ifndef HMC58XX_CHAN_Z_SIGN
50 #define HMC58XX_CHAN_Z_SIGN +
51 #endif
52 
53 #if MODULE_HMC58XX_UPDATE_AHRS
54 #include "subsystems/imu.h"
55 #include "subsystems/abi.h"
56 
57 #if defined HMC58XX_MAG_TO_IMU_PHI && defined HMC58XX_MAG_TO_IMU_THETA && defined HMC58XX_MAG_TO_IMU_PSI
58 #define USE_MAG_TO_IMU 1
59 static struct Int32RMat mag_to_imu;
60 #else
61 #define USE_MAG_TO_IMU 0
62 #endif
63 #endif
64 
66 
68 {
69  hmc58xx_init(&mag_hmc58xx, &(MAG_HMC58XX_I2C_DEV), HMC58XX_ADDR);
70 
71 #if MODULE_HMC58XX_UPDATE_AHRS && USE_MAG_TO_IMU
72  struct Int32Eulers mag_to_imu_eulers = {
73  ANGLE_BFP_OF_REAL(HMC58XX_MAG_TO_IMU_PHI),
74  ANGLE_BFP_OF_REAL(HMC58XX_MAG_TO_IMU_THETA),
75  ANGLE_BFP_OF_REAL(HMC58XX_MAG_TO_IMU_PSI)
76  };
77  int32_rmat_of_eulers(&mag_to_imu, &mag_to_imu_eulers);
78 #endif
79 }
80 
82 {
84 }
85 
87 {
89 
91 #if MODULE_HMC58XX_UPDATE_AHRS
92  // current timestamp
93  uint32_t now_ts = get_sys_time_usec();
94 
95  // set channel order
96  struct Int32Vect3 mag = {
100  };
101  // only rotate if needed
102 #if USE_MAG_TO_IMU
103  struct Int32Vect3 imu_mag;
104  // rotate data from mag frame to imu frame
105  int32_rmat_vmult(&imu_mag, &mag_to_imu, &mag);
106  // unscaled vector
107  VECT3_COPY(imu.mag_unscaled, imu_mag);
108 #else
109  // unscaled vector
111 #endif
112  // scale vector
113  imu_scale_mag(&imu);
114 
115  AbiSendMsgIMU_MAG_INT32(MAG_HMC58XX_SENDER_ID, now_ts, &imu.mag);
116 #endif
117 #if MODULE_HMC58XX_SYNC_SEND
119 #endif
120 #if MODULE_HMC58XX_UPDATE_AHRS || MODULE_HMC58XX_SYNC_SEND
121  mag_hmc58xx.data_available = false;
122 #endif
123  }
124 }
125 
127 {
128  struct Int32Vect3 mag = {
132  };
133  DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &mag.x, &mag.y, &mag.z);
134 }
mag_hmc58xx_module_periodic
void mag_hmc58xx_module_periodic(void)
Definition: mag_hmc58xx.c:81
mag_hmc58xx
struct Hmc58xx mag_hmc58xx
Definition: mag_hmc58xx.c:65
hmc58xx_event
void hmc58xx_event(struct Hmc58xx *hmc)
Definition: hmc58xx.c:152
Hmc58xx::data
union Hmc58xx::@317 data
Int32RMat
rotation matrix
Definition: pprz_algebra_int.h:159
HMC58XX_CHAN_Y
#define HMC58XX_CHAN_Y
Definition: mag_hmc58xx.c:38
abi.h
HMC58XX_CHAN_Z_SIGN
#define HMC58XX_CHAN_Z_SIGN
Definition: mag_hmc58xx.c:50
Int32Vect3::z
int32_t z
Definition: pprz_algebra_int.h:91
uint32_t
unsigned long uint32_t
Definition: types.h:18
HMC58XX_CHAN_X
#define HMC58XX_CHAN_X
Definition: mag_hmc58xx.c:35
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
HMC58XX_ADDR
#define HMC58XX_ADDR
Definition: hmc58xx_regs.h:31
imu.h
mag_hmc58xx_report
void mag_hmc58xx_report(void)
Definition: mag_hmc58xx.c:126
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
ANGLE_BFP_OF_REAL
#define ANGLE_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:210
Int32Vect3
Definition: pprz_algebra_int.h:88
Int32Vect3::y
int32_t y
Definition: pprz_algebra_int.h:90
Int32Eulers
euler angles
Definition: pprz_algebra_int.h:146
hmc58xx_periodic
static void hmc58xx_periodic(struct Hmc58xx *hmc)
convenience function: read or start configuration if not already initialized
Definition: hmc58xx.h:85
HMC58XX_CHAN_X_SIGN
#define HMC58XX_CHAN_X_SIGN
Definition: mag_hmc58xx.c:44
int32_rmat_of_eulers
#define int32_rmat_of_eulers
Rotation matrix from Euler angles.
Definition: pprz_algebra_int.h:408
MAG_HMC58XX_SENDER_ID
#define MAG_HMC58XX_SENDER_ID
Definition: abi_sender_ids.h:193
imu
struct Imu imu
global IMU state
Definition: imu.c:108
hmc58xx_init
void hmc58xx_init(struct Hmc58xx *hmc, struct i2c_periph *i2c_p, uint8_t addr)
Initialize Hmc58xx struct and set default config options.
Definition: hmc58xx.c:75
int32_rmat_vmult
void int32_rmat_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_a2b, struct Int32Vect3 *va)
rotate 3D vector by rotation matrix.
Definition: pprz_algebra_int.c:107
int32_t
signed long int32_t
Definition: types.h:19
imu_scale_mag
void imu_scale_mag(struct Imu *_imu)
Definition: ahrs_gx3.c:352
HMC58XX_CHAN_Z
#define HMC58XX_CHAN_Z
Definition: mag_hmc58xx.c:41
Int32Vect3::x
int32_t x
Definition: pprz_algebra_int.h:89
mag_hmc58xx.h
mag_hmc58xx_module_event
void mag_hmc58xx_module_event(void)
Definition: mag_hmc58xx.c:86
Imu::mag
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
Definition: imu.h:40
Imu::mag_unscaled
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:48
Hmc58xx
Definition: hmc58xx.h:60
Hmc58xx::data_available
volatile bool data_available
data ready flag
Definition: hmc58xx.h:65
mag_hmc58xx_module_init
void mag_hmc58xx_module_init(void)
Definition: mag_hmc58xx.c:67
HMC58XX_CHAN_Y_SIGN
#define HMC58XX_CHAN_Y_SIGN
Definition: mag_hmc58xx.c:47
VECT3_COPY
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140