Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
mag_ist8310.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2019 Freek van Tienen <freek.v.tienen@gmail.com>
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 IST8310_CHAN_X
34 #define IST8310_CHAN_X 1
35 #endif
36 #ifndef IST8310_CHAN_Y
37 #define IST8310_CHAN_Y 0
38 #endif
39 #ifndef IST8310_CHAN_Z
40 #define IST8310_CHAN_Z 2
41 #endif
42 #ifndef IST8310_CHAN_X_SIGN
43 #define IST8310_CHAN_X_SIGN +
44 #endif
45 #ifndef IST8310_CHAN_Y_SIGN
46 #define IST8310_CHAN_Y_SIGN +
47 #endif
48 #ifndef IST8310_CHAN_Z_SIGN
49 #define IST8310_CHAN_Z_SIGN +
50 #endif
51 
52 #if MODULE_IST8310_UPDATE_AHRS
53 #include "subsystems/imu.h"
54 #include "subsystems/abi.h"
55 
56 #if defined IST8310_MAG_TO_IMU_PHI && defined IST8310_MAG_TO_IMU_THETA && defined IST8310_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  ist8310_init(&mag_ist8310, &(MAG_IST8310_I2C_DEV), IST8310_ADDR);
69 
70 #if MODULE_IST8310_UPDATE_AHRS && USE_MAG_TO_IMU
71  struct Int32Eulers mag_to_imu_eulers = {
72  ANGLE_BFP_OF_REAL(IST8310_MAG_TO_IMU_PHI),
73  ANGLE_BFP_OF_REAL(IST8310_MAG_TO_IMU_THETA),
74  ANGLE_BFP_OF_REAL(IST8310_MAG_TO_IMU_PSI)
75  };
76  int32_rmat_of_eulers(&mag_to_imu, &mag_to_imu_eulers);
77 #endif
78 }
79 
81 {
83 }
84 
86 {
88 
90 #if MODULE_IST8310_UPDATE_AHRS
91  // current timestamp
92  uint32_t now_ts = get_sys_time_usec();
93 
94  // set channel order
95  struct Int32Vect3 mag = {
99  };
100  // only rotate if needed
101 #if USE_MAG_TO_IMU
102  struct Int32Vect3 imu_mag;
103  // rotate data from mag frame to imu frame
104  int32_rmat_vmult(&imu_mag, &mag_to_imu, &mag);
105  // unscaled vector
106  VECT3_COPY(imu.mag_unscaled, imu_mag);
107 #else
108  // unscaled vector
110 #endif
111  // scale vector
112  imu_scale_mag(&imu);
113 
114  AbiSendMsgIMU_MAG_INT32(MAG_IST8310_SENDER_ID, now_ts, &imu.mag);
115 #endif
116 #if MODULE_IST8310_SYNC_SEND
118 #endif
119 #if MODULE_IST8310_UPDATE_AHRS || MODULE_IST8310_SYNC_SEND
120  mag_ist8310.data_available = false;
121 #endif
122  }
123 }
124 
126 {
127  struct Int32Vect3 mag = {
131  };
132  DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &mag.x, &mag.y, &mag.z);
133 }
IST8310_CHAN_Y
#define IST8310_CHAN_Y
Definition: mag_ist8310.c:37
Int32RMat
rotation matrix
Definition: pprz_algebra_int.h:159
abi.h
IST8310_CHAN_Z_SIGN
#define IST8310_CHAN_Z_SIGN
Definition: mag_ist8310.c:49
IST8310_CHAN_X_SIGN
#define IST8310_CHAN_X_SIGN
Definition: mag_ist8310.c:43
Int32Vect3::z
int32_t z
Definition: pprz_algebra_int.h:91
IST8310_CHAN_Z
#define IST8310_CHAN_Z
Definition: mag_ist8310.c:40
uint32_t
unsigned long uint32_t
Definition: types.h:18
ist8310_init
void ist8310_init(struct IST8310 *ist, struct i2c_periph *i2c_p, uint8_t addr)
Initialize IST8310 struct.
Definition: ist8310.c:34
IST8310
Default IST8310 structure.
Definition: ist8310.h:55
MAG_IST8310_SENDER_ID
#define MAG_IST8310_SENDER_ID
Definition: abi_sender_ids.h:201
IST8310::data
union IST8310::@318 data
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
mag_ist8310_module_event
void mag_ist8310_module_event(void)
Definition: mag_ist8310.c:85
ANGLE_BFP_OF_REAL
#define ANGLE_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:210
ist8310_event
void ist8310_event(struct IST8310 *ist)
Definition: ist8310.c:111
Int32Vect3
Definition: pprz_algebra_int.h:88
Int32Vect3::y
int32_t y
Definition: pprz_algebra_int.h:90
mag_ist8310_module_init
void mag_ist8310_module_init(void)
Definition: mag_ist8310.c:66
Int32Eulers
euler angles
Definition: pprz_algebra_int.h:146
IST8310_CHAN_X
#define IST8310_CHAN_X
Definition: mag_ist8310.c:34
mag_ist8310_report
void mag_ist8310_report(void)
Definition: mag_ist8310.c:125
ist8310_periodic
static void ist8310_periodic(struct IST8310 *ist)
convenience function: read or start configuration if not already initialized
Definition: ist8310.h:77
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
IST8310_CHAN_Y_SIGN
#define IST8310_CHAN_Y_SIGN
Definition: mag_ist8310.c:46
IST8310::data_available
volatile bool data_available
data ready flag
Definition: ist8310.h:63
mag_ist8310.h
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
signed long int32_t
Definition: types.h:19
imu_scale_mag
void imu_scale_mag(struct Imu *_imu)
Definition: ahrs_gx3.c:352
IST8310_ADDR
#define IST8310_ADDR
Definition: ist8310_regs.h:31
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
mag_ist8310_module_periodic
void mag_ist8310_module_periodic(void)
Definition: mag_ist8310.c:80
mag_ist8310
struct IST8310 mag_ist8310
Definition: mag_ist8310.c:64
VECT3_COPY
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140