Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
mag_qmc5883l.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2022 Paparazzi Team
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 #include "modules/core/abi.h"
33 
34 #ifndef QMC5883L_CHAN_X
35 #define QMC5883L_CHAN_X 0
36 #endif
37 #ifndef QMC5883L_CHAN_Y
38 #define QMC5883L_CHAN_Y 1
39 #endif
40 #ifndef QMC5883L_CHAN_Z
41 #define QMC5883L_CHAN_Z 2
42 #endif
43 #ifndef QMC5883L_CHAN_X_SIGN
44 #define QMC5883L_CHAN_X_SIGN +
45 #endif
46 #ifndef QMC5883L_CHAN_Y_SIGN
47 #define QMC5883L_CHAN_Y_SIGN +
48 #endif
49 #ifndef QMC5883L_CHAN_Z_SIGN
50 #define QMC5883L_CHAN_Z_SIGN +
51 #endif
52 
53 #ifndef QMC5883L_DATA_RATE
54 #define QMC5883L_DATA_RATE QMC5883L_ODR_DEFAULT
55 #endif
56 
57 #ifndef QMC5883L_ADDR
58 #define QMC5883L_ADDR QMC5883L_ADDR0
59 #endif
60 
61 #if MODULE_QMC5883L_UPDATE_AHRS
62 
63 #if defined QMC5883L_MAG_TO_IMU_PHI && defined QMC5883L_MAG_TO_IMU_THETA && defined QMC5883L_MAG_TO_IMU_PSI
64 #define USE_MAG_TO_IMU 1
65 static struct Int32RMat mag_to_imu;
66 #else
67 #define USE_MAG_TO_IMU 0
68 #endif
69 #endif
70 
71 struct Qmc5883l mag_qmc5883l;
72 
74 {
75  qmc5883l_init(&mag_qmc5883l, &(MAG_QMC5883L_I2C_DEV), QMC5883L_ADDR, QMC5883L_DATA_RATE);
76 
77 #if MODULE_QMC5883L_UPDATE_AHRS && USE_MAG_TO_IMU
78  struct Int32Eulers mag_to_imu_eulers = {
79  ANGLE_BFP_OF_REAL(QMC5883L_MAG_TO_IMU_PHI),
80  ANGLE_BFP_OF_REAL(QMC5883L_MAG_TO_IMU_THETA),
81  ANGLE_BFP_OF_REAL(QMC5883L_MAG_TO_IMU_PSI)
82  };
83  int32_rmat_of_eulers(&mag_to_imu, &mag_to_imu_eulers);
84 #endif
85 }
86 
88 {
90 }
91 
93 {
95 
97 #if MODULE_QMC5883L_UPDATE_AHRS
98  // current timestamp
99  uint32_t now_ts = get_sys_time_usec();
100 
101  // set channel order
102  struct Int32Vect3 mag = {
106  };
107  // only rotate if needed
108 #if USE_MAG_TO_IMU
109  struct Int32Vect3 imu_mag;
110  // rotate data from mag frame to imu frame
111  int32_rmat_vmult(&imu_mag, &mag_to_imu, &mag);
112  // unscaled vector
113  VECT3_COPY(mag, imu_mag);
114 #endif
115 
116  AbiSendMsgIMU_MAG_RAW(MAG_QMC5883L_SENDER_ID, now_ts, &mag);
117 #endif
118 #if MODULE_QMC5883L_SYNC_SEND
120 #endif
121 #if MODULE_QMC5883L_UPDATE_AHRS || MODULE_QMC5883L_SYNC_SEND
123 #endif
124  }
125 }
126 
128 {
130  struct Int32Vect3 mag = {
134  };
135  DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &id, &mag.x, &mag.y, &mag.z);
136 }
Main include for ABI (AirBorneInterface).
#define MAG_QMC5883L_SENDER_ID
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:71
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
#define ANGLE_BFP_OF_REAL(_af)
#define int32_rmat_of_eulers
Rotation matrix from Euler angles.
void int32_rmat_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_a2b, struct Int32Vect3 *va)
rotate 3D vector by rotation matrix.
euler angles
rotation matrix
void mag_qmc5883l_module_periodic(void)
Definition: mag_qmc5883l.c:87
#define QMC5883L_CHAN_Z
Definition: mag_qmc5883l.c:41
#define QMC5883L_CHAN_Y_SIGN
Definition: mag_qmc5883l.c:47
#define QMC5883L_DATA_RATE
Definition: mag_qmc5883l.c:54
void mag_qmc5883l_report(void)
Definition: mag_qmc5883l.c:127
#define QMC5883L_CHAN_Y
Definition: mag_qmc5883l.c:38
#define QMC5883L_ADDR
Definition: mag_qmc5883l.c:58
#define QMC5883L_CHAN_Z_SIGN
Definition: mag_qmc5883l.c:50
struct Qmc5883l mag_qmc5883l
Definition: mag_qmc5883l.c:71
#define QMC5883L_CHAN_X
Definition: mag_qmc5883l.c:35
#define QMC5883L_CHAN_X_SIGN
Definition: mag_qmc5883l.c:44
void mag_qmc5883l_module_init(void)
Definition: mag_qmc5883l.c:73
void mag_qmc5883l_module_event(void)
Definition: mag_qmc5883l.c:92
Module wrapper for QNI QMC5883L magnetometer.
void qmc5883l_event(struct Qmc5883l *mag)
Definition: qmc5883l.c:154
void qmc5883l_init(struct Qmc5883l *mag, struct i2c_periph *i2c_p, uint8_t addr, uint8_t data_rate)
Definition: qmc5883l.c:66
volatile bool data_available
data ready flag
Definition: qmc5883l.h:66
union Qmc5883l::@348 data
static void qmc5883l_periodic(struct Qmc5883l *mag)
convenience function: read or start configuration if not already initialized
Definition: qmc5883l.h:79
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98