Paparazzi UAS  v6.0_unstable-92-g17422e4-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 "subsystems/imu.h"
62 #include "subsystems/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 }
#define RM3100_CHAN_X
Definition: mag_rm3100.c:34
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
void mag_rm3100_module_event(void)
Definition: mag_rm3100.c:93
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
Main include for ABI (AirBorneInterface).
static void rm3100_periodic(struct Rm3100 *mag)
convenience function: read or start configuration if not already initialized
Definition: rm3100.h:92
#define RM3100_CHAN_X_SIGN
Definition: mag_rm3100.c:43
#define RM3100_ADDR
Definition: mag_rm3100.c:57
struct Imu imu
global IMU state
Definition: imu.c:108
void rm3100_event(struct Rm3100 *mag)
Definition: rm3100.c:152
#define RM3100_CHAN_Y
Definition: mag_rm3100.c:37
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:48
struct Rm3100 mag_rm3100
Definition: mag_rm3100.c:72
void int32_rmat_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_a2b, struct Int32Vect3 *va)
rotate 3D vector by rotation matrix.
void rm3100_init(struct Rm3100 *mag, struct i2c_periph *i2c_p, uint8_t addr, uint8_t data_rate)
Definition: rm3100.c:59
void mag_rm3100_module_init(void)
Definition: mag_rm3100.c:74
euler angles
#define RM3100_CHAN_Z_SIGN
Definition: mag_rm3100.c:49
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
Definition: imu.h:40
Inertial Measurement Unit interface.
void mag_rm3100_report(void)
Definition: mag_rm3100.c:133
#define ANGLE_BFP_OF_REAL(_af)
#define RM3100_CHAN_Y_SIGN
Definition: mag_rm3100.c:46
union Rm3100::@330 data
#define RM3100_CHAN_Z
Definition: mag_rm3100.c:40
void imu_scale_mag(struct Imu *_imu)
Definition: ahrs_gx3.c:352
void mag_rm3100_module_periodic(void)
Definition: mag_rm3100.c:88
volatile bool data_available
data ready flag
Definition: rm3100.h:78
Definition: rm3100.h:72
rotation matrix
Module wrapper for PNI RM3100 magnetometers.
#define MAG_RM3100_SENDER_ID
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:71
#define RM3100_DATA_RATE
Definition: mag_rm3100.c:53
#define int32_rmat_of_eulers
Rotation matrix from Euler angles.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78