Paparazzi UAS  v5.18.0_stable
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 "subsystems/ahrs.h"
30 #include "subsystems/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_INV_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 (IMU frame) */
54  struct FloatEulers ltp_to_imu_euler;
55  float_eulers_of_quat(&ltp_to_imu_euler, &ahrs_float_inv.state.quat);
56  struct Int32Eulers eulers_imu;
57  EULERS_BFP_OF_REAL(eulers_imu, ltp_to_imu_euler);
58 
59  /* compute Eulers in int (body frame) */
60  struct FloatQuat ltp_to_body_quat;
61  struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_float_inv.body_to_imu);
62  float_quat_comp_inv(&ltp_to_body_quat, &ahrs_float_inv.state.quat, body_to_imu_quat);
63  struct FloatEulers ltp_to_body_euler;
64  float_eulers_of_quat(&ltp_to_body_euler, &ltp_to_body_quat);
65  struct Int32Eulers eulers_body;
66  EULERS_BFP_OF_REAL(eulers_body, ltp_to_body_euler);
67 
68  pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
69  &eulers_imu.phi,
70  &eulers_imu.theta,
71  &eulers_imu.psi,
72  &eulers_body.phi,
73  &eulers_body.theta,
74  &eulers_body.psi,
75  &ahrs_finv_id);
76 }
77 
78 static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
79 {
80  pprz_msg_send_GEO_MAG(trans, dev, AC_ID,
84 }
85 
86 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
87 {
88  uint8_t mde = 3;
89  uint16_t val = 0;
90  if (!ahrs_float_inv.is_aligned) { mde = 2; }
92  /* set lost if no new gyro measurements for 50ms */
93  if (t_diff > 50000) { mde = 5; }
94  pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_finv_id, &mde, &val);
95 }
96 #endif
97 
98 
99 /*
100  * ABI bindings
101  */
103 #ifndef AHRS_FINV_IMU_ID
104 #define AHRS_FINV_IMU_ID ABI_BROADCAST
105 #endif
106 PRINT_CONFIG_VAR(AHRS_FINV_IMU_ID)
107 
108 
109 #ifndef AHRS_FINV_MAG_ID
110 #define AHRS_FINV_MAG_ID AHRS_FINV_IMU_ID
111 #endif
112 PRINT_CONFIG_VAR(AHRS_FINV_MAG_ID)
113 
120 
126 static void gyro_cb(uint8_t sender_id __attribute__((unused)),
127  uint32_t stamp, struct Int32Rates *gyro)
128 {
129  struct FloatRates gyro_f;
130  RATES_FLOAT_OF_BFP(gyro_f, *gyro);
131 
132 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
133  PRINT_CONFIG_MSG("Calculating dt for AHRS float_invariant propagation.")
134  /* timestamp in usec when last callback was received */
135  static uint32_t last_stamp = 0;
136 
137  if (last_stamp > 0 && ahrs_float_inv.is_aligned) {
138  float dt = (float)(stamp - last_stamp) * 1e-6;
139  ahrs_float_invariant_propagate(&gyro_f, dt);
141  }
142  last_stamp = stamp;
143 #else
144  PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS float_invariant propagation.")
145  PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
146  const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
148  ahrs_float_invariant_propagate(&gyro_f, dt);
150  }
151 #endif
152 
153  ahrs_finv_last_stamp = stamp;
154 }
155 
156 static void accel_cb(uint8_t sender_id __attribute__((unused)),
157  uint32_t stamp __attribute__((unused)),
158  struct Int32Vect3 *accel)
159 {
161  struct FloatVect3 accel_f;
162  ACCELS_FLOAT_OF_BFP(accel_f, *accel);
164  }
165 }
166 
167 static void mag_cb(uint8_t sender_id __attribute__((unused)),
168  uint32_t stamp __attribute__((unused)),
169  struct Int32Vect3 *mag)
170 {
172  struct FloatVect3 mag_f;
173  MAGS_FLOAT_OF_BFP(mag_f, *mag);
175  }
176 }
177 
178 static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
179  uint32_t stamp __attribute__((unused)),
180  struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
181  struct Int32Vect3 *lp_mag)
182 {
183  if (!ahrs_float_inv.is_aligned) {
184  /* convert to float */
185  struct FloatRates gyro_f;
186  RATES_FLOAT_OF_BFP(gyro_f, *lp_gyro);
187  struct FloatVect3 accel_f;
188  ACCELS_FLOAT_OF_BFP(accel_f, *lp_accel);
189  struct FloatVect3 mag_f;
190  MAGS_FLOAT_OF_BFP(mag_f, *lp_mag);
191  ahrs_float_invariant_align(&gyro_f, &accel_f, &mag_f);
193  }
194 }
195 
196 static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
197  struct FloatQuat *q_b2i_f)
198 {
200 }
201 
202 static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
203 {
205 }
206 
207 static bool ahrs_float_invariant_enable_output(bool enable)
208 {
209  ahrs_finv_output_enabled = enable;
211 }
212 
217 {
219  /* Compute LTP to BODY quaternion */
220  struct FloatQuat ltp_to_body_quat;
221  struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_float_inv.body_to_imu);
222  float_quat_comp_inv(&ltp_to_body_quat, &ahrs_float_inv.state.quat, body_to_imu_quat);
223  /* Set state */
224  stateSetNedToBodyQuat_f(&ltp_to_body_quat);
225 
226  /* compute body rates */
227  struct FloatRates body_rate;
229  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_float_inv.body_to_imu);
230  float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &body_rate);
231  stateSetBodyRates_f(&body_rate);
232 
233  }
234 }
235 
236 
238 {
242 
243  /*
244  * Subscribe to scaled IMU measurements and attach callbacks
245  */
246  AbiBindMsgIMU_MAG_INT32(AHRS_FINV_MAG_ID, &mag_ev, mag_cb);
247  AbiBindMsgIMU_GYRO_INT32(AHRS_FINV_IMU_ID, &gyro_ev, gyro_cb);
248  AbiBindMsgIMU_ACCEL_INT32(AHRS_FINV_IMU_ID, &accel_ev, accel_cb);
249  AbiBindMsgIMU_LOWPASSED(AHRS_FINV_IMU_ID, &aligner_ev, aligner_cb);
250  AbiBindMsgBODY_TO_IMU_QUAT(AHRS_FINV_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
251  AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
252 
253 #if PERIODIC_TELEMETRY
254  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_EULER_INT, send_att);
256  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
257 #endif
258 }
Int32Eulers::theta
int32_t theta
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:148
MAGS_FLOAT_OF_BFP
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:807
uint16_t
unsigned short uint16_t
Definition: types.h:16
val
uint16_t val[TCOUPLE_NB]
Definition: temp_tcouple_adc.c:49
ahrs_float_invariant_register
void ahrs_float_invariant_register(void)
Definition: ahrs_float_invariant_wrapper.c:237
ahrs_float_invariant_wrapper.h
ahrs_float_invariant_update_mag
void ahrs_float_invariant_update_mag(struct FloatVect3 *mag)
Definition: ahrs_float_invariant.c:239
geo_mag_cb
static void geo_mag_cb(uint8_t sender_id, struct FloatVect3 *h)
Definition: ahrs_float_invariant_wrapper.c:202
AHRS_FINV_OUTPUT_ENABLED
#define AHRS_FINV_OUTPUT_ENABLED
Definition: ahrs_float_invariant_wrapper.c:36
ahrs_float_inv_set_body_to_imu_quat
void ahrs_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i)
Definition: ahrs_float_invariant.c:384
Int32Rates
angular rates
Definition: pprz_algebra_int.h:179
ahrs_float_invariant_propagate
void ahrs_float_invariant_propagate(struct FloatRates *gyro, float dt)
Definition: ahrs_float_invariant.c:170
h
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
Definition: UKF_Wind_Estimator.c:821
abi.h
body_to_imu_ev
static abi_event body_to_imu_ev
Definition: ahrs_float_invariant_wrapper.c:118
inv_command::rates
struct FloatRates rates
Input gyro rates.
Definition: ahrs_float_invariant.h:65
AHRS_FINV_MAG_ID
#define AHRS_FINV_MAG_ID
magnetometer
Definition: ahrs_float_invariant_wrapper.c:110
RATES_FLOAT_OF_BFP
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:759
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
uint32_t
unsigned long uint32_t
Definition: types.h:18
inv_state::bias
struct FloatRates bias
Estimated gyro biases.
Definition: ahrs_float_invariant.h:42
compute_body_orientation_and_rates
static void compute_body_orientation_and_rates(void)
Compute body orientation and rates from imu orientation and rates.
Definition: ahrs_float_invariant_wrapper.c:216
orientationGetRMat_f
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
Definition: pprz_orientation_conversion.h:234
mag_ev
static abi_event mag_ev
Definition: ahrs_float_invariant_wrapper.c:114
ahrs_float_invariant_update_accel
void ahrs_float_invariant_update_accel(struct FloatVect3 *accel)
Definition: ahrs_float_invariant.c:231
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
FloatVect3
Definition: pprz_algebra_float.h:54
telemetry.h
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
AHRS_FINV_IMU_ID
#define AHRS_FINV_IMU_ID
IMU (gyro, accel)
Definition: ahrs_float_invariant_wrapper.c:104
Int32Eulers::psi
int32_t psi
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:149
Int32Eulers::phi
int32_t phi
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:147
AhrsFloatInv::state
struct inv_state state
state vector
Definition: ahrs_float_invariant.h:93
AHRS_COMP_ID_FINV
#define AHRS_COMP_ID_FINV
Definition: ahrs.h:39
accel_ev
static abi_event accel_ev
Definition: ahrs_float_invariant_wrapper.c:116
RATES_DIFF
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:372
orientationGetQuat_f
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
Definition: pprz_orientation_conversion.h:225
aligner_ev
static abi_event aligner_ev
Definition: ahrs_float_invariant_wrapper.c:117
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
stateSetBodyRates_f
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
Int32Vect3
Definition: pprz_algebra_int.h:88
sys_time.h
Architecture independent timing functions.
uint8_t
unsigned char uint8_t
Definition: types.h:14
ahrs_finv_last_stamp
static uint32_t ahrs_finv_last_stamp
last gyro msg timestamp
Definition: ahrs_float_invariant_wrapper.c:43
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
Int32Eulers
euler angles
Definition: pprz_algebra_int.h:146
gyro_cb
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
Call ahrs_float_invariant_propagate on new gyro measurements.
Definition: ahrs_float_invariant_wrapper.c:126
AhrsFloatInv::is_aligned
bool is_aligned
Definition: ahrs_float_invariant.h:105
AhrsFloatInv::mag_h
struct FloatVect3 mag_h
Definition: ahrs_float_invariant.h:104
ahrs.h
AhrsFloatInv::body_to_imu
struct OrientationReps body_to_imu
body_to_imu rotation
Definition: ahrs_float_invariant.h:102
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
PRINT_CONFIG_MSG
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
body_to_imu_cb
static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f)
Definition: ahrs_float_invariant_wrapper.c:196
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
geo_mag_ev
static abi_event geo_mag_ev
Definition: ahrs_float_invariant_wrapper.c:119
ahrs_float_inv
struct AhrsFloatInv ahrs_float_inv
Definition: ahrs_float_invariant.c:95
ahrs_finv_output_enabled
static bool ahrs_finv_output_enabled
if TRUE with push the estimation results to the state interface
Definition: ahrs_float_invariant_wrapper.c:41
float_rmat_transp_ratemult
void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
rotate anglular rates by transposed rotation matrix.
Definition: pprz_algebra_float.c:160
send_att
static void send_att(struct transport_tx *trans, struct link_device *dev)
Definition: ahrs_float_invariant_wrapper.c:51
mag_cb
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
Definition: ahrs_float_invariant_wrapper.c:167
gyro_ev
static abi_event gyro_ev
Definition: ahrs_float_invariant_wrapper.c:115
AHRS_PROPAGATE_FREQUENCY
#define AHRS_PROPAGATE_FREQUENCY
Definition: hf_float.c:55
FloatRMat
rotation matrix
Definition: pprz_algebra_float.h:77
ACCELS_FLOAT_OF_BFP
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
accel_cb
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
Definition: ahrs_float_invariant_wrapper.c:156
AhrsFloatInv::cmd
struct inv_command cmd
command vector
Definition: ahrs_float_invariant.h:95
FloatEulers
euler angles
Definition: pprz_algebra_float.h:84
ahrs_float_invariant_enable_output
static bool ahrs_float_invariant_enable_output(bool enable)
Definition: ahrs_float_invariant_wrapper.c:207
ahrs_register_impl
void ahrs_register_impl(AhrsEnableOutput enable)
Register an AHRS implementation.
Definition: ahrs.c:62
state.h
send_filter_status
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
Definition: ahrs_float_invariant_wrapper.c:86
float_eulers_of_quat
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
Definition: pprz_algebra_float.c:650
ahrs_finv_id
static uint8_t ahrs_finv_id
Definition: ahrs_float_invariant_wrapper.c:44
EULERS_BFP_OF_REAL
#define EULERS_BFP_OF_REAL(_ei, _ef)
Definition: pprz_algebra.h:715
ABI_BROADCAST
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:56
send_geo_mag
static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
Definition: ahrs_float_invariant_wrapper.c:78
ahrs_float_invariant_align
void ahrs_float_invariant_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
Definition: ahrs_float_invariant.c:156
ahrs_float_invariant_init
void ahrs_float_invariant_init(void)
Definition: ahrs_float_invariant.c:131
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
stateSetNedToBodyQuat_f
static void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition: state.h:1093
inv_state::quat
struct FloatQuat quat
Estimated attitude (quaternion)
Definition: ahrs_float_invariant.h:41
aligner_cb
static void aligner_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
Definition: ahrs_float_invariant_wrapper.c:178
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
float_quat_comp_inv
void float_quat_comp_inv(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
Definition: pprz_algebra_float.c:328