Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
27
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
35PRINT_CONFIG_VAR(AHRS_FINV_TYPE)
36
41
43
44#if PERIODIC_TELEMETRY
46
47static void send_att(struct transport_tx *trans, struct link_device *dev)
48{
49 /* compute Eulers in int (body frame) */
50 struct FloatEulers ltp_to_body_euler;
51 float_eulers_of_quat(&ltp_to_body_euler, &ahrs_float_inv.state.quat);
52 struct Int32Eulers eulers_body;
53 EULERS_BFP_OF_REAL(eulers_body, ltp_to_body_euler);
54
55 pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
56 &eulers_body.phi,
57 &eulers_body.theta,
58 &eulers_body.psi,
59 &eulers_body.phi,
60 &eulers_body.theta,
61 &eulers_body.psi,
63}
64
65static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
66{
67 pprz_msg_send_GEO_MAG(trans, dev, AC_ID,
68 &ahrs_float_inv.mag_h.x,
69 &ahrs_float_inv.mag_h.y,
70 &ahrs_float_inv.mag_h.z, &ahrs_finv_id);
71}
72
73static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
74{
75 uint8_t mde = 3;
76 uint16_t val = 0;
77 if (!ahrs_float_inv.is_aligned) { mde = 2; }
79 /* set lost if no new gyro measurements for 50ms */
80 if (t_diff > 50000) { mde = 5; }
81 pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_finv_id, &mde, &val);
82}
83#endif
84
85
86/*
87 * ABI bindings
88 */
90#ifndef AHRS_FINV_IMU_ID
91#define AHRS_FINV_IMU_ID ABI_BROADCAST
92#endif
94
95
96#ifndef AHRS_FINV_MAG_ID
97#define AHRS_FINV_MAG_ID AHRS_FINV_IMU_ID
98#endif
100
106
112static void gyro_cb(uint8_t sender_id __attribute__((unused)),
113 uint32_t stamp, struct Int32Rates *gyro)
114{
115 struct FloatRates gyro_f;
116 RATES_FLOAT_OF_BFP(gyro_f, *gyro);
117
118#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
119 PRINT_CONFIG_MSG("Calculating dt for AHRS float_invariant propagation.")
120 /* timestamp in usec when last callback was received */
121 static uint32_t last_stamp = 0;
122
123 if (last_stamp > 0 && ahrs_float_inv.is_aligned) {
124 float dt = (float)(stamp - last_stamp) * 1e-6;
127 }
128 last_stamp = stamp;
129#else
130 PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS float_invariant propagation.")
132 const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
133 if (ahrs_float_inv.is_aligned) {
136 }
137#endif
138
139 ahrs_finv_last_stamp = stamp;
140}
141
142static void accel_cb(uint8_t sender_id __attribute__((unused)),
143 uint32_t stamp __attribute__((unused)),
144 struct Int32Vect3 *accel)
145{
146 if (ahrs_float_inv.is_aligned) {
147 struct FloatVect3 accel_f;
148 ACCELS_FLOAT_OF_BFP(accel_f, *accel);
150 }
151}
152
153static void mag_cb(uint8_t sender_id __attribute__((unused)),
154 uint32_t stamp __attribute__((unused)),
155 struct Int32Vect3 *mag)
156{
157 if (ahrs_float_inv.is_aligned) {
158 struct FloatVect3 mag_f;
159 MAGS_FLOAT_OF_BFP(mag_f, *mag);
161 }
162}
163
164static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
165 uint32_t stamp __attribute__((unused)),
166 struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
167 struct Int32Vect3 *lp_mag)
168{
169 if (!ahrs_float_inv.is_aligned) {
170 /* convert to float */
171 struct FloatRates gyro_f;
172 RATES_FLOAT_OF_BFP(gyro_f, *lp_gyro);
173 struct FloatVect3 accel_f;
174 ACCELS_FLOAT_OF_BFP(accel_f, *lp_accel);
175 struct FloatVect3 mag_f;
176 MAGS_FLOAT_OF_BFP(mag_f, *lp_mag);
177 ahrs_float_invariant_align(&gyro_f, &accel_f, &mag_f);
179 }
180}
181
182static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
183{
184 ahrs_float_inv.mag_h = *h;
185}
186
191{
192 /* Set state */
193 stateSetNedToBodyQuat_f(MODULE_AHRS_FLOAT_INVARIANT_ID, &ahrs_float_inv.state.quat);
194
195 /* compute body rates */
196 struct FloatRates body_rate;
197 RATES_DIFF(body_rate, ahrs_float_inv.cmd.rates, ahrs_float_inv.state.bias);
198 stateSetBodyRates_f(MODULE_AHRS_FLOAT_INVARIANT_ID, &body_rate);
199}
200
201
203{
205 if (AHRS_FINV_TYPE == AHRS_PRIMARY) {
207 } else {
209 }
210
211 /*
212 * Subscribe to scaled IMU measurements and attach callbacks
213 */
214 AbiBindMsgIMU_MAG(AHRS_FINV_MAG_ID, &mag_ev, mag_cb);
215 AbiBindMsgIMU_GYRO(AHRS_FINV_IMU_ID, &gyro_ev, gyro_cb);
216 AbiBindMsgIMU_ACCEL(AHRS_FINV_IMU_ID, &accel_ev, accel_cb);
217 AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
218 AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
219
220#if PERIODIC_TELEMETRY
221 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_EULER_INT, send_att);
223 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
224#endif
225}
226
228{
229 if (enable) {
230 stateSetInputFilter(STATE_INPUT_ATTITUDE, MODULE_AHRS_FLOAT_INVARIANT_ID);
231 stateSetInputFilter(STATE_INPUT_RATES, MODULE_AHRS_FLOAT_INVARIANT_ID);
232 }
233 ahrs_finv_enable = enable;
234}
235
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
#define ABI_BROADCAST
Broadcast address.
Definition abi_common.h:80
#define AHRS_COMP_ID_FINV
Definition ahrs.h:42
#define AHRS_PRIMARY
Definition ahrs.h:32
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
static abi_event gyro_ev
static void compute_body_orientation_and_rates(void)
Compute body orientation and rates from imu orientation and rates.
static abi_event mag_ev
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
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)
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)
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)
uint8_t ahrs_finv_enable
void ahrs_finv_wrapper_init(void)
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 void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
void ahrs_float_invariant_wrapper_enable(uint8_t enable)
static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
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_MAG_ID
magnetometer
static void send_att(struct transport_tx *trans, struct link_device *dev)
Paparazzi specific wrapper to run INVARIANT ahrs filter.
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
static struct uart_periph * dev
static abi_event accel_ev
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
#define STATE_INPUT_RATES
Definition state.h:145
#define STATE_INPUT_ATTITUDE
Definition state.h:144
void float_eulers_of_quat(struct FloatEulers *e, const struct FloatQuat *q)
euler rotation 'ZYX'
euler angles
angular rates
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
#define EULERS_BFP_OF_REAL(_ei, _ef)
#define RATES_DIFF(_c, _a, _b)
#define RATES_FLOAT_OF_BFP(_rf, _ri)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
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(uint16_t id, struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition state.h:1272
void stateSetInputFilter(uint8_t type, uint16_t flag)
set the input filter for a specified type of data.
Definition state.c:91
static void stateSetBodyRates_f(uint16_t id, struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition state.h:1371
#define AHRS_PROPAGATE_FREQUENCY
Definition hf_float.c:55
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
static void send_att(struct transport_tx *trans, struct link_device *dev)
API to get/set the generic vehicle states.
Architecture independent timing functions.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_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.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.