Paparazzi UAS  v5.15_devel-113-g1b57ff1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
imu_b2.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
3  * Copyright (C) 2012 Gautier Hattenberger
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
32 #include "subsystems/imu.h"
33 
35 
36 PRINT_CONFIG_VAR(IMU_B2_MAG_TYPE)
37 
38 void imu_b2_init(void)
39 {
40 
41  max1168_init();
42 #if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2100
43  ms2100_init(&ms2100, &(MS2100_SPI_DEV), MS2100_SLAVE_IDX);
44 #elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_AMI601
45  ami601_init();
46 #elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC5843
47  hmc5843_init();
48 #elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC58XX
49  hmc58xx_init(&imu_b2.mag_hmc, &(IMU_B2_I2C_DEV), HMC58XX_ADDR);
50  // Booz2 v1.2 has HMC5843
51  imu_b2.mag_hmc.type = HMC_TYPE_5843;
52 #endif
53 
54 }
55 
56 #include "led.h"
57 void imu_b2_periodic(void)
58 {
59 
60  // read adc
62  // read mag
63 #if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2100
65 #elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_AMI601
66  RunOnceEvery(10, { ami601_read(); });
67 #elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC58XX
68  RunOnceEvery(5, hmc58xx_periodic(&imu_b2.mag_hmc));
69 #endif
70 
71 }
72 
73 #if defined IMU_MAG_45_HACK
74 void imu_scale_mag(struct Imu *_imu)
75 {
78  _imu->mag.x = msx - msy;
79  _imu->mag.y = msx + msy;
81 }
82 #elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_NONE
83 void imu_scale_mag(struct Imu *_imu __attribute__((unused))) {}
84 #endif
85 
86 
87 
90 #include "subsystems/abi.h"
91 #include "mcu_periph/sys_time.h"
92 
93 #if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2100
94 static inline void ImuMagEvent(void)
95 {
102  imu_scale_mag(&imu);
103  AbiSendMsgIMU_MAG_INT32(IMU_B2_ID, get_sys_time_usec(), &imu.mag);
104  }
105 }
106 #elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_AMI601
107 static inline void foo_handler(void) {}
108 static inline void ImuMagEvent(void)
109 {
110  AMI601Event(foo_handler);
116  imu_scale_mag(&imu);
117  AbiSendMsgIMU_MAG_INT32(IMU_B2_ID, get_sys_time_usec(), &imu.mag);
118  }
119 }
120 #elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC5843
121 static inline void foo_handler(void) {}
122 static inline void ImuMagEvent(void)
123 {
125  if (hmc5843.data_available) {
129  imu_scale_mag(&imu);
130  AbiSendMsgIMU_MAG_INT32(IMU_B2_ID, get_sys_time_usec(), &imu.mag);
131  hmc5843.data_available = false;
132  }
133 }
134 #elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC58XX
135 static inline void ImuMagEvent(void)
136 {
137  hmc58xx_event(&imu_b2.mag_hmc);
138  if (imu_b2.mag_hmc.data_available) {
139  imu.mag_unscaled.x = imu_b2.mag_hmc.data.value[IMU_MAG_X_CHAN];
140  imu.mag_unscaled.y = imu_b2.mag_hmc.data.value[IMU_MAG_Y_CHAN];
141  imu.mag_unscaled.z = imu_b2.mag_hmc.data.value[IMU_MAG_Z_CHAN];
142  imu_scale_mag(&imu);
143  AbiSendMsgIMU_MAG_INT32(IMU_B2_ID, get_sys_time_usec(), &imu.mag);
144  imu_b2.mag_hmc.data_available = false;
145  }
146 }
147 #else
148 #define ImuMagEvent() {}
149 #endif
150 
151 
152 void imu_b2_event(void)
153 {
154  max1168_event();
156  uint32_t now_ts = get_sys_time_usec();
166  AbiSendMsgIMU_GYRO_INT32(IMU_ASPIRIN2_ID, now_ts, &imu.gyro);
167  AbiSendMsgIMU_ACCEL_INT32(IMU_ASPIRIN2_ID, now_ts, &imu.accel);
168  }
169  ImuMagEvent();
170 }
void ami601_init(void)
Definition: ami601.c:12
void max1168_init(void)
Definition: max1168.c:40
void max1168_event(void)
Definition: max1168.c:101
#define IMU_MAG_Y_CHAN
Definition: imu_hbmini.c:57
uint16_t max1168_values[MAX1168_NB_CHAN]
Definition: max1168.c:28
void hmc5843_idle_task(void)
Definition: hmc5843.c:60
void imu_b2_periodic(void)
Definition: imu_b2.c:57
#define IMU_MAG_Z_SENS_DEN
Definition: imu_ardrone2.h:79
uint8_t data_available
Definition: hmc5843.h:34
#define IMU_MAG_X_SIGN
Definition: imu_hbmini.h:55
#define AMI601_IDLE
Definition: ami601.h:19
#define IMU_GYRO_P_CHAN
Definition: imu_hbmini.c:46
Main include for ABI (AirBorneInterface).
void ms2100_event(struct Ms2100 *ms)
Definition: ms2100.c:105
volatile uint8_t ami601_status
Definition: ami601.c:8
struct Imu imu
global IMU state
Definition: imu.c:108
#define IMU_MAG_Z_SIGN
Definition: imu_hbmini.h:57
#define MAX1168_IDLE
Definition: max1168.h:59
#define HMC58XX_ADDR
Definition: hmc58xx_regs.h:31
struct ImuBooz2 imu_b2
Definition: imu_b2.c:34
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:48
int32_t r
in rad/s with INT32_RATE_FRAC
#define IMU_MAG_Y_SENS_NUM
Definition: imu_ardrone2.h:75
#define IMU_MAG_X_SENS_NUM
Definition: imu_ardrone2.h:72
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
Definition: imu.h:46
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:39
#define IMU_ACCEL_Y_CHAN
Definition: imu_hbmini.c:52
uint16_t ami601_values[AMI601_NB_CHAN]
Definition: ami601.c:6
void hmc5843_init(void)
Definition: hmc5843.c:16
struct Int32Vect3 mag_neutral
magnetometer neutral readings (bias) in raw/unscaled units
Definition: imu.h:45
Architecture independent timing functions.
#define IMU_GYRO_Q_CHAN
Definition: imu_hbmini.c:47
union Hmc5843::@308 data
unsigned long uint32_t
Definition: types.h:18
#define IMU_GYRO_R_CHAN
Definition: imu_hbmini.c:48
struct Ms2100 ms2100
Definition: ms2100.c:44
#define Max1168Periodic()
Definition: max1168.h:68
#define IMU_MAG_Z_CHAN
Definition: imu_hbmini.c:58
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
Definition: imu.h:40
Inertial Measurement Unit interface.
void imu_scale_gyro(struct Imu *_imu)
Definition: imu_vectornav.c:43
#define IMU_ACCEL_Z_CHAN
Definition: imu_hbmini.c:53
signed long int32_t
Definition: types.h:19
#define IMU_MAG_X_SENS_DEN
Definition: imu_ardrone2.h:73
volatile uint8_t max1168_status
Definition: max1168.c:27
#define IMU_ASPIRIN2_ID
void imu_b2_event(void)
Definition: imu_b2.c:152
#define IMU_MAG_Y_SIGN
Definition: imu_hbmini.h:56
union Ms2100::@330 data
void imu_scale_mag(struct Imu *_imu)
Definition: ahrs_gx3.c:351
#define MAX1168_DATA_AVAILABLE
Definition: max1168.h:63
void ami601_read(void)
Definition: ami601.c:26
#define AMI601Event(_handler)
Definition: ami601.h:38
static void hmc58xx_periodic(struct Hmc58xx *hmc)
convenience function: read or start configuration if not already initialized
Definition: hmc58xx.h:85
#define IMU_MAG_X_CHAN
Definition: imu_hbmini.c:56
void ms2100_init(struct Ms2100 *ms, struct spi_periph *spi_p, uint8_t slave_idx)
Definition: ms2100.c:47
#define IMU_MAG_Z_SENS_NUM
Definition: imu_ardrone2.h:78
#define AMI601_DATA_AVAILABLE
Definition: ami601.h:23
int32_t p
in rad/s with INT32_RATE_FRAC
arch independent LED (Light Emitting Diodes) API
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
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
Definition: imu.h:47
void imu_scale_accel(struct Imu *_imu)
Definition: imu_vectornav.c:44
static void ms2100_periodic(struct Ms2100 *ms)
Definition: ms2100.h:65
#define IMU_B2_ID
abstract IMU interface providing fixed point interface
Definition: imu.h:37
#define ImuMagEvent()
Event functions for imu_b2.
Definition: imu_b2.c:148
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
enum Ms2100Status status
Definition: ms2100.h:50
void imu_b2_init(void)
Definition: imu_b2.c:38
void hmc58xx_event(struct Hmc58xx *hmc)
Definition: hmc58xx.c:152
#define IMU_ACCEL_X_CHAN
Definition: imu_hbmini.c:51
int32_t q
in rad/s with INT32_RATE_FRAC
struct Hmc5843 hmc5843
Definition: hmc5843.c:9
struct Int32Rates gyro
gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC
Definition: imu.h:38
#define IMU_MAG_Y_SENS_DEN
Definition: imu_ardrone2.h:76