Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
ins_float_invariant_wrapper.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2013 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 "modules/core/abi.h"
30 #include "mcu_periph/sys_time.h"
31 #include "message_pragmas.h"
32 #if USE_AHRS_ALIGNER
34 #endif
35 
36 #ifndef INS_FINV_FILTER_ID
37 #define INS_FINV_FILTER_ID 2
38 #endif
39 
41 static struct FloatVect3 ins_finv_accel;
42 
45 
46 #if PERIODIC_TELEMETRY && !INS_FINV_USE_UTM
48 #include "state.h"
49 static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
50 {
51  float foo = 0.;
53  pprz_msg_send_INS_REF(trans, dev, AC_ID,
58  }
59 }
60 
61 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
62 {
64  uint8_t mde = 3;
65  uint16_t val = 0;
66  if (!ins_float_inv.is_aligned) { mde = 2; }
68  /* set lost if no new gyro measurements for 50ms */
69  if (t_diff > 50000) { mde = 5; }
70  pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &id, &mde, &val);
71 }
72 #endif
73 
74 
75 /*
76  * ABI bindings
77  */
79 #ifndef INS_FINV_BARO_ID
80 #if USE_BARO_BOARD
81 #define INS_FINV_BARO_ID BARO_BOARD_SENDER_ID
82 #else
83 #define INS_FINV_BARO_ID ABI_BROADCAST
84 #endif
85 #endif
86 PRINT_CONFIG_VAR(INS_FINV_BARO_ID)
87 
88 
89 #ifndef INS_FINV_IMU_ID
90 #define INS_FINV_IMU_ID ABI_BROADCAST
91 #endif
92 PRINT_CONFIG_VAR(INS_FINV_IMU_ID)
93 
94 
95 #if USE_MAGNETOMETER
96 #ifndef INS_FINV_MAG_ID
97 #define INS_FINV_MAG_ID ABI_BROADCAST
98 #endif
99 PRINT_CONFIG_VAR(INS_FINV_MAG_ID)
100 #else
101 PRINT_CONFIG_MSG("INS invariant use GPS heading as magnetometer")
102 #endif
103 
107 #ifndef INS_FINV_GPS_ID
108 #define INS_FINV_GPS_ID GPS_MULTI_ID
109 #endif
110 PRINT_CONFIG_VAR(INS_FINV_GPS_ID)
111 
116 #if USE_MAGNETOMETER
119 #endif
121 
122 static void baro_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) uint32_t stamp, float pressure)
123 {
125 }
126 
132 static void gyro_cb(uint8_t sender_id __attribute__((unused)),
133  uint32_t stamp, struct Int32Rates *gyro)
134 {
135  struct FloatRates gyro_f;
136  RATES_FLOAT_OF_BFP(gyro_f, *gyro);
137 
138 #if USE_AHRS_ALIGNER
139  if (ins_float_inv.reset) {
141  }
142 #endif
143 
144 #if USE_AUTO_INS_FREQ || !defined(INS_PROPAGATE_FREQUENCY)
145  PRINT_CONFIG_MSG("Calculating dt for INS float_invariant propagation.")
146  /* timestamp in usec when last callback was received */
147  static uint32_t last_stamp = 0;
148 
149  if (last_stamp > 0) {
150  float dt = (float)(stamp - last_stamp) * 1e-6;
152  }
153  last_stamp = stamp;
154 #else
155  PRINT_CONFIG_MSG("Using fixed INS_PROPAGATE_FREQUENCY for INS float_invariant propagation.")
156  PRINT_CONFIG_VAR(INS_PROPAGATE_FREQUENCY)
157  const float dt = 1. / (INS_PROPAGATE_FREQUENCY);
159 #endif
160 
161  ins_finv_last_stamp = stamp;
162 }
163 
164 static void accel_cb(uint8_t sender_id __attribute__((unused)),
165  uint32_t stamp __attribute__((unused)),
166  struct Int32Vect3 *accel)
167 {
169 }
170 
171 static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
172  uint32_t stamp __attribute__((unused)),
173  struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
174  struct Int32Vect3 *lp_mag)
175 {
176  if (!ins_float_inv.is_aligned) {
177  /* convert to float */
178  struct FloatRates gyro_f;
179  RATES_FLOAT_OF_BFP(gyro_f, *lp_gyro);
180  struct FloatVect3 accel_f;
181  ACCELS_FLOAT_OF_BFP(accel_f, *lp_accel);
182  struct FloatVect3 mag_f;
183  MAGS_FLOAT_OF_BFP(mag_f, *lp_mag);
184  ins_float_invariant_align(&gyro_f, &accel_f, &mag_f);
185  }
186 }
187 
188 #if USE_MAGNETOMETER
189 static void mag_cb(uint8_t sender_id __attribute__((unused)),
190  uint32_t stamp __attribute__((unused)),
191  struct Int32Vect3 *mag)
192 {
194  struct FloatVect3 mag_f;
195  MAGS_FLOAT_OF_BFP(mag_f, *mag);
197  }
198 }
199 
200 static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
201 {
202  ins_float_inv.mag_h = *h;
203 }
204 #endif
205 
206 static void gps_cb(uint8_t sender_id __attribute__((unused)),
207  uint32_t stamp __attribute__((unused)),
208  struct GpsState *gps_s)
209 {
211 }
212 
213 
215 {
216  // aligner
217 #if USE_AHRS_ALIGNER
219 #endif
220 
222 
223  // Bind to ABI messages
224  AbiBindMsgBARO_ABS(INS_FINV_BARO_ID, &baro_ev, baro_cb);
225  AbiBindMsgIMU_GYRO(INS_FINV_IMU_ID, &gyro_ev, gyro_cb);
226  AbiBindMsgIMU_ACCEL(INS_FINV_IMU_ID, &accel_ev, accel_cb);
227  AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
228 #if USE_MAGNETOMETER
229  AbiBindMsgIMU_MAG(INS_FINV_MAG_ID, &mag_ev, mag_cb);
230  AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
231 #endif
232  AbiBindMsgGPS(INS_FINV_GPS_ID, &gps_ev, gps_cb);
233 
234 #if PERIODIC_TELEMETRY && !INS_FINV_USE_UTM
236  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
237 #endif
238 }
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
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
void ahrs_aligner_init(void)
Definition: ahrs_aligner.c:84
void ahrs_aligner_restart(void)
Definition: ahrs_aligner.c:96
Interface to align the AHRS via low-passed measurements at startup.
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:71
data structure for GPS information
Definition: gps.h:85
angular rates
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:807
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:759
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
angular rates
int32_t lat
in degrees*1e7
int32_t hmsl
Height above mean sea level in mm.
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t z
in centimeters
struct LlaCoor_i lla
Reference point in lla.
int32_t x
in centimeters
struct EcefCoor_i ecef
Reference point in ecef.
int32_t y
in centimeters
int32_t lon
in degrees*1e7
struct State state
Definition: state.c:36
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:171
void ins_float_invariant_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
void ins_float_invariant_propagate(struct FloatRates *gyro, struct FloatVect3 *accel, float dt)
void ins_float_invariant_init(void)
void ins_float_invariant_update_gps(struct GpsState *gps_s)
void ins_float_invariant_update_mag(struct FloatVect3 *mag)
struct InsFloatInv ins_float_inv
void ins_float_invariant_update_baro(float pressure)
struct FloatVect3 mag_h
bool reset
flag to request reset/reinit the filter
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
#define INS_FINV_MAG_ID
magnetometer
static abi_event mag_ev
static abi_event accel_ev
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
Call ins_float_invariant_propagate on new gyro measurements.
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
static abi_event gyro_ev
#define INS_FINV_BARO_ID
baro
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
#define INS_FINV_IMU_ID
IMU (gyro, accel)
static abi_event aligner_ev
static abi_event baro_ev
static abi_event geo_mag_ev
#define INS_FINV_GPS_ID
ABI binding for gps data.
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
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)
void ins_float_invariant_wrapper_init(void)
static uint32_t ins_finv_last_stamp
last gyro msg timestamp
static struct FloatVect3 ins_finv_accel
last accel measurement
#define INS_FINV_FILTER_ID
static abi_event gps_ev
Paparazzi specific wrapper to run INVARIANT filter.
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
uint16_t foo
Definition: main_demo5.c:58
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