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 #include "modules/core/abi.h"
33 
34 #ifndef RM3100_CHAN_X
35 #define RM3100_CHAN_X 0
36 #endif
37 #ifndef RM3100_CHAN_Y
38 #define RM3100_CHAN_Y 1
39 #endif
40 #ifndef RM3100_CHAN_Z
41 #define RM3100_CHAN_Z 2
42 #endif
43 #ifndef RM3100_CHAN_X_SIGN
44 #define RM3100_CHAN_X_SIGN +
45 #endif
46 #ifndef RM3100_CHAN_Y_SIGN
47 #define RM3100_CHAN_Y_SIGN +
48 #endif
49 #ifndef RM3100_CHAN_Z_SIGN
50 #define RM3100_CHAN_Z_SIGN +
51 #endif
52 
53 #ifndef RM3100_DATA_RATE
54 #define RM3100_DATA_RATE RM3100_TMRC_DEFAULT
55 #endif
56 
57 #ifndef RM3100_ADDR
58 #define RM3100_ADDR RM3100_ADDR0
59 #endif
60 
61 #if MODULE_RM3100_UPDATE_AHRS
62 
63 #if defined RM3100_MAG_TO_IMU_PHI && defined RM3100_MAG_TO_IMU_THETA && defined RM3100_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 
72 
74 {
75  rm3100_init(&mag_rm3100, &(MAG_RM3100_I2C_DEV), RM3100_ADDR, RM3100_DATA_RATE);
76 
77 #if MODULE_RM3100_UPDATE_AHRS && USE_MAG_TO_IMU
78  struct Int32Eulers mag_to_imu_eulers = {
79  ANGLE_BFP_OF_REAL(RM3100_MAG_TO_IMU_PHI),
80  ANGLE_BFP_OF_REAL(RM3100_MAG_TO_IMU_THETA),
81  ANGLE_BFP_OF_REAL(RM3100_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_RM3100_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_RM3100_SENDER_ID, now_ts, &mag);
117 #endif
118 #if MODULE_RM3100_SYNC_SEND
120 #endif
121 #if MODULE_RM3100_UPDATE_AHRS || MODULE_RM3100_SYNC_SEND
122  mag_rm3100.data_available = false;
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 }
RM3100_CHAN_Z
#define RM3100_CHAN_Z
Definition: mag_rm3100.c:41
mag_rm3100_module_event
void mag_rm3100_module_event(void)
Definition: mag_rm3100.c:92
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
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:38
Int32RMat
rotation matrix
Definition: pprz_algebra_int.h:159
RM3100_ADDR
#define RM3100_ADDR
Definition: mag_rm3100.c:58
abi.h
RM3100_CHAN_Z_SIGN
#define RM3100_CHAN_Z_SIGN
Definition: mag_rm3100.c:50
mag_rm3100_module_periodic
void mag_rm3100_module_periodic(void)
Definition: mag_rm3100.c:87
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:73
Rm3100::data
union Rm3100::@334 data
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:75
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
RM3100_CHAN_X_SIGN
#define RM3100_CHAN_X_SIGN
Definition: mag_rm3100.c:44
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:35
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
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:47
mag_rm3100
struct Rm3100 mag_rm3100
Definition: mag_rm3100.c:71
mag_rm3100_report
void mag_rm3100_report(void)
Definition: mag_rm3100.c:127
Int32Vect3::x
int32_t x
Definition: pprz_algebra_int.h:89
Rm3100::data_available
volatile bool data_available
data ready flag
Definition: rm3100.h:78
mag_rm3100.h
VECT3_COPY
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
RM3100_DATA_RATE
#define RM3100_DATA_RATE
Definition: mag_rm3100.c:54