Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
mag_lis3mdl.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
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, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
28 #include "mcu_periph/uart.h"
29 #include "pprzlink/messages.h"
31 #include "generated/airframe.h"
32 
33 #ifndef LIS3MDL_CHAN_X
34 #define LIS3MDL_CHAN_X 0
35 #endif
36 #ifndef LIS3MDL_CHAN_Y
37 #define LIS3MDL_CHAN_Y 1
38 #endif
39 #ifndef LIS3MDL_CHAN_Z
40 #define LIS3MDL_CHAN_Z 2
41 #endif
42 #ifndef LIS3MDL_CHAN_X_SIGN
43 #define LIS3MDL_CHAN_X_SIGN +
44 #endif
45 #ifndef LIS3MDL_CHAN_Y_SIGN
46 #define LIS3MDL_CHAN_Y_SIGN +
47 #endif
48 #ifndef LIS3MDL_CHAN_Z_SIGN
49 #define LIS3MDL_CHAN_Z_SIGN +
50 #endif
51 
52 #if MODULE_LIS3MDL_UPDATE_AHRS
53 #include "subsystems/imu.h"
54 #include "subsystems/abi.h"
55 
56 #if defined LIS3MDL_MAG_TO_IMU_PHI && defined LIS3MDL_MAG_TO_IMU_THETA && defined LIS3MDL_MAG_TO_IMU_PSI
57 #define USE_MAG_TO_IMU 1
58 static struct Int32RMat mag_to_imu;
59 #else
60 #define USE_MAG_TO_IMU 0
61 #endif
62 #endif
63 
65 
67 {
68  lis3mdl_init(&mag_lis3mdl, &(MAG_LIS3MDL_I2C_DEV), LIS3MDL_ADDR1,
73 
74 #if MODULE_LIS3MDL_UPDATE_AHRS && USE_MAG_TO_IMU
75  struct Int32Eulers mag_to_imu_eulers = {
76  ANGLE_BFP_OF_REAL(LIS3MDL_MAG_TO_IMU_PHI),
77  ANGLE_BFP_OF_REAL(LIS3MDL_MAG_TO_IMU_THETA),
78  ANGLE_BFP_OF_REAL(LIS3MDL_MAG_TO_IMU_PSI)
79  };
80  int32_rmat_of_eulers(&mag_to_imu, &mag_to_imu_eulers);
81 #endif
82 }
83 
85 {
87 }
88 
90 {
92 
94 #if MODULE_LIS3MDL_UPDATE_AHRS
95  // current timestamp
96  uint32_t now_ts = get_sys_time_usec();
97 
98  // set channel order
99  struct Int32Vect3 mag = {
103  };
104  // only rotate if needed
105 #if USE_MAG_TO_IMU
106  struct Int32Vect3 imu_mag;
107  // rotate data from mag frame to imu frame
108  int32_rmat_vmult(&imu_mag, &mag_to_imu, &mag);
109  // unscaled vector
110  VECT3_COPY(imu.mag_unscaled, imu_mag);
111 #else
112  // unscaled vector
114 #endif
115  // scale vector
116  imu_scale_mag(&imu);
117 
118  AbiSendMsgIMU_MAG_INT32(MAG_LIS3MDL_SENDER_ID, now_ts, &imu.mag);
119 #endif
120 #if MODULE_LIS3MDL_SYNC_SEND
122 #endif
123 #if MODULE_LIS3MDL_UPDATE_AHRS || MODULE_LIS3MDL_SYNC_SEND
124  mag_lis3mdl.data_available = false;
125 #endif
126  }
127 }
128 
130 {
131  struct Int32Vect3 mag = {
135  };
136  DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &mag.x, &mag.y, &mag.z);
137 }
mag_lis3mdl
struct Lis3mdl mag_lis3mdl
Definition: mag_lis3mdl.c:64
mag_lis3mdl_module_periodic
void mag_lis3mdl_module_periodic(void)
Definition: mag_lis3mdl.c:84
LIS3MDL_CHAN_X_SIGN
#define LIS3MDL_CHAN_X_SIGN
Definition: mag_lis3mdl.c:43
Int32RMat
rotation matrix
Definition: pprz_algebra_int.h:159
abi.h
LIS3MDL_CHAN_Z
#define LIS3MDL_CHAN_Z
Definition: mag_lis3mdl.c:40
Int32Vect3::z
int32_t z
Definition: pprz_algebra_int.h:91
uint32_t
unsigned long uint32_t
Definition: types.h:18
LIS3MDL_CHAN_Y
#define LIS3MDL_CHAN_Y
Definition: mag_lis3mdl.c:37
LIS3MDL_MODE_CONTINUOUS
#define LIS3MDL_MODE_CONTINUOUS
Definition: lis3mdl.h:51
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
imu.h
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
LIS3MDL_PERFORMANCE_ULTRA_HIGH
#define LIS3MDL_PERFORMANCE_ULTRA_HIGH
Definition: lis3mdl.h:40
mag_lis3mdl_report
void mag_lis3mdl_report(void)
Definition: mag_lis3mdl.c:129
LIS3MDL_SCALE_4_GAUSS
#define LIS3MDL_SCALE_4_GAUSS
Definition: lis3mdl.h:55
ANGLE_BFP_OF_REAL
#define ANGLE_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:210
Int32Vect3
Definition: pprz_algebra_int.h:88
Lis3mdl::data
union Lis3mdl::@323 data
Int32Vect3::y
int32_t y
Definition: pprz_algebra_int.h:90
Int32Eulers
euler angles
Definition: pprz_algebra_int.h:146
LIS3MDL_DATA_RATE_80_HZ
#define LIS3MDL_DATA_RATE_80_HZ
Definition: lis3mdl.h:49
int32_rmat_of_eulers
#define int32_rmat_of_eulers
Rotation matrix from Euler angles.
Definition: pprz_algebra_int.h:408
imu
struct Imu imu
global IMU state
Definition: imu.c:108
LIS3MDL_CHAN_X
#define LIS3MDL_CHAN_X
Definition: mag_lis3mdl.c:34
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
Lis3mdl
Definition: lis3mdl.h:69
Lis3mdl::data_available
volatile bool data_available
data ready flag
Definition: lis3mdl.h:74
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
Int32Vect3::x
int32_t x
Definition: pprz_algebra_int.h:89
MAG_LIS3MDL_SENDER_ID
#define MAG_LIS3MDL_SENDER_ID
Definition: abi_sender_ids.h:197
Imu::mag
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
Definition: imu.h:40
mag_lis3mdl_module_init
void mag_lis3mdl_module_init(void)
Definition: mag_lis3mdl.c:66
Imu::mag_unscaled
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:48
lis3mdl_init
void lis3mdl_init(struct Lis3mdl *mag, struct i2c_periph *i2c_p, uint8_t addr, uint8_t data_rate, uint8_t scale, uint8_t mode, uint8_t perf)
Definition: lis3mdl.c:60
lis3mdl_event
void lis3mdl_event(struct Lis3mdl *mag)
Definition: lis3mdl.c:127
LIS3MDL_CHAN_Z_SIGN
#define LIS3MDL_CHAN_Z_SIGN
Definition: mag_lis3mdl.c:49
LIS3MDL_CHAN_Y_SIGN
#define LIS3MDL_CHAN_Y_SIGN
Definition: mag_lis3mdl.c:46
VECT3_COPY
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
mag_lis3mdl_module_event
void mag_lis3mdl_module_event(void)
Definition: mag_lis3mdl.c:89
LIS3MDL_ADDR1
#define LIS3MDL_ADDR1
Definition: lis3mdl.h:34
mag_lis3mdl.h
lis3mdl_periodic
static void lis3mdl_periodic(struct Lis3mdl *mag)
convenience function: read or start configuration if not already initialized
Definition: lis3mdl.h:88