Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
ahrs_int_cmpl_quat_wrapper.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2015 Felix Ruess <felix.ruess@gmail.com>
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
26
28#include "modules/ahrs/ahrs.h"
29#include "modules/core/abi.h"
30#include "state.h"
31
32PRINT_CONFIG_VAR(AHRS_ICQ_TYPE)
33
37
38static void set_body_state_from_quat(void);
39
40#if PERIODIC_TELEMETRY
42#include "mcu_periph/sys_time.h"
43#include "state.h"
44
45static void send_quat(struct transport_tx *trans, struct link_device *dev)
46{
47 struct Int32Quat *quat = stateGetNedToBodyQuat_i();
48 pprz_msg_send_AHRS_QUAT_INT(trans, dev, AC_ID,
49 &ahrs_icq.weight,
50 &ahrs_icq.ltp_to_body_quat.qi,
51 &ahrs_icq.ltp_to_body_quat.qx,
52 &ahrs_icq.ltp_to_body_quat.qy,
53 &ahrs_icq.ltp_to_body_quat.qz,
54 &(quat->qi),
55 &(quat->qx),
56 &(quat->qy),
57 &(quat->qz),
59}
60
61static void send_euler(struct transport_tx *trans, struct link_device *dev)
62{
63 struct Int32Eulers ltp_to_body_euler;
64 int32_eulers_of_quat(&ltp_to_body_euler, &ahrs_icq.ltp_to_body_quat);
65 struct Int32Eulers *eulers = stateGetNedToBodyEulers_i();
66 pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
67 &ltp_to_body_euler.phi,
68 &ltp_to_body_euler.theta,
69 &ltp_to_body_euler.psi,
70 &(eulers->phi),
71 &(eulers->theta),
72 &(eulers->psi),
74}
75
76static void send_bias(struct transport_tx *trans, struct link_device *dev)
77{
78 pprz_msg_send_AHRS_GYRO_BIAS_INT(trans, dev, AC_ID,
79 &ahrs_icq.gyro_bias.p, &ahrs_icq.gyro_bias.q,
80 &ahrs_icq.gyro_bias.r, &ahrs_icq_id);
81}
82
83static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
84{
85 struct FloatVect3 h_float;
86 h_float.x = MAG_FLOAT_OF_BFP(ahrs_icq.mag_h.x);
87 h_float.y = MAG_FLOAT_OF_BFP(ahrs_icq.mag_h.y);
88 h_float.z = MAG_FLOAT_OF_BFP(ahrs_icq.mag_h.z);
89 pprz_msg_send_GEO_MAG(trans, dev, AC_ID,
90 &h_float.x, &h_float.y, &h_float.z, &ahrs_icq_id);
91}
92
93static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
94{
95 uint8_t mde = 3;
96 uint16_t val = 0;
97 if (!ahrs_icq.is_aligned) { mde = 2; }
99 /* set lost if no new gyro measurements for 50ms */
100 if (t_diff > 50000) { mde = 5; }
101 pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_icq_id, &mde, &val);
102}
103#endif
104
105
109#ifndef AHRS_ICQ_IMU_ID
110#define AHRS_ICQ_IMU_ID ABI_BROADCAST
111#endif
116#ifndef AHRS_ICQ_MAG_ID
117#define AHRS_ICQ_MAG_ID AHRS_ICQ_IMU_ID
118#endif
123#ifndef AHRS_ICQ_GPS_ID
124#define AHRS_ICQ_GPS_ID GPS_MULTI_ID
125#endif
133
134
135static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
136 uint32_t stamp, struct Int32Rates *gyro)
137{
138 ahrs_icq_last_stamp = stamp;
139#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
140 PRINT_CONFIG_MSG("Calculating dt for AHRS_ICQ propagation.")
141 /* timestamp in usec when last callback was received */
142 static uint32_t last_stamp = 0;
143
144 if (last_stamp > 0 && ahrs_icq.is_aligned) {
145 float dt = (float)(stamp - last_stamp) * 1e-6;
146 ahrs_icq_propagate(gyro, dt);
148 }
149 last_stamp = stamp;
150#else
151 PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_ICQ propagation.")
153 if (ahrs_icq.status == AHRS_ICQ_RUNNING) {
154 const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
155 ahrs_icq_propagate(gyro, dt);
157 }
158#endif
159}
160
161static void accel_cb(uint8_t __attribute__((unused)) sender_id,
162 uint32_t __attribute__((unused)) stamp,
163 struct Int32Vect3 *accel)
164{
165#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
166 PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_quat accel update.")
167 static uint32_t last_stamp = 0;
168 if (last_stamp > 0 && ahrs_icq.is_aligned) {
169 float dt = (float)(stamp - last_stamp) * 1e-6;
170 ahrs_icq_update_accel(accel, dt);
172 }
173 last_stamp = stamp;
174#else
175 PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS int_cmpl_quat accel update.")
176 PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
177 if (ahrs_icq.is_aligned) {
178 const float dt = 1. / (AHRS_CORRECT_FREQUENCY);
179 ahrs_icq_update_accel(accel, dt);
181 }
182#endif
183}
184
185static void mag_cb(uint8_t __attribute__((unused)) sender_id,
186 uint32_t __attribute__((unused)) stamp,
187 struct Int32Vect3 *mag)
188{
189#if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
190 PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_quat mag update.")
191 static uint32_t last_stamp = 0;
192 if (last_stamp > 0 && ahrs_icq.is_aligned) {
193 float dt = (float)(stamp - last_stamp) * 1e-6;
194 ahrs_icq_update_mag(mag, dt);
196 }
197 last_stamp = stamp;
198#else
199 PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS int_cmpl_quat mag update.")
200 PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
201 if (ahrs_icq.is_aligned) {
202 const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY);
203 ahrs_icq_update_mag(mag, dt);
205 }
206#endif
207}
208
209static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
210 uint32_t stamp __attribute__((unused)),
211 struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
212 struct Int32Vect3 *lp_mag)
213{
214 if (!ahrs_icq.is_aligned) {
215 if (ahrs_icq_align(lp_gyro, lp_accel, lp_mag)) {
217 }
218 }
219}
220
221static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
222{
224 MAG_BFP_OF_REAL(h->z));
225}
226
227static void gps_cb(uint8_t sender_id __attribute__((unused)),
228 uint32_t stamp __attribute__((unused)),
229 struct GpsState *gps_s)
230{
231 ahrs_icq_update_gps(gps_s);
232}
233
236{
237 /* Set state */
238 stateSetNedToBodyQuat_i(MODULE_AHRS_INT_CMPL_QUAT_ID, &ahrs_icq.ltp_to_body_quat);
239 stateSetBodyRates_i(MODULE_AHRS_INT_CMPL_QUAT_ID, &ahrs_icq.body_rate);
240}
241
243{
245 if (AHRS_ICQ_TYPE == AHRS_PRIMARY) {
247 } else {
249 }
250
251 /*
252 * Subscribe to scaled IMU measurements and attach callbacks
253 */
254 AbiBindMsgIMU_GYRO(AHRS_ICQ_IMU_ID, &gyro_ev, gyro_cb);
255 AbiBindMsgIMU_ACCEL(AHRS_ICQ_IMU_ID, &accel_ev, accel_cb);
256 AbiBindMsgIMU_MAG(AHRS_ICQ_MAG_ID, &mag_ev, mag_cb);
257 AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
258 AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
259 AbiBindMsgGPS(AHRS_ICQ_GPS_ID, &gps_ev, gps_cb);
260
261#if PERIODIC_TELEMETRY
262 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_QUAT_INT, send_quat);
263 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_EULER_INT, send_euler);
264 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_GYRO_BIAS_INT, send_bias);
266 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
267#endif
268}
269
271{
272 if (enable) {
273 stateSetInputFilter(STATE_INPUT_ATTITUDE, MODULE_AHRS_INT_CMPL_QUAT_ID);
274 stateSetInputFilter(STATE_INPUT_RATES, MODULE_AHRS_INT_CMPL_QUAT_ID);
275 }
276 ahrs_icq_enable = enable;
277}
278
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_PRIMARY
Definition ahrs.h:32
#define AHRS_COMP_ID_ICQ
Definition ahrs.h:38
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
static abi_event gyro_ev
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
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)
static void send_euler(struct transport_tx *trans, struct link_device *dev)
static void send_bias(struct transport_tx *trans, struct link_device *dev)
static abi_event gps_ev
static void set_body_state_from_quat(void)
Compute body orientation and rates from imu orientation and rates.
bool ahrs_icq_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
void ahrs_icq_update_accel(struct Int32Vect3 *accel, float dt)
struct AhrsIntCmplQuat ahrs_icq
Default Rate filter Low pass.
void ahrs_icq_init(void)
void ahrs_icq_update_gps(struct GpsState *gps_s)
void ahrs_icq_update_mag(struct Int32Vect3 *mag, float dt)
void ahrs_icq_propagate(struct Int32Rates *gyro, float dt)
@ AHRS_ICQ_RUNNING
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
void ahrs_icq_wrapper_init(void)
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
static uint8_t ahrs_icq_id
static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
#define AHRS_ICQ_IMU_ID
ABI binding for IMU data.
void ahrs_int_cmpl_quat_wrapper_enable(uint8_t enable)
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
static void send_quat(struct transport_tx *trans, struct link_device *dev)
#define AHRS_ICQ_GPS_ID
ABI binding for gps data.
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 uint32_t ahrs_icq_last_stamp
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_euler(struct transport_tx *trans, struct link_device *dev)
static void set_body_state_from_quat(void)
Rotate angles and rates from imu to body frame and set state.
static void send_bias(struct transport_tx *trans, struct link_device *dev)
#define AHRS_ICQ_MAG_ID
ABI binding for magnetometer data.
uint8_t ahrs_icq_enable
Paparazzi specific wrapper to run floating point complementary 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)
data structure for GPS information
Definition gps.h:90
#define STATE_INPUT_RATES
Definition state.h:145
#define STATE_INPUT_ATTITUDE
Definition state.h:144
#define VECT3_ASSIGN(_a, _x, _y, _z)
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
void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q)
#define MAG_BFP_OF_REAL(_af)
#define MAG_FLOAT_OF_BFP(_ai)
euler angles
Rotation quaternion.
angular rates
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
Definition state.h:1295
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition state.h:1307
static void stateSetNedToBodyQuat_i(uint16_t id, struct Int32Quat *ned_to_body_quat)
Set vehicle body attitude from quaternion (int).
Definition state.h:1251
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_i(uint16_t id, struct Int32Rates *body_rate)
Set vehicle body angular rate (int).
Definition state.h:1361
#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.
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.