Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
ahrs_float_invariant_wrapper.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2015 Jean-Philippe Condomines, Gautier Hattenberger
3  * 2015 Felix Ruess <felix.ruess@gmail.com>
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, see
19  * <http://www.gnu.org/licenses/>.
20  */
21 
29 #include "modules/ahrs/ahrs.h"
30 #include "modules/core/abi.h"
31 #include "mcu_periph/sys_time.h"
32 #include "message_pragmas.h"
33 #include "state.h"
34 
35 #ifndef AHRS_FINV_OUTPUT_ENABLED
36 #define AHRS_FINV_OUTPUT_ENABLED TRUE
37 #endif
38 PRINT_CONFIG_VAR(AHRS_FINV_OUTPUT_ENABLED)
39 
40 
45 
46 static void compute_body_orientation_and_rates(void);
47 
48 #if PERIODIC_TELEMETRY
50 
51 static void send_att(struct transport_tx *trans, struct link_device *dev)
52 {
53  /* compute Eulers in int (body frame) */
54  struct FloatEulers ltp_to_body_euler;
55  float_eulers_of_quat(&ltp_to_body_euler, &ahrs_float_inv.state.quat);
56  struct Int32Eulers eulers_body;
57  EULERS_BFP_OF_REAL(eulers_body, ltp_to_body_euler);
58 
59  pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
60  &eulers_body.phi,
61  &eulers_body.theta,
62  &eulers_body.psi,
63  &eulers_body.phi,
64  &eulers_body.theta,
65  &eulers_body.psi,
66  &ahrs_finv_id);
67 }
68 
69 static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
70 {
71  pprz_msg_send_GEO_MAG(trans, dev, AC_ID,
75 }
76 
77 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
78 {
79  uint8_t mde = 3;
80  uint16_t val = 0;
81  if (!ahrs_float_inv.is_aligned) { mde = 2; }
83  /* set lost if no new gyro measurements for 50ms */
84  if (t_diff > 50000) { mde = 5; }
85  pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_finv_id, &mde, &val);
86 }
87 #endif
88 
89 
90 /*
91  * ABI bindings
92  */
94 #ifndef AHRS_FINV_IMU_ID
95 #define AHRS_FINV_IMU_ID ABI_BROADCAST
96 #endif
97 PRINT_CONFIG_VAR(AHRS_FINV_IMU_ID)
98 
99 
100 #ifndef AHRS_FINV_MAG_ID
101 #define AHRS_FINV_MAG_ID AHRS_FINV_IMU_ID
102 #endif
103 PRINT_CONFIG_VAR(AHRS_FINV_MAG_ID)
104 
110 
116 static void gyro_cb(uint8_t sender_id __attribute__((unused)),
117  uint32_t stamp, struct Int32Rates *gyro)
118 {
119  struct FloatRates gyro_f;
120  RATES_FLOAT_OF_BFP(gyro_f, *gyro);
121 
122 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
123  PRINT_CONFIG_MSG("Calculating dt for AHRS float_invariant propagation.")
124  /* timestamp in usec when last callback was received */
125  static uint32_t last_stamp = 0;
126 
127  if (last_stamp > 0 && ahrs_float_inv.is_aligned) {
128  float dt = (float)(stamp - last_stamp) * 1e-6;
129  ahrs_float_invariant_propagate(&gyro_f, dt);
131  }
132  last_stamp = stamp;
133 #else
134  PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS float_invariant propagation.")
135  PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
136  const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
138  ahrs_float_invariant_propagate(&gyro_f, dt);
140  }
141 #endif
142 
143  ahrs_finv_last_stamp = stamp;
144 }
145 
146 static void accel_cb(uint8_t sender_id __attribute__((unused)),
147  uint32_t stamp __attribute__((unused)),
148  struct Int32Vect3 *accel)
149 {
151  struct FloatVect3 accel_f;
152  ACCELS_FLOAT_OF_BFP(accel_f, *accel);
154  }
155 }
156 
157 static void mag_cb(uint8_t sender_id __attribute__((unused)),
158  uint32_t stamp __attribute__((unused)),
159  struct Int32Vect3 *mag)
160 {
162  struct FloatVect3 mag_f;
163  MAGS_FLOAT_OF_BFP(mag_f, *mag);
165  }
166 }
167 
168 static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
169  uint32_t stamp __attribute__((unused)),
170  struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
171  struct Int32Vect3 *lp_mag)
172 {
173  if (!ahrs_float_inv.is_aligned) {
174  /* convert to float */
175  struct FloatRates gyro_f;
176  RATES_FLOAT_OF_BFP(gyro_f, *lp_gyro);
177  struct FloatVect3 accel_f;
178  ACCELS_FLOAT_OF_BFP(accel_f, *lp_accel);
179  struct FloatVect3 mag_f;
180  MAGS_FLOAT_OF_BFP(mag_f, *lp_mag);
181  ahrs_float_invariant_align(&gyro_f, &accel_f, &mag_f);
183  }
184 }
185 
186 static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
187 {
189 }
190 
191 static bool ahrs_float_invariant_enable_output(bool enable)
192 {
193  ahrs_finv_output_enabled = enable;
195 }
196 
201 {
203  /* Set state */
205 
206  /* compute body rates */
207  struct FloatRates body_rate;
209  stateSetBodyRates_f(&body_rate);
210  }
211 }
212 
213 
215 {
219 
220  /*
221  * Subscribe to scaled IMU measurements and attach callbacks
222  */
223  AbiBindMsgIMU_MAG(AHRS_FINV_MAG_ID, &mag_ev, mag_cb);
224  AbiBindMsgIMU_GYRO(AHRS_FINV_IMU_ID, &gyro_ev, gyro_cb);
225  AbiBindMsgIMU_ACCEL(AHRS_FINV_IMU_ID, &accel_ev, accel_cb);
226  AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
227  AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
228 
229 #if PERIODIC_TELEMETRY
230  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_EULER_INT, send_att);
232  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
233 #endif
234 }
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
Main include for ABI (AirBorneInterface).
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:58
Event structure to store callbacks in a linked list.
Definition: abi_common.h:67
void ahrs_register_impl(AhrsEnableOutput enable)
Register an AHRS implementation.
Definition: ahrs.c:62
Dispatcher to register actual AHRS implementations.
#define AHRS_COMP_ID_FINV
Definition: ahrs.h:39
struct AhrsFloatInv ahrs_float_inv
void ahrs_float_invariant_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
void ahrs_float_invariant_init(void)
void ahrs_float_invariant_propagate(struct FloatRates *gyro, float dt)
void ahrs_float_invariant_update_accel(struct FloatVect3 *accel)
void ahrs_float_invariant_update_mag(struct FloatVect3 *mag)
struct inv_command cmd
command vector
struct inv_state state
state vector
struct FloatVect3 mag_h
static uint32_t ahrs_finv_last_stamp
last gyro msg timestamp
static void compute_body_orientation_and_rates(void)
Compute body orientation and rates from imu orientation and rates.
static abi_event mag_ev
static abi_event accel_ev
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
Call ahrs_float_invariant_propagate on new gyro measurements.
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
static abi_event gyro_ev
static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
static abi_event aligner_ev
static abi_event geo_mag_ev
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
#define AHRS_FINV_IMU_ID
IMU (gyro, accel)
static uint8_t ahrs_finv_id
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
static void geo_mag_cb(uint8_t sender_id, struct FloatVect3 *h)
static void aligner_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
#define AHRS_FINV_OUTPUT_ENABLED
static bool ahrs_float_invariant_enable_output(bool enable)
static bool ahrs_finv_output_enabled
if TRUE with push the estimation results to the state interface
#define AHRS_FINV_MAG_ID
magnetometer
static void send_att(struct transport_tx *trans, struct link_device *dev)
void ahrs_float_invariant_register(void)
Paparazzi specific wrapper to run INVARIANT ahrs filter.
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:71
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
euler angles
angular rates
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:807
#define EULERS_BFP_OF_REAL(_ei, _ef)
Definition: pprz_algebra.h:715
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:372
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:759
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
int32_t phi
in rad with INT32_ANGLE_FRAC
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t theta
in rad with INT32_ANGLE_FRAC
euler angles
angular rates
static void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition: state.h:1093
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
#define AHRS_PROPAGATE_FREQUENCY
Definition: hf_float.c:55
struct FloatRates bias
Estimated gyro biases.
struct FloatQuat quat
Estimated attitude (quaternion)
struct FloatRates rates
Input gyro rates.
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
Architecture independent timing functions.
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
uint16_t val[TCOUPLE_NB]
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
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