Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
mag_rm3100.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2021 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 RM3100_CHAN_X
34 #define RM3100_CHAN_X 0
35 #endif
36 #ifndef RM3100_CHAN_Y
37 #define RM3100_CHAN_Y 1
38 #endif
39 #ifndef RM3100_CHAN_Z
40 #define RM3100_CHAN_Z 2
41 #endif
42 #ifndef RM3100_CHAN_X_SIGN
43 #define RM3100_CHAN_X_SIGN +
44 #endif
45 #ifndef RM3100_CHAN_Y_SIGN
46 #define RM3100_CHAN_Y_SIGN +
47 #endif
48 #ifndef RM3100_CHAN_Z_SIGN
49 #define RM3100_CHAN_Z_SIGN +
50 #endif
51 
52 #ifndef RM3100_DATA_RATE
53 #define RM3100_DATA_RATE RM3100_TMRC_DEFAULT
54 #endif
55 
56 #ifndef RM3100_ADDR
57 #define RM3100_ADDR RM3100_ADDR0
58 #endif
59 
60 #if MODULE_RM3100_UPDATE_AHRS
61 #include "modules/imu/imu.h"
62 #include "modules/core/abi.h"
63 
64 #if defined RM3100_MAG_TO_IMU_PHI && defined RM3100_MAG_TO_IMU_THETA && defined RM3100_MAG_TO_IMU_PSI
65 #define USE_MAG_TO_IMU 1
66 static struct Int32RMat mag_to_imu;
67 #else
68 #define USE_MAG_TO_IMU 0
69 #endif
70 #endif
71 
73 
75 {
76  rm3100_init(&mag_rm3100, &(MAG_RM3100_I2C_DEV), RM3100_ADDR, RM3100_DATA_RATE);
77 
78 #if MODULE_RM3100_UPDATE_AHRS && USE_MAG_TO_IMU
79  struct Int32Eulers mag_to_imu_eulers = {
80  ANGLE_BFP_OF_REAL(RM3100_MAG_TO_IMU_PHI),
81  ANGLE_BFP_OF_REAL(RM3100_MAG_TO_IMU_THETA),
82  ANGLE_BFP_OF_REAL(RM3100_MAG_TO_IMU_PSI)
83  };
84  int32_rmat_of_eulers(&mag_to_imu, &mag_to_imu_eulers);
85 #endif
86 }
87 
89 {
91 }
92 
94 {
96 
98 #if MODULE_RM3100_UPDATE_AHRS
99  // current timestamp
100  uint32_t now_ts = get_sys_time_usec();
101 
102  // set channel order
103  struct Int32Vect3 mag = {
107  };
108  // only rotate if needed
109 #if USE_MAG_TO_IMU
110  struct Int32Vect3 imu_mag;
111  // rotate data from mag frame to imu frame
112  int32_rmat_vmult(&imu_mag, &mag_to_imu, &mag);
113  // unscaled vector
114  VECT3_COPY(imu.mag_unscaled, imu_mag);
115 #else
116  // unscaled vector
118 #endif
119  // scale vector
120  imu_scale_mag(&imu);
121 
122  AbiSendMsgIMU_MAG_INT32(MAG_RM3100_SENDER_ID, now_ts, &imu.mag);
123 #endif
124 #if MODULE_RM3100_SYNC_SEND
126 #endif
127 #if MODULE_RM3100_UPDATE_AHRS || MODULE_RM3100_SYNC_SEND
128  mag_rm3100.data_available = false;
129 #endif
130  }
131 }
132 
134 {
135  struct Int32Vect3 mag = {
139  };
140  DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &mag.x, &mag.y, &mag.z);
141 }
RM3100_CHAN_Z
#define RM3100_CHAN_Z
Definition: mag_rm3100.c:40
mag_rm3100_module_event
void mag_rm3100_module_event(void)
Definition: mag_rm3100.c:93
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
rm3100_periodic
static void rm3100_periodic(struct Rm3100 *mag)
convenience function: read or start configuration if not already initialized
Definition: rm3100.h:92
RM3100_CHAN_Y
#define RM3100_CHAN_Y
Definition: mag_rm3100.c:37
Int32RMat
rotation matrix
Definition: pprz_algebra_int.h:159
RM3100_ADDR
#define RM3100_ADDR
Definition: mag_rm3100.c:57
abi.h
RM3100_CHAN_Z_SIGN
#define RM3100_CHAN_Z_SIGN
Definition: mag_rm3100.c:49
mag_rm3100_module_periodic
void mag_rm3100_module_periodic(void)
Definition: mag_rm3100.c:88
rm3100_event
void rm3100_event(struct Rm3100 *mag)
Definition: rm3100.c:152
rm3100_init
void rm3100_init(struct Rm3100 *mag, struct i2c_periph *i2c_p, uint8_t addr, uint8_t data_rate)
Definition: rm3100.c:59
Int32Vect3::z
int32_t z
Definition: pprz_algebra_int.h:91
mag_rm3100_module_init
void mag_rm3100_module_init(void)
Definition: mag_rm3100.c:74
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:75
imu.h
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
RM3100_CHAN_X_SIGN
#define RM3100_CHAN_X_SIGN
Definition: mag_rm3100.c:43
Rm3100::data
union Rm3100::@331 data
MAG_RM3100_SENDER_ID
#define MAG_RM3100_SENDER_ID
Definition: abi_sender_ids.h:213
ANGLE_BFP_OF_REAL
#define ANGLE_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:211
RM3100_CHAN_X
#define RM3100_CHAN_X
Definition: mag_rm3100.c:34
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
Rm3100
Definition: rm3100.h:72
int32_rmat_of_eulers
#define int32_rmat_of_eulers
Rotation matrix from Euler angles.
Definition: pprz_algebra_int.h:409
imu
struct Imu imu
global IMU state
Definition: imu.c:108
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
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
RM3100_CHAN_Y_SIGN
#define RM3100_CHAN_Y_SIGN
Definition: mag_rm3100.c:46
mag_rm3100
struct Rm3100 mag_rm3100
Definition: mag_rm3100.c:72
mag_rm3100_report
void mag_rm3100_report(void)
Definition: mag_rm3100.c:133
Int32Vect3::x
int32_t x
Definition: pprz_algebra_int.h:89
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
Rm3100::data_available
volatile bool data_available
data ready flag
Definition: rm3100.h:78
mag_rm3100.h
imu_scale_mag
void WEAK imu_scale_mag(struct Imu *_imu)
Definition: imu.c:246
VECT3_COPY
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
RM3100_DATA_RATE
#define RM3100_DATA_RATE
Definition: mag_rm3100.c:53