Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 "subsystems/abi.h"
30 #include "mcu_periph/sys_time.h"
31 #include "message_pragmas.h"
32 
33 #ifndef INS_FINV_FILTER_ID
34 #define INS_FINV_FILTER_ID 2
35 #endif
36 
38 static struct Int32Vect3 ins_finv_accel;
39 
42 
43 #if PERIODIC_TELEMETRY && !INS_FINV_USE_UTM
45 #include "state.h"
46 static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
47 {
48  float foo = 0.;
50  pprz_msg_send_INS_REF(trans, dev, AC_ID,
54  &state.ned_origin_i.hmsl, &foo);
55  }
56 }
57 
58 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
59 {
61  uint8_t mde = 3;
62  uint16_t val = 0;
63  if (!ins_float_inv.is_aligned) { mde = 2; }
65  /* set lost if no new gyro measurements for 50ms */
66  if (t_diff > 50000) { mde = 5; }
67  pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &id, &mde, &val);
68 }
69 #endif
70 
71 
72 /*
73  * ABI bindings
74  */
76 #ifndef INS_FINV_BARO_ID
77 #if USE_BARO_BOARD
78 #define INS_FINV_BARO_ID BARO_BOARD_SENDER_ID
79 #else
80 #define INS_FINV_BARO_ID ABI_BROADCAST
81 #endif
82 #endif
83 PRINT_CONFIG_VAR(INS_FINV_BARO_ID)
84 
85 
86 #ifndef INS_FINV_IMU_ID
87 #define INS_FINV_IMU_ID ABI_BROADCAST
88 #endif
89 PRINT_CONFIG_VAR(INS_FINV_IMU_ID)
90 
91 
92 #ifndef INS_FINV_MAG_ID
93 #define INS_FINV_MAG_ID ABI_BROADCAST
94 #endif
95 PRINT_CONFIG_VAR(INS_FINV_MAG_ID)
96 
105 
106 static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
107 {
109 }
110 
116 static void gyro_cb(uint8_t sender_id __attribute__((unused)),
117  uint32_t stamp, struct Int32Rates *gyro)
118 {
119 #if USE_AUTO_INS_FREQ || !defined(INS_PROPAGATE_FREQUENCY)
120  PRINT_CONFIG_MSG("Calculating dt for INS float_invariant propagation.")
121  /* timestamp in usec when last callback was received */
122  static uint32_t last_stamp = 0;
123 
124  if (last_stamp > 0) {
125  float dt = (float)(stamp - last_stamp) * 1e-6;
127  }
128  last_stamp = stamp;
129 #else
130  PRINT_CONFIG_MSG("Using fixed INS_PROPAGATE_FREQUENCY for INS float_invariant propagation.")
131  PRINT_CONFIG_VAR(INS_PROPAGATE_FREQUENCY)
132  const float dt = 1. / (INS_PROPAGATE_FREQUENCY);
134 #endif
135 
136  ins_finv_last_stamp = stamp;
137 }
138 
139 static void accel_cb(uint8_t sender_id __attribute__((unused)),
140  uint32_t stamp __attribute__((unused)),
141  struct Int32Vect3 *accel)
142 {
143  ins_finv_accel = *accel;
144 }
145 
146 static void mag_cb(uint8_t sender_id __attribute__((unused)),
147  uint32_t stamp __attribute__((unused)),
148  struct Int32Vect3 *mag)
149 {
152  }
153 }
154 
155 static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
156  uint32_t stamp __attribute__((unused)),
157  struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
158  struct Int32Vect3 *lp_mag)
159 {
160  if (!ins_float_inv.is_aligned) {
161  ins_float_invariant_align(lp_gyro, lp_accel, lp_mag);
162  }
163 }
164 
165 static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
166  struct FloatQuat *q_b2i_f)
167 {
169 }
170 
171 static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
172 {
173  ins_float_inv.mag_h = *h;
174 }
175 
176 static void gps_cb(uint8_t sender_id __attribute__((unused)),
177  uint32_t stamp __attribute__((unused)),
178  struct GpsState *gps_s)
179 {
181 }
182 
183 
185 {
187 
188  // Bind to ABI messages
189  AbiBindMsgBARO_ABS(INS_FINV_BARO_ID, &baro_ev, baro_cb);
190  AbiBindMsgIMU_MAG_INT32(INS_FINV_MAG_ID, &mag_ev, mag_cb);
191  AbiBindMsgIMU_GYRO_INT32(INS_FINV_IMU_ID, &gyro_ev, gyro_cb);
192  AbiBindMsgIMU_ACCEL_INT32(INS_FINV_IMU_ID, &accel_ev, accel_cb);
193  AbiBindMsgIMU_LOWPASSED(INS_FINV_IMU_ID, &aligner_ev, aligner_cb);
194  AbiBindMsgBODY_TO_IMU_QUAT(INS_FINV_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
195  AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
196  AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb);
197 
198 #if PERIODIC_TELEMETRY && !INS_FINV_USE_UTM
199  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_REF, send_ins_ref);
200  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
201 #endif
202 }
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
static abi_event gps_ev
void ins_float_invariant_update_mag(struct Int32Vect3 *mag)
struct FloatVect3 mag_h
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
static abi_event aligner_ev
unsigned short uint16_t
Definition: types.h:16
int32_t z
in centimeters
#define INS_FINV_IMU_ID
IMU (gyro, accel)
angular rates
void ins_float_invariant_propagate(struct Int32Rates *gyro, struct Int32Vect3 *accel, float dt)
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 body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f)
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
Call ins_float_invariant_propagate on new gyro measurements.
Generic transmission transport header.
Definition: transport.h:89
static abi_event geo_mag_ev
Periodic telemetry system header (includes downlink utility and generated code).
void ins_float_invariant_register(void)
int32_t y
in centimeters
Main include for ABI (AirBorneInterface).
void ins_float_invariant_update_gps(struct GpsState *gps_s)
struct EcefCoor_i ecef
Reference point in ecef.
int32_t hmsl
Height above mean sea level in mm.
static abi_event mag_ev
int32_t alt
in millimeters above WGS84 reference ellipsoid
static uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.h:39
Roation quaternion.
Paparazzi specific wrapper to run INVARIANT filter.
#define INS_FINV_MAG_ID
magnetometer
Architecture independent timing functions.
static struct Int32Vect3 ins_finv_accel
last accel measurement
data structure for GPS information
Definition: gps.h:67
uint16_t val[TCOUPLE_NB]
unsigned long uint32_t
Definition: types.h:18
static abi_event accel_ev
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
static void geo_mag_cb(uint8_t sender_id, struct FloatVect3 *h)
struct LlaCoor_i lla
Reference point in lla.
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
int32_t lon
in degrees*1e7
uint16_t foo
Definition: main_demo5.c:59
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:69
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:162
void ins_float_invariant_update_baro(float pressure)
static abi_event body_to_imu_ev
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
void ins_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i)
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
static abi_event gyro_ev
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static abi_event baro_ev
int32_t x
in centimeters
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:56
void ins_float_invariant_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
bool_t ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:167
static uint32_t ins_finv_last_stamp
last gyro msg timestamp
int32_t lat
in degrees*1e7
struct InsFloatInv ins_float_inv
void ins_register_impl(InsInit init)
Definition: ins.c:53
#define INS_FINV_FILTER_ID
void ins_float_invariant_init(void)
struct State state
Definition: state.c:36
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
static void baro_cb(uint8_t sender_id, float pressure)
#define INS_FINV_BARO_ID
baro