Paparazzi UAS  v6.2_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 
117 #if USE_MAGNETOMETER
120 #endif
122 
123 static void baro_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) uint32_t stamp, float pressure)
124 {
126 }
127 
133 static void gyro_cb(uint8_t sender_id __attribute__((unused)),
134  uint32_t stamp, struct Int32Rates *gyro)
135 {
136  struct FloatRates gyro_f;
137  RATES_FLOAT_OF_BFP(gyro_f, *gyro);
138 
139 #if USE_AHRS_ALIGNER
140  if (ins_float_inv.reset) {
142  }
143 #endif
144 
145 #if USE_AUTO_INS_FREQ || !defined(INS_PROPAGATE_FREQUENCY)
146  PRINT_CONFIG_MSG("Calculating dt for INS float_invariant propagation.")
147  /* timestamp in usec when last callback was received */
148  static uint32_t last_stamp = 0;
149 
150  if (last_stamp > 0) {
151  float dt = (float)(stamp - last_stamp) * 1e-6;
153  }
154  last_stamp = stamp;
155 #else
156  PRINT_CONFIG_MSG("Using fixed INS_PROPAGATE_FREQUENCY for INS float_invariant propagation.")
157  PRINT_CONFIG_VAR(INS_PROPAGATE_FREQUENCY)
158  const float dt = 1. / (INS_PROPAGATE_FREQUENCY);
160 #endif
161 
162  ins_finv_last_stamp = stamp;
163 }
164 
165 static void accel_cb(uint8_t sender_id __attribute__((unused)),
166  uint32_t stamp __attribute__((unused)),
167  struct Int32Vect3 *accel)
168 {
170 }
171 
172 static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
173  uint32_t stamp __attribute__((unused)),
174  struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
175  struct Int32Vect3 *lp_mag)
176 {
177  if (!ins_float_inv.is_aligned) {
178  /* convert to float */
179  struct FloatRates gyro_f;
180  RATES_FLOAT_OF_BFP(gyro_f, *lp_gyro);
181  struct FloatVect3 accel_f;
182  ACCELS_FLOAT_OF_BFP(accel_f, *lp_accel);
183  struct FloatVect3 mag_f;
184  MAGS_FLOAT_OF_BFP(mag_f, *lp_mag);
185  ins_float_invariant_align(&gyro_f, &accel_f, &mag_f);
186  }
187 }
188 
189 static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
190  struct FloatQuat *q_b2i_f)
191 {
193 }
194 
195 #if USE_MAGNETOMETER
196 static void mag_cb(uint8_t sender_id __attribute__((unused)),
197  uint32_t stamp __attribute__((unused)),
198  struct Int32Vect3 *mag)
199 {
201  struct FloatVect3 mag_f;
202  MAGS_FLOAT_OF_BFP(mag_f, *mag);
204  }
205 }
206 
207 static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
208 {
209  ins_float_inv.mag_h = *h;
210 }
211 #endif
212 
213 static void gps_cb(uint8_t sender_id __attribute__((unused)),
214  uint32_t stamp __attribute__((unused)),
215  struct GpsState *gps_s)
216 {
218 }
219 
220 
222 {
223  // aligner
224 #if USE_AHRS_ALIGNER
226 #endif
227 
229 
230  // Bind to ABI messages
231  AbiBindMsgBARO_ABS(INS_FINV_BARO_ID, &baro_ev, baro_cb);
232  AbiBindMsgIMU_GYRO_INT32(INS_FINV_IMU_ID, &gyro_ev, gyro_cb);
233  AbiBindMsgIMU_ACCEL_INT32(INS_FINV_IMU_ID, &accel_ev, accel_cb);
234  AbiBindMsgIMU_LOWPASSED(INS_FINV_IMU_ID, &aligner_ev, aligner_cb);
235  AbiBindMsgBODY_TO_IMU_QUAT(INS_FINV_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
236 #if USE_MAGNETOMETER
237  AbiBindMsgIMU_MAG_INT32(INS_FINV_MAG_ID, &mag_ev, mag_cb);
238  AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
239 #endif
240  AbiBindMsgGPS(INS_FINV_GPS_ID, &gps_ev, gps_cb);
241 
242 #if PERIODIC_TELEMETRY && !INS_FINV_USE_UTM
244  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
245 #endif
246 }
InsFloatInv::reset
bool reset
flag to request reset/reinit the filter
Definition: ins_float_invariant.h:113
baro_ev
static abi_event baro_ev
Definition: ins_float_invariant_wrapper.c:112
baro_cb
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
Definition: ins_float_invariant_wrapper.c:123
MAGS_FLOAT_OF_BFP
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:807
LlaCoor_i::lon
int32_t lon
in degrees*1e7
Definition: pprz_geodetic_int.h:61
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
val
uint16_t val[TCOUPLE_NB]
Definition: temp_tcouple_adc.c:49
ins_float_invariant_wrapper_init
void ins_float_invariant_wrapper_init(void)
Definition: ins_float_invariant_wrapper.c:221
LlaCoor_i::alt
int32_t alt
in millimeters above WGS84 reference ellipsoid
Definition: pprz_geodetic_int.h:62
gyro_ev
static abi_event gyro_ev
Definition: ins_float_invariant_wrapper.c:113
gps_cb
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: ins_float_invariant_wrapper.c:213
Int32Rates
angular rates
Definition: pprz_algebra_int.h:179
State::ned_origin_i
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
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
ins_finv_last_stamp
static uint32_t ins_finv_last_stamp
last gyro msg timestamp
Definition: ins_float_invariant_wrapper.c:44
INS_FINV_MAG_ID
#define INS_FINV_MAG_ID
magnetometer
Definition: ins_float_invariant_wrapper.c:97
LtpDef_i::ecef
struct EcefCoor_i ecef
Reference point in ecef.
Definition: pprz_geodetic_int.h:99
GpsState
data structure for GPS information
Definition: gps.h:87
INS_FINV_GPS_ID
#define INS_FINV_GPS_ID
ABI binding for gps data.
Definition: ins_float_invariant_wrapper.c:108
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:66
EcefCoor_i::x
int32_t x
in centimeters
Definition: pprz_geodetic_int.h:51
ins_finv_accel
static struct FloatVect3 ins_finv_accel
last accel measurement
Definition: ins_float_invariant_wrapper.c:41
INS_FINV_IMU_ID
#define INS_FINV_IMU_ID
IMU (gyro, accel)
Definition: ins_float_invariant_wrapper.c:90
aligner_ev
static abi_event aligner_ev
Definition: ins_float_invariant_wrapper.c:115
body_to_imu_ev
static abi_event body_to_imu_ev
Definition: ins_float_invariant_wrapper.c:116
foo
uint16_t foo
Definition: main_demo5.c:58
State::ned_initialized_i
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:171
ahrs_aligner.h
EcefCoor_i::y
int32_t y
in centimeters
Definition: pprz_geodetic_int.h:52
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:75
FloatVect3
Definition: pprz_algebra_float.h:54
LlaCoor_i::lat
int32_t lat
in degrees*1e7
Definition: pprz_geodetic_int.h:60
telemetry.h
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
EcefCoor_i::z
int32_t z
in centimeters
Definition: pprz_geodetic_int.h:53
body_to_imu_cb
static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f)
Definition: ins_float_invariant_wrapper.c:189
ins_float_invariant_init
void ins_float_invariant_init(void)
Definition: ins_float_invariant.c:219
geo_mag_ev
static abi_event geo_mag_ev
Definition: ins_float_invariant_wrapper.c:119
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
Int32Vect3
Definition: pprz_algebra_int.h:88
sys_time.h
Architecture independent timing functions.
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:51
ins_float_invariant_update_mag
void ins_float_invariant_update_mag(struct FloatVect3 *mag)
Definition: ins_float_invariant.c:565
ins_float_invariant_update_gps
void ins_float_invariant_update_gps(struct GpsState *gps_s)
Definition: ins_float_invariant.c:445
InsFloatInv::mag_h
struct FloatVect3 mag_h
Definition: ins_float_invariant.h:118
send_ins_ref
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
Definition: ins_float_invariant_wrapper.c:49
INS_FINV_BARO_ID
#define INS_FINV_BARO_ID
baro
Definition: ins_float_invariant_wrapper.c:83
ahrs_aligner_restart
void ahrs_aligner_restart(void)
Definition: ahrs_aligner.c:93
gps_ev
static abi_event gps_ev
Definition: ins_float_invariant_wrapper.c:121
PRINT_CONFIG_MSG
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
INS_FINV_FILTER_ID
#define INS_FINV_FILTER_ID
Definition: ins_float_invariant_wrapper.c:37
send_filter_status
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
Definition: ins_float_invariant_wrapper.c:61
mag_ev
static abi_event mag_ev
Definition: ins_float_invariant_wrapper.c:118
ins_float_invariant_align
void ins_float_invariant_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
Definition: ins_float_invariant.c:321
LtpDef_i::hmsl
int32_t hmsl
Height above mean sea level in mm.
Definition: pprz_geodetic_int.h:102
InsFloatInv::is_aligned
bool is_aligned
Definition: ins_float_invariant.h:119
gyro_cb
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
Call ins_float_invariant_propagate on new gyro measurements.
Definition: ins_float_invariant_wrapper.c:133
accel_cb
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
Definition: ins_float_invariant_wrapper.c:165
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: ins_float_invariant_wrapper.c:172
ins_float_invariant_wrapper.h
ACCELS_FLOAT_OF_BFP
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
ins_float_invariant_update_baro
void ins_float_invariant_update_baro(float pressure)
Definition: ins_float_invariant.c:529
ins_float_inv
struct InsFloatInv ins_float_inv
Definition: ins_float_invariant.c:137
ins_float_invariant_propagate
void ins_float_invariant_propagate(struct FloatRates *gyro, struct FloatVect3 *accel, float dt)
Definition: ins_float_invariant.c:343
accel_ev
static abi_event accel_ev
Definition: ins_float_invariant_wrapper.c:114
state.h
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
ABI_BROADCAST
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:57
state
struct State state
Definition: state.c:36
geo_mag_cb
static void geo_mag_cb(uint8_t sender_id, struct FloatVect3 *h)
Definition: ins_float_invariant_wrapper.c:207
LtpDef_i::lla
struct LlaCoor_i lla
Reference point in lla.
Definition: pprz_geodetic_int.h:100
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
mag_cb
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
Definition: ins_float_invariant_wrapper.c:196
ins_float_inv_set_body_to_imu_quat
void ins_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i)
Definition: ins_float_invariant.c:751
ahrs_aligner_init
void ahrs_aligner_init(void)
Definition: ahrs_aligner.c:81
FloatRates
angular rates
Definition: pprz_algebra_float.h:93