Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
imu.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2010 The Paparazzi Team
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 
27 #include BOARD_CONFIG
28 #include "subsystems/imu.h"
29 #include "state.h"
30 
31 #ifdef IMU_POWER_GPIO
32 #include "mcu_periph/gpio.h"
33 
34 #ifndef IMU_POWER_GPIO_ON
35 #define IMU_POWER_GPIO_ON gpio_set
36 #endif
37 #endif
38 
39 #if PERIODIC_TELEMETRY
41 
42 #if USE_IMU_FLOAT
43 
44 static void send_accel(void) {
45  DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, DefaultDevice,
46  &imuf.accel.x, &imuf.accel.y, &imuf.accel.z);
47 }
48 
49 static void send_gyro(void) {
50  DOWNLINK_SEND_IMU_GYRO(DefaultChannel, DefaultDevice,
51  &imuf.gyro.p, &imuf.gyro.q, &imuf.gyro.r);
52 }
53 
54 #else // !USE_IMU_FLOAT
55 
56 static void send_accel_raw(void) {
57  DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice,
59 }
60 
61 static void send_accel_scaled(void) {
62  DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, DefaultDevice,
63  &imu.accel.x, &imu.accel.y, &imu.accel.z);
64 }
65 
66 static void send_accel(void) {
67  struct FloatVect3 accel_float;
69  DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, DefaultDevice,
71 }
72 
73 static void send_gyro_raw(void) {
74  DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice,
76 }
77 
78 static void send_gyro_scaled(void) {
79  DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, DefaultDevice,
80  &imu.gyro.p, &imu.gyro.q, &imu.gyro.r);
81 }
82 
83 static void send_gyro(void) {
84  struct FloatRates gyro_float;
85  RATES_FLOAT_OF_BFP(gyro_float, imu.gyro);
86  DOWNLINK_SEND_IMU_GYRO(DefaultChannel, DefaultDevice,
87  &gyro_float.p, &gyro_float.q, &gyro_float.r);
88 }
89 
90 static void send_mag_raw(void) {
91  DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice,
93 }
94 
95 static void send_mag_scaled(void) {
96  DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, DefaultDevice,
97  &imu.mag.x, &imu.mag.y, &imu.mag.z);
98 }
99 
100 static void send_mag(void) {
101  struct FloatVect3 mag_float;
102  MAGS_FLOAT_OF_BFP(mag_float, imu.mag);
103  DOWNLINK_SEND_IMU_MAG(DefaultChannel, DefaultDevice,
104  &mag_float.x, &mag_float.y, &mag_float.z);
105 }
106 #endif // !USE_IMU_FLOAT
107 
108 #endif
109 
110 struct Imu imu;
111 struct ImuFloat imuf;
112 
113 void imu_init(void) {
114 
115 #ifdef IMU_POWER_GPIO
117  IMU_POWER_GPIO_ON(IMU_POWER_GPIO);
118 #endif
119 
120  /* initialises neutrals */
122 
124 
125 #if defined IMU_MAG_X_NEUTRAL && defined IMU_MAG_Y_NEUTRAL && defined IMU_MAG_Z_NEUTRAL
127 #else
128 #if USE_MAGNETOMETER
129 INFO("Magnetometer neutrals are set to zero, you should calibrate!")
130 #endif
132 #endif
133 
134  struct FloatEulers body_to_imu_eulers =
136  orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
137 
138 #if PERIODIC_TELEMETRY
139  register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel);
140  register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro);
141 #if !USE_IMU_FLOAT
142  register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_RAW", send_accel_raw);
143  register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_SCALED", send_accel_scaled);
144  register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel);
145  register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_RAW", send_gyro_raw);
146  register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_SCALED", send_gyro_scaled);
147  register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro);
148  register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_RAW", send_mag_raw);
149  register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_SCALED", send_mag_scaled);
150  register_periodic_telemetry(DefaultPeriodic, "IMU_MAG", send_mag);
151 #endif // !USE_IMU_FLOAT
152 #endif // DOWNLINK
153 
154  imu_impl_init();
155 }
156 
157 
158 void imu_float_init(void) {
159  struct FloatEulers body_to_imu_eulers =
161  orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers);
162 }
163 
165  struct FloatEulers imu_to_body_eulers;
166  memcpy(&imu_to_body_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers));
167  imu_to_body_eulers.phi = phi;
168  orientationSetEulers_f(&imu.body_to_imu, &imu_to_body_eulers);
169 }
170 
172  struct FloatEulers imu_to_body_eulers;
173  memcpy(&imu_to_body_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers));
174  imu_to_body_eulers.theta = theta;
175  orientationSetEulers_f(&imu.body_to_imu, &imu_to_body_eulers);
176 }
177 
179  struct FloatEulers imu_to_body_eulers;
180  memcpy(&imu_to_body_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers));
181  imu_to_body_eulers.psi = psi;
182  orientationSetEulers_f(&imu.body_to_imu, &imu_to_body_eulers);
183 }
184 
185 void imu_SetBodyToImuCurrent(float set) {
186  imu.b2i_set_current = set;
187 
188  if (imu.b2i_set_current) {
189  // adjust imu_to_body roll and pitch by current NedToBody roll and pitch
190  struct FloatEulers imu_to_body_eulers;
191  memcpy(&imu_to_body_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers));
192  if (stateIsAttitudeValid()) {
193  // adjust imu_to_body roll and pitch by current NedToBody roll and pitch
194  imu_to_body_eulers.phi += stateGetNedToBodyEulers_f()->phi;
195  imu_to_body_eulers.theta += stateGetNedToBodyEulers_f()->theta;
196  orientationSetEulers_f(&imu.body_to_imu, &imu_to_body_eulers);
197  }
198  else {
199  // indicate that we couldn't set to current roll/pitch
201  }
202  }
203  else {
204  // reset to BODY_TO_IMU as defined in airframe file
205  struct FloatEulers imu_to_body_eulers =
207  orientationSetEulers_f(&imu.body_to_imu, &imu_to_body_eulers);
208  }
209 }
210 
#define IMU_BODY_TO_IMU_THETA
Definition: imu.h:91
struct OrientationReps body_to_imu
rotation from body to imu frame
Definition: imu.h:66
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1027
struct FloatVect3 accel_float
void imu_float_init(void)
Definition: imu.c:158
int32_t p
in rad/s with INT32_RATE_FRAC
Periodic telemetry system header (includes downlink utility and generated code).
angular rates
static bool_t stateIsAttitudeValid(void)
Test if attitudes are valid.
Definition: state.h:963
float psi
in radians
Some architecture independent helper functions for GPIOs.
struct FloatRates gyro
Definition: imu.h:62
#define IMU_MAG_X_NEUTRAL
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:42
struct OrientationReps body_to_imu
rotation from body to imu frame
Definition: imu.h:52
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
Definition: imu.h:49
void imu_SetBodyToImuPhi(float phi)
Definition: imu.c:164
abstract IMU interface providing fixed point interface
Definition: imu.h:40
struct ImuFloat imuf
Definition: imu.c:111
float theta
in radians
struct Imu imu
global IMU state
Definition: imu.c:110
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
euler angles
struct Int32Rates gyro_neutral
gyroscope bias
Definition: imu.h:46
#define IMU_GYRO_P_NEUTRAL
Definition: imu_apogee.h:69
#define FALSE
Definition: imu_chimu.h:141
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:720
#define IMU_MAG_Y_NEUTRAL
float p
in rad/s
void imu_impl_init(void)
Definition: imu_apogee.c:68
struct FloatVect3 accel
Definition: imu.h:63
struct Int32Vect3 mag_neutral
magnetometer neutral readings (bias)
Definition: imu.h:48
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
Definition: imu.h:50
float phi
in radians
#define INT_VECT3_ZERO(_v)
Inertial Measurement Unit interface.
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:672
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:51
float r
in rad/s
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
Definition: imu.h:43
#define IMU_ACCEL_Z_NEUTRAL
Definition: imu_apogee.h:94
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:107
void imu_SetBodyToImuCurrent(float set)
Definition: imu.c:185
API to get/set the generic vehicle states.
static void gpio_setup_output(uint32_t port, uint32_t gpios)
Setup one or more pins of the given GPIO port as outputs.
Definition: gpio_arch.h:76
struct Int32Vect3 accel_neutral
accelerometer bias
Definition: imu.h:47
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:319
bool_t b2i_set_current
flag for adjusting body_to_imu via settings.
Definition: imu.h:57
static struct FloatEulers * orientationGetEulers_f(struct OrientationReps *orientation)
Get vehicle body attitude euler angles (float).
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:708
void imu_SetBodyToImuPsi(float psi)
Definition: imu.c:178
int32_t q
in rad/s with INT32_RATE_FRAC
struct Int32Rates gyro
gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC
Definition: imu.h:41
#define IMU_ACCEL_Y_NEUTRAL
Definition: imu_apogee.h:93
float q
in rad/s
#define IMU_MAG_Z_NEUTRAL
int32_t r
in rad/s with INT32_RATE_FRAC
#define IMU_BODY_TO_IMU_PSI
Definition: imu.h:92
void imu_init(void)
Definition: imu.c:113
abstract IMU interface providing floating point interface
Definition: imu.h:61
#define IMU_GYRO_Q_NEUTRAL
Definition: imu_apogee.h:70
#define IMU_GYRO_R_NEUTRAL
Definition: imu_apogee.h:71
#define IMU_BODY_TO_IMU_PHI
Definition: imu.h:90
#define IMU_ACCEL_X_NEUTRAL
Definition: imu_apogee.h:92
void imu_SetBodyToImuTheta(float theta)
Definition: imu.c:171
static void orientationSetEulers_f(struct OrientationReps *orientation, struct FloatEulers *eulers)
Set vehicle body attitude from euler angles (float).