Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
mag_hmc58xx.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013 Felix Ruess <felix.ruess@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, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
29 #include "mcu_periph/uart.h"
30 #include "pprzlink/messages.h"
32 #include "generated/airframe.h"
33 #include "modules/core/abi.h"
34 
35 #ifndef HMC58XX_CHAN_X
36 #define HMC58XX_CHAN_X 0
37 #endif
38 #ifndef HMC58XX_CHAN_Y
39 #define HMC58XX_CHAN_Y 1
40 #endif
41 #ifndef HMC58XX_CHAN_Z
42 #define HMC58XX_CHAN_Z 2
43 #endif
44 #ifndef HMC58XX_CHAN_X_SIGN
45 #define HMC58XX_CHAN_X_SIGN +
46 #endif
47 #ifndef HMC58XX_CHAN_Y_SIGN
48 #define HMC58XX_CHAN_Y_SIGN +
49 #endif
50 #ifndef HMC58XX_CHAN_Z_SIGN
51 #define HMC58XX_CHAN_Z_SIGN +
52 #endif
53 
54 #if MODULE_HMC58XX_UPDATE_AHRS
55 
56 #if defined HMC58XX_MAG_TO_IMU_PHI && defined HMC58XX_MAG_TO_IMU_THETA && defined HMC58XX_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 
64 struct Hmc58xx mag_hmc58xx;
65 
67 {
68  hmc58xx_init(&mag_hmc58xx, &(MAG_HMC58XX_I2C_DEV), HMC58XX_ADDR);
69 
70 #if MODULE_HMC58XX_UPDATE_AHRS && USE_MAG_TO_IMU
71  struct Int32Eulers mag_to_imu_eulers = {
72  ANGLE_BFP_OF_REAL(HMC58XX_MAG_TO_IMU_PHI),
73  ANGLE_BFP_OF_REAL(HMC58XX_MAG_TO_IMU_THETA),
74  ANGLE_BFP_OF_REAL(HMC58XX_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_HMC58XX_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(mag, imu_mag);
107 #endif
108 
109  AbiSendMsgIMU_MAG_RAW(MAG_HMC58XX_SENDER_ID, now_ts, &mag);
110 #endif
111 #if MODULE_HMC58XX_SYNC_SEND
113 #endif
114 #if MODULE_HMC58XX_UPDATE_AHRS || MODULE_HMC58XX_SYNC_SEND
115  mag_hmc58xx.data_available = false;
116 #endif
117  }
118 }
119 
121 {
123  struct Int32Vect3 mag = {
127  };
128  DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &id, &mag.x, &mag.y, &mag.z);
129 }
Main include for ABI (AirBorneInterface).
#define MAG_HMC58XX_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 hmc58xx_init(struct Hmc58xx *hmc, struct i2c_periph *i2c_p, uint8_t addr)
Initialize Hmc58xx struct and set default config options.
Definition: hmc58xx.c:75
void hmc58xx_event(struct Hmc58xx *hmc)
Definition: hmc58xx.c:152
static void hmc58xx_periodic(struct Hmc58xx *hmc)
convenience function: read or start configuration if not already initialized
Definition: hmc58xx.h:85
union Hmc58xx::@321 data
volatile bool data_available
data ready flag
Definition: hmc58xx.h:65
#define HMC58XX_ADDR
Definition: hmc58xx_regs.h:31
#define HMC58XX_CHAN_Y
Definition: mag_hmc58xx.c:39
struct Hmc58xx mag_hmc58xx
Definition: mag_hmc58xx.c:64
void mag_hmc58xx_report(void)
Definition: mag_hmc58xx.c:120
void mag_hmc58xx_module_periodic(void)
Definition: mag_hmc58xx.c:80
#define HMC58XX_CHAN_X
Definition: mag_hmc58xx.c:36
#define HMC58XX_CHAN_X_SIGN
Definition: mag_hmc58xx.c:45
void mag_hmc58xx_module_event(void)
Definition: mag_hmc58xx.c:85
void mag_hmc58xx_module_init(void)
Definition: mag_hmc58xx.c:66
#define HMC58XX_CHAN_Z
Definition: mag_hmc58xx.c:42
#define HMC58XX_CHAN_Y_SIGN
Definition: mag_hmc58xx.c:48
#define HMC58XX_CHAN_Z_SIGN
Definition: mag_hmc58xx.c:51
Module wrapper for Honeywell HMC5843 and HMC5883 magnetometers.
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