Paparazzi UAS  v7.0_unstable
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 "modules/ahrs/ahrs.h"
29 #include "modules/core/abi.h"
30 #include "mcu_periph/sys_time.h"
31 #include "message_pragmas.h"
32 #include "state.h"
33 
34 PRINT_CONFIG_VAR(AHRS_MADGWICK_TYPE)
35 
40 
41 static void compute_body_orientation_and_rates(void);
42 
43 #if PERIODIC_TELEMETRY
45 
46 static void send_att(struct transport_tx *trans, struct link_device *dev)
47 {
48  /* compute eulers in int (IMU frame) */
49  struct FloatEulers ltp_to_imu_euler;
50  float_eulers_of_quat(&ltp_to_imu_euler, &ahrs_madgwick.quat);
51  struct Int32Eulers eulers_imu;
52  EULERS_BFP_OF_REAL(eulers_imu, ltp_to_imu_euler);
53 
54  /* compute Eulers in int (body frame) */
55  struct FloatEulers ltp_to_body_euler;
56  float_eulers_of_quat(&ltp_to_body_euler, &ahrs_madgwick.quat);
57  struct Int32Eulers eulers_body;
58  EULERS_BFP_OF_REAL(eulers_body, ltp_to_body_euler);
59 
60  pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
61  &eulers_imu.phi,
62  &eulers_imu.theta,
63  &eulers_imu.psi,
64  &eulers_body.phi,
65  &eulers_body.theta,
66  &eulers_body.psi,
68 }
69 
70 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
71 {
72  uint8_t mde = 3;
73  uint16_t val = 0;
74  if (!ahrs_madgwick.is_aligned) { mde = 2; }
76  /* set lost if no new gyro measurements for 50ms */
77  if (t_diff > 50000) { mde = 5; }
78  pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_madgwick_id, &mde, &val);
79 }
80 #endif
81 
82 
83 /*
84  * ABI bindings
85  */
87 #ifndef AHRS_MADGWICK_IMU_ID
88 #define AHRS_MADGWICK_IMU_ID ABI_BROADCAST
89 #endif
91 
92 
93 #ifndef AHRS_MADGWICK_MAG_ID
94 #define AHRS_MADGWICK_MAG_ID AHRS_MADGWICK_IMU_ID
95 #endif
97 
101 
107 static void gyro_cb(uint8_t sender_id __attribute__((unused)),
108  uint32_t stamp, struct Int32Rates *gyro)
109 {
110  struct FloatRates gyro_f;
111  RATES_FLOAT_OF_BFP(gyro_f, *gyro);
112 
113 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
114  PRINT_CONFIG_MSG("Calculating dt for AHRS Madgwick propagation.")
115  /* timestamp in usec when last callback was received */
116  static uint32_t last_stamp = 0;
117 
118  if (last_stamp > 0 && ahrs_madgwick.is_aligned) {
119  float dt = (float)(stamp - last_stamp) * 1e-6;
120  ahrs_madgwick_propagate(&gyro_f, dt);
122  }
123  last_stamp = stamp;
124 #else
125  PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS Madgwick propagation.")
127  const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
129  ahrs_madgwick_propagate(&gyro_f, dt);
131  }
132 #endif
133 
134  ahrs_madgwick_last_stamp = stamp;
135 }
136 
137 static void accel_cb(uint8_t sender_id __attribute__((unused)),
138  uint32_t stamp __attribute__((unused)),
139  struct Int32Vect3 *accel)
140 {
142  struct FloatVect3 accel_f;
143  ACCELS_FLOAT_OF_BFP(accel_f, *accel);
144  ahrs_madgwick_update_accel(&accel_f);
145  }
146 }
147 
148 static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
149  uint32_t stamp __attribute__((unused)),
150  struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
151  struct Int32Vect3 *lp_mag __attribute__((unused)))
152 {
153  if (!ahrs_madgwick.is_aligned) {
154  /* convert to float */
155  struct FloatRates gyro_f;
156  RATES_FLOAT_OF_BFP(gyro_f, *lp_gyro);
157  struct FloatVect3 accel_f;
158  ACCELS_FLOAT_OF_BFP(accel_f, *lp_accel);
159  ahrs_madgwick_align(&gyro_f, &accel_f);
161  }
162 }
163 
168 {
169  /* Set state */
170  stateSetNedToBodyQuat_f(MODULE_AHRS_MADGWICK_ID, &ahrs_madgwick.quat);
171 
172  /* compute body rates */
173  stateSetBodyRates_f(MODULE_AHRS_MADGWICK_ID, &ahrs_madgwick.rates);
174 }
175 
176 
178 {
180  if (AHRS_MADGWICK_TYPE == AHRS_PRIMARY) {
182  } else {
184  }
185 
186  /*
187  * Subscribe to scaled IMU measurements and attach callbacks
188  */
189  AbiBindMsgIMU_GYRO(AHRS_MADGWICK_IMU_ID, &gyro_ev, gyro_cb);
190  AbiBindMsgIMU_ACCEL(AHRS_MADGWICK_IMU_ID, &accel_ev, accel_cb);
191  AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
192 
193 #if PERIODIC_TELEMETRY
194  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_EULER_INT, send_att);
195  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
196 #endif
197 }
198 
200 {
201  if (enable) {
202  stateSetInputFilter(STATE_INPUT_ATTITUDE, MODULE_AHRS_MADGWICK_ID);
203  stateSetInputFilter(STATE_INPUT_RATES, MODULE_AHRS_MADGWICK_ID);
204  }
205  ahrs_madgwick_enable = enable;
206 }
207 
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
#define AHRS_COMP_ID_MADGWICK
Definition: ahrs.h:48
#define AHRS_PRIMARY
Definition: ahrs.h:32
struct AhrsMadgwick ahrs_madgwick
Definition: ahrs_madgwick.c:39
void ahrs_madgwick_propagate(struct FloatRates *gyro, float dt)
Definition: ahrs_madgwick.c:69
void ahrs_madgwick_update_accel(struct FloatVect3 *accel)
void ahrs_madgwick_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel)
Definition: ahrs_madgwick.c:59
void ahrs_madgwick_init(void)
Definition: ahrs_madgwick.c:50
bool is_aligned
aligned flag
Definition: ahrs_madgwick.h:44
struct FloatRates rates
Measured gyro rates.
Definition: ahrs_madgwick.h:40
struct FloatQuat quat
Estimated attitude (quaternion)
Definition: ahrs_madgwick.h:39
void ahrs_madgwick_wrapper_init(void)
static void compute_body_orientation_and_rates(void)
Compute body orientation and rates from imu orientation and rates.
#define AHRS_MADGWICK_IMU_ID
IMU (gyro, accel)
static uint8_t ahrs_madgwick_id
static abi_event accel_ev
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
Call ahrs_madgwick_propagate on new gyro measurements.
static uint32_t ahrs_madgwick_last_stamp
last gyro msg timestamp
static abi_event gyro_ev
static abi_event aligner_ev
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
#define AHRS_MADGWICK_MAG_ID
magnetometer
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
void ahrs_madgwick_wrapper_enable(uint8_t enable)
static void aligner_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
static void send_att(struct transport_tx *trans, struct link_device *dev)
uint8_t ahrs_madgwick_enable
Paparazzi specific wrapper to run Madgwick ahrs filter.
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:71
#define STATE_INPUT_RATES
Definition: state.h:145
#define STATE_INPUT_ATTITUDE
Definition: state.h:144
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
euler angles
angular rates
#define EULERS_BFP_OF_REAL(_ei, _ef)
Definition: pprz_algebra.h:715
#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(uint16_t id, struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition: state.h:1253
void stateSetInputFilter(uint8_t type, uint16_t flag)
set the input filter for a specified type of data.
Definition: state.c:85
static void stateSetBodyRates_f(uint16_t id, struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1346
#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)
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