Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
imu_apogee.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2013 Gautier Hattenberger
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
30#include <math.h>
32#include "mcu_periph/i2c.h"
33#include "led.h"
34#include "modules/core/abi.h"
35#include "generated/modules.h"
36
37// Downlink
38#include "mcu_periph/uart.h"
39#include "pprzlink/messages.h"
41
42#ifndef IMU_APOGEE_CHAN_X
43#define IMU_APOGEE_CHAN_X 0
44#endif
45#ifndef IMU_APOGEE_CHAN_Y
46#define IMU_APOGEE_CHAN_Y 1
47#endif
48#ifndef IMU_APOGEE_CHAN_Z
49#define IMU_APOGEE_CHAN_Z 2
50#endif
51
52#if !defined APOGEE_LOWPASS_FILTER && !defined APOGEE_SMPLRT_DIV
53#define APOGEE_LOWPASS_FILTER MPU60X0_DLPF_42HZ
54#define APOGEE_SMPLRT_DIV 9
55PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz")
56#endif
59
62
63#if APOGEE_USE_MPU9150
68#ifndef APOGEE_MAG_FREQ
69#define APOGEE_MAG_FREQ 50
70#endif
71PRINT_CONFIG_VAR(APOGEE_MAG_FREQ)
74#define MAG_PRESCALER Max(1,((PERIODIC_FREQUENCY)/APOGEE_MAG_FREQ))
75PRINT_CONFIG_VAR(MAG_PRESCALER)
76
77// mag config will be done later in bypass mode
78bool configure_mag_slave(Mpu60x0ConfigSet mpu_set, void *mpu);
79bool configure_mag_slave(Mpu60x0ConfigSet mpu_set __attribute__((unused)), void *mpu __attribute__((unused)))
80{
81 return true;
82}
83
84#endif
85
87
88// baro config will be done later in bypass mode
89bool configure_baro_slave(Mpu60x0ConfigSet mpu_set, void *mpu);
90bool configure_baro_slave(Mpu60x0ConfigSet mpu_set __attribute__((unused)), void *mpu __attribute__((unused)))
91{
92 return true;
93}
94
96{
98 // MPU-60X0
99 mpu60x0_i2c_init(&imu_apogee.mpu, &(IMU_APOGEE_I2C_DEV), MPU60X0_ADDR_ALT);
100 // change the default configuration
101 imu_apogee.mpu.config.smplrt_div = APOGEE_SMPLRT_DIV;
102 imu_apogee.mpu.config.dlpf_cfg = APOGEE_LOWPASS_FILTER;
103 imu_apogee.mpu.config.gyro_range = APOGEE_GYRO_RANGE;
104 imu_apogee.mpu.config.accel_range = APOGEE_ACCEL_RANGE;
105 // set MPU in bypass mode for the baro
106 imu_apogee.mpu.config.nb_slaves = 1;
107 imu_apogee.mpu.config.slaves[0].configure = &configure_baro_slave;
108 imu_apogee.mpu.config.i2c_bypass = true;
109 // set the default values
112
113#if APOGEE_USE_MPU9150
114 // if using MPU9150, internal mag needs to be configured
115 ak8975_init(&imu_apogee.ak, &(IMU_APOGEE_I2C_DEV), AK8975_I2C_SLV_ADDR);
116 imu_apogee.mpu.config.nb_slaves = 2;
117 imu_apogee.mpu.config.slaves[1].configure = &configure_mag_slave;
118#endif
119}
120
122{
123 // Start reading the latest gyroscope data
125
126#if APOGEE_USE_MPU9150
127 // Start reading internal mag if available
128 RunOnceEvery(MAG_PRESCALER, ak8975_periodic(&imu_apogee.ak));
129#endif
130}
131
133{
134 uint32_t now_ts = get_sys_time_usec();
135
136 // If the itg3200 I2C transaction has succeeded: convert the data
138 if (imu_apogee.mpu.data_available) {
139 struct Int32Rates rates = {
140 (int32_t)( imu_apogee.mpu.data_rates.value[IMU_APOGEE_CHAN_X]),
141 (int32_t)(-imu_apogee.mpu.data_rates.value[IMU_APOGEE_CHAN_Y]),
142 (int32_t)(-imu_apogee.mpu.data_rates.value[IMU_APOGEE_CHAN_Z])
143 };
144 AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &rates, 1, IMU_APOGEE_PERIODIC_FREQ, imu_apogee.mpu.temp);
145 struct Int32Vect3 accel = {
146 (int32_t)( imu_apogee.mpu.data_accel.value[IMU_APOGEE_CHAN_X]),
147 (int32_t)(-imu_apogee.mpu.data_accel.value[IMU_APOGEE_CHAN_Y]),
148 (int32_t)(-imu_apogee.mpu.data_accel.value[IMU_APOGEE_CHAN_Z])
149 };
150 AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1, IMU_APOGEE_PERIODIC_FREQ, imu_apogee.mpu.temp);
151 imu_apogee.mpu.data_available = false;
152 }
153
154#if APOGEE_USE_MPU9150
156 if (imu_apogee.ak.data_available) {
157 struct Int32Vect3 mag = {
158 (int32_t)( imu_apogee.ak.data.value[IMU_APOGEE_CHAN_Y]),
159 (int32_t)(-imu_apogee.ak.data.value[IMU_APOGEE_CHAN_X]),
160 (int32_t)( imu_apogee.ak.data.value[IMU_APOGEE_CHAN_Z])
161 };
162 AbiSendMsgIMU_MAG_RAW(IMU_BOARD_ID, now_ts, &mag);
163 imu_apogee.ak.data_available = false;
164 }
165#endif
166}
167
Main include for ABI (AirBorneInterface).
#define IMU_BOARD_ID
void ak8975_init(struct Ak8975 *ak, struct i2c_periph *i2c_p, uint8_t addr)
Definition ak8975.c:47
void ak8975_event(struct Ak8975 *ak)
Definition ak8975.c:131
#define AK8975_I2C_SLV_ADDR
Definition ak8975.h:35
static void ak8975_periodic(struct Ak8975 *ak)
convenience function: read or start configuration if not already initialized
Definition ak8975.h:83
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
angular rates
Architecture independent I2C (Inter-Integrated Circuit Bus) API.
void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct FloatRates *scale_f)
Set the defaults for a gyro sensor WARNING: Should be called before sensor is publishing messages to ...
Definition imu.c:842
void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct FloatVect3 *scale_f)
Set the defaults for a accel sensor WARNING: Should be called before sensor is publishing messages to...
Definition imu.c:871
struct ImuApogee imu_apogee
Definition imu_apogee.c:86
#define APOGEE_SMPLRT_DIV
Definition imu_apogee.c:54
#define IMU_APOGEE_CHAN_X
Definition imu_apogee.c:43
void imu_apogee_periodic(void)
Definition imu_apogee.c:121
bool configure_baro_slave(Mpu60x0ConfigSet mpu_set, void *mpu)
Definition imu_apogee.c:90
void imu_apogee_init(void)
Definition imu_apogee.c:95
void imu_apogee_event(void)
Definition imu_apogee.c:132
#define IMU_APOGEE_CHAN_Z
Definition imu_apogee.c:49
#define APOGEE_LOWPASS_FILTER
Definition imu_apogee.c:53
#define IMU_APOGEE_CHAN_Y
Definition imu_apogee.c:46
Driver for the IMU on the Apogee board.
#define APOGEE_GYRO_RANGE
Definition imu_apogee.h:44
struct Mpu60x0_I2c mpu
Definition imu_apogee.h:51
#define APOGEE_ACCEL_RANGE
Definition imu_apogee.h:47
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
arch independent LED (Light Emitting Diodes) API
const struct FloatVect3 MPU60X0_ACCEL_SENS_F[4]
Definition mpu60x0.c:54
const struct FloatRates MPU60X0_GYRO_SENS_F[4]
Definition mpu60x0.c:38
void(* Mpu60x0ConfigSet)(void *mpu, uint8_t _reg, uint8_t _val)
Configuration function prototype.
Definition mpu60x0.h:112
void mpu60x0_i2c_init(struct Mpu60x0_I2c *mpu, struct i2c_periph *i2c_p, uint8_t addr)
Definition mpu60x0_i2c.c:31
void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu)
Definition mpu60x0_i2c.c:82
static void mpu60x0_i2c_periodic(struct Mpu60x0_I2c *mpu)
convenience function: read or start configuration if not already initialized
Definition mpu60x0_i2c.h:79
#define MPU60X0_ADDR_ALT
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.