Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
ahrs_madgwick_wrapper.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2020 Gautier Hattenberger <gautier.hattenberger@enac.fr>
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, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
28 #include "subsystems/ahrs.h"
29 #include "subsystems/abi.h"
30 #include "mcu_periph/sys_time.h"
31 #include "message_pragmas.h"
32 #include "state.h"
33 
34 #ifndef AHRS_MADGWICK_OUTPUT_ENABLED
35 #define AHRS_MADGWICK_OUTPUT_ENABLED TRUE
36 #endif
37 PRINT_CONFIG_VAR(AHRS_MADGWICK_OUTPUT_ENABLED)
38 
39 
44 
45 static void compute_body_orientation_and_rates(void);
46 
47 #if PERIODIC_TELEMETRY
49 
50 static void send_att(struct transport_tx *trans, struct link_device *dev)
51 {
52  /* compute eulers in int (IMU frame) */
53  struct FloatEulers ltp_to_imu_euler;
54  float_eulers_of_quat(&ltp_to_imu_euler, &ahrs_madgwick.quat);
55  struct Int32Eulers eulers_imu;
56  EULERS_BFP_OF_REAL(eulers_imu, ltp_to_imu_euler);
57 
58  /* compute Eulers in int (body frame) */
59  struct FloatQuat ltp_to_body_quat;
60  struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_madgwick.body_to_imu);
61  float_quat_comp_inv(&ltp_to_body_quat, &ahrs_madgwick.quat, body_to_imu_quat);
62  struct FloatEulers ltp_to_body_euler;
63  float_eulers_of_quat(&ltp_to_body_euler, &ltp_to_body_quat);
64  struct Int32Eulers eulers_body;
65  EULERS_BFP_OF_REAL(eulers_body, ltp_to_body_euler);
66 
67  pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
68  &eulers_imu.phi,
69  &eulers_imu.theta,
70  &eulers_imu.psi,
71  &eulers_body.phi,
72  &eulers_body.theta,
73  &eulers_body.psi,
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_madgwick.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_madgwick_id, &mde, &val);
86 }
87 #endif
88 
89 
90 /*
91  * ABI bindings
92  */
94 #ifndef AHRS_MADGWICK_IMU_ID
95 #define AHRS_MADGWICK_IMU_ID ABI_BROADCAST
96 #endif
97 PRINT_CONFIG_VAR(AHRS_MADGWICK_IMU_ID)
98 
99 
100 #ifndef AHRS_MADGWICK_MAG_ID
101 #define AHRS_MADGWICK_MAG_ID AHRS_MADGWICK_IMU_ID
102 #endif
103 PRINT_CONFIG_VAR(AHRS_MADGWICK_MAG_ID)
104 
109 
115 static void gyro_cb(uint8_t sender_id __attribute__((unused)),
116  uint32_t stamp, struct Int32Rates *gyro)
117 {
118  struct FloatRates gyro_f;
119  RATES_FLOAT_OF_BFP(gyro_f, *gyro);
120 
121 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
122  PRINT_CONFIG_MSG("Calculating dt for AHRS Madgwick propagation.")
123  /* timestamp in usec when last callback was received */
124  static uint32_t last_stamp = 0;
125 
126  if (last_stamp > 0 && ahrs_madgwick.is_aligned) {
127  float dt = (float)(stamp - last_stamp) * 1e-6;
128  ahrs_madgwick_propagate(&gyro_f, dt);
130  }
131  last_stamp = stamp;
132 #else
133  PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS Madgwick propagation.")
134  PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
135  const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
137  ahrs_madgwick_propagate(&gyro_f, dt);
139  }
140 #endif
141 
142  ahrs_madgwick_last_stamp = stamp;
143 }
144 
145 static void accel_cb(uint8_t sender_id __attribute__((unused)),
146  uint32_t stamp __attribute__((unused)),
147  struct Int32Vect3 *accel)
148 {
150  struct FloatVect3 accel_f;
151  ACCELS_FLOAT_OF_BFP(accel_f, *accel);
152  ahrs_madgwick_update_accel(&accel_f);
153  }
154 }
155 
156 static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
157  uint32_t stamp __attribute__((unused)),
158  struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
159  struct Int32Vect3 *lp_mag __attribute__((unused)))
160 {
161  if (!ahrs_madgwick.is_aligned) {
162  /* convert to float */
163  struct FloatRates gyro_f;
164  RATES_FLOAT_OF_BFP(gyro_f, *lp_gyro);
165  struct FloatVect3 accel_f;
166  ACCELS_FLOAT_OF_BFP(accel_f, *lp_accel);
167  ahrs_madgwick_align(&gyro_f, &accel_f);
169  }
170 }
171 
172 static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
173  struct FloatQuat *q_b2i_f)
174 {
176 }
177 
178 static bool ahrs_madgwick_enable_output(bool enable)
179 {
182 }
183 
188 {
190  /* Compute LTP to BODY quaternion */
191  struct FloatQuat ltp_to_body_quat;
192  struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_madgwick.body_to_imu);
193  float_quat_comp_inv(&ltp_to_body_quat, &ahrs_madgwick.quat, body_to_imu_quat);
194  /* Set state */
195  stateSetNedToBodyQuat_f(&ltp_to_body_quat);
196 
197  /* compute body rates */
199  }
200 }
201 
202 
204 {
208 
209  /*
210  * Subscribe to scaled IMU measurements and attach callbacks
211  */
212  AbiBindMsgIMU_GYRO_INT32(AHRS_MADGWICK_IMU_ID, &gyro_ev, gyro_cb);
213  AbiBindMsgIMU_ACCEL_INT32(AHRS_MADGWICK_IMU_ID, &accel_ev, accel_cb);
214  AbiBindMsgIMU_LOWPASSED(AHRS_MADGWICK_IMU_ID, &aligner_ev, aligner_cb);
215  AbiBindMsgBODY_TO_IMU_QUAT(AHRS_MADGWICK_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
216 
217 #if PERIODIC_TELEMETRY
218  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_EULER_INT, send_att);
219  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
220 #endif
221 }
222 
ahrs_madgwick_enable_output
static bool ahrs_madgwick_enable_output(bool enable)
Definition: ahrs_madgwick_wrapper.c:178
Int32Eulers::theta
int32_t theta
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:148
ahrs_madgwick_init
void ahrs_madgwick_init(void)
Definition: ahrs_madgwick.c:50
uint16_t
unsigned short uint16_t
Definition: types.h:16
ahrs_madgwick_set_body_to_imu_quat
void ahrs_madgwick_set_body_to_imu_quat(struct FloatQuat *q_b2i)
Definition: ahrs_madgwick.c:147
val
uint16_t val[TCOUPLE_NB]
Definition: temp_tcouple_adc.c:49
gyro_ev
static abi_event gyro_ev
Definition: ahrs_madgwick_wrapper.c:105
Int32Rates
angular rates
Definition: pprz_algebra_int.h:179
ahrs_madgwick_last_stamp
static uint32_t ahrs_madgwick_last_stamp
last gyro msg timestamp
Definition: ahrs_madgwick_wrapper.c:42
abi.h
gyro_cb
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
Call ahrs_madgwick_propagate on new gyro measurements.
Definition: ahrs_madgwick_wrapper.c:115
ahrs_madgwick_update_accel
void ahrs_madgwick_update_accel(struct FloatVect3 *accel)
Definition: ahrs_madgwick.c:142
ahrs_madgwick_register
void ahrs_madgwick_register(void)
Definition: ahrs_madgwick_wrapper.c:203
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
ahrs_madgwick
struct AhrsMadgwick ahrs_madgwick
Definition: ahrs_madgwick.c:39
accel_cb
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
Definition: ahrs_madgwick_wrapper.c:145
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_madgwick_wrapper.c:156
AHRS_MADGWICK_IMU_ID
#define AHRS_MADGWICK_IMU_ID
IMU (gyro, accel)
Definition: ahrs_madgwick_wrapper.c:95
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
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
body_to_imu_ev
static abi_event body_to_imu_ev
Definition: ahrs_madgwick_wrapper.c:108
AhrsMadgwick::quat
struct FloatQuat quat
Estimated attitude (quaternion)
Definition: ahrs_madgwick.h:39
ahrs_madgwick_output_enabled
static bool ahrs_madgwick_output_enabled
if TRUE with push the estimation results to the state interface
Definition: ahrs_madgwick_wrapper.c:40
orientationGetQuat_f
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
Definition: pprz_orientation_conversion.h:225
ahrs_madgwick_wrapper.h
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
AhrsMadgwick::is_aligned
bool is_aligned
aligned flag
Definition: ahrs_madgwick.h:45
Int32Vect3
Definition: pprz_algebra_int.h:88
sys_time.h
Architecture independent timing functions.
uint8_t
unsigned char uint8_t
Definition: types.h:14
accel_ev
static abi_event accel_ev
Definition: ahrs_madgwick_wrapper.c:106
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
ahrs_madgwick_id
static uint8_t ahrs_madgwick_id
Definition: ahrs_madgwick_wrapper.c:43
Int32Eulers
euler angles
Definition: pprz_algebra_int.h:146
AHRS_MADGWICK_MAG_ID
#define AHRS_MADGWICK_MAG_ID
magnetometer
Definition: ahrs_madgwick_wrapper.c:101
aligner_ev
static abi_event aligner_ev
Definition: ahrs_madgwick_wrapper.c:107
ahrs.h
AHRS_MADGWICK_OUTPUT_ENABLED
#define AHRS_MADGWICK_OUTPUT_ENABLED
Definition: ahrs_madgwick_wrapper.c:35
ahrs_madgwick_align
void ahrs_madgwick_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel)
Definition: ahrs_madgwick.c:59
PRINT_CONFIG_MSG
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
send_att
static void send_att(struct transport_tx *trans, struct link_device *dev)
Definition: ahrs_madgwick_wrapper.c:50
AhrsMadgwick::body_to_imu
struct OrientationReps body_to_imu
body_to_imu rotation
Definition: ahrs_madgwick.h:43
AhrsMadgwick::rates
struct FloatRates rates
Measured gyro rates.
Definition: ahrs_madgwick.h:40
ahrs_madgwick_propagate
void ahrs_madgwick_propagate(struct FloatRates *gyro, float dt)
Definition: ahrs_madgwick.c:69
AHRS_PROPAGATE_FREQUENCY
#define AHRS_PROPAGATE_FREQUENCY
Definition: hf_float.c:55
ACCELS_FLOAT_OF_BFP
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
FloatEulers
euler angles
Definition: pprz_algebra_float.h:84
ahrs_register_impl
void ahrs_register_impl(AhrsEnableOutput enable)
Register an AHRS implementation.
Definition: ahrs.c:62
state.h
float_eulers_of_quat
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
Definition: pprz_algebra_float.c:650
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_madgwick_wrapper.c:187
body_to_imu_cb
static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f)
Definition: ahrs_madgwick_wrapper.c:172
EULERS_BFP_OF_REAL
#define EULERS_BFP_OF_REAL(_ei, _ef)
Definition: pprz_algebra.h:715
send_filter_status
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
Definition: ahrs_madgwick_wrapper.c:77
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
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
AHRS_COMP_ID_MADGWICK
#define AHRS_COMP_ID_MADGWICK
Definition: ahrs.h:45
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