Paparazzi UAS
v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
Paparazzi specific wrapper to run INVARIANT filter. More...
#include "modules/ins/ins_float_invariant_wrapper.h"
#include "modules/core/abi.h"
#include "mcu_periph/sys_time.h"
#include "message_pragmas.h"
#include "modules/ahrs/ahrs_aligner.h"
#include "modules/datalink/telemetry.h"
#include "state.h"
Go to the source code of this file.
Macros | |
#define | INS_FINV_FILTER_ID 2 |
#define | INS_FINV_BARO_ID ABI_BROADCAST |
baro More... | |
#define | INS_FINV_IMU_ID ABI_BROADCAST |
IMU (gyro, accel) More... | |
#define | INS_FINV_MAG_ID ABI_BROADCAST |
magnetometer More... | |
#define | INS_FINV_GPS_ID GPS_MULTI_ID |
ABI binding for gps data. More... | |
Functions | |
static void | send_ins_ref (struct transport_tx *trans, struct link_device *dev) |
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 | gyro_cb (uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro) |
Call ins_float_invariant_propagate on new gyro measurements. More... | |
static void | accel_cb (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel) |
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 | mag_cb (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag) |
static void | geo_mag_cb (uint8_t sender_id, struct FloatVect3 *h) |
static void | gps_cb (uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s) |
void | ins_float_invariant_wrapper_init (void) |
Variables | |
static struct FloatVect3 | ins_finv_accel |
last accel measurement More... | |
static uint32_t | ins_finv_last_stamp = 0 |
last gyro msg timestamp More... | |
static abi_event | baro_ev |
static abi_event | gyro_ev |
static abi_event | accel_ev |
static abi_event | aligner_ev |
static abi_event | mag_ev |
static abi_event | geo_mag_ev |
static abi_event | gps_ev |
Paparazzi specific wrapper to run INVARIANT filter.
Definition in file ins_float_invariant_wrapper.c.
#define INS_FINV_BARO_ID ABI_BROADCAST |
baro
Definition at line 83 of file ins_float_invariant_wrapper.c.
#define INS_FINV_FILTER_ID 2 |
Definition at line 37 of file ins_float_invariant_wrapper.c.
#define INS_FINV_GPS_ID GPS_MULTI_ID |
ABI binding for gps data.
Used for GPS ABI messages.
Definition at line 108 of file ins_float_invariant_wrapper.c.
#define INS_FINV_IMU_ID ABI_BROADCAST |
IMU (gyro, accel)
Definition at line 90 of file ins_float_invariant_wrapper.c.
#define INS_FINV_MAG_ID ABI_BROADCAST |
magnetometer
Definition at line 97 of file ins_float_invariant_wrapper.c.
|
static |
Definition at line 164 of file ins_float_invariant_wrapper.c.
References ACCELS_FLOAT_OF_BFP, and ins_finv_accel.
Referenced by ins_float_invariant_wrapper_init().
|
static |
Definition at line 171 of file ins_float_invariant_wrapper.c.
References ACCELS_FLOAT_OF_BFP, ins_float_inv, ins_float_invariant_align(), InsFloatInv::is_aligned, MAGS_FLOAT_OF_BFP, and RATES_FLOAT_OF_BFP.
Referenced by ins_float_invariant_wrapper_init().
Definition at line 122 of file ins_float_invariant_wrapper.c.
References ins_float_invariant_update_baro().
Referenced by ins_float_invariant_wrapper_init().
|
static |
Definition at line 200 of file ins_float_invariant_wrapper.c.
References h(), ins_float_inv, and InsFloatInv::mag_h.
Referenced by ins_float_invariant_wrapper_init().
Definition at line 206 of file ins_float_invariant_wrapper.c.
References ins_float_invariant_update_gps().
Referenced by ins_float_invariant_wrapper_init().
|
static |
Call ins_float_invariant_propagate on new gyro measurements.
Since acceleration measurement is also needed for propagation, use the last stored accel from ins_finv_accel.
Definition at line 132 of file ins_float_invariant_wrapper.c.
References ahrs_aligner_restart(), ins_finv_accel, ins_finv_last_stamp, ins_float_inv, ins_float_invariant_propagate(), PRINT_CONFIG_MSG(), PRINT_CONFIG_VAR(), RATES_FLOAT_OF_BFP, and InsFloatInv::reset.
Referenced by ins_float_invariant_wrapper_init().
void ins_float_invariant_wrapper_init | ( | void | ) |
Definition at line 214 of file ins_float_invariant_wrapper.c.
References ABI_BROADCAST, accel_cb(), accel_ev, ahrs_aligner_init(), aligner_cb(), aligner_ev, baro_cb(), baro_ev, DefaultPeriodic, geo_mag_cb(), geo_mag_ev, gps_cb(), gps_ev, gyro_cb(), gyro_ev, INS_FINV_BARO_ID, INS_FINV_GPS_ID, INS_FINV_IMU_ID, INS_FINV_MAG_ID, ins_float_invariant_init(), mag_cb(), mag_ev, register_periodic_telemetry(), send_filter_status(), and send_ins_ref().
|
static |
Definition at line 189 of file ins_float_invariant_wrapper.c.
References ins_float_inv, ins_float_invariant_update_mag(), InsFloatInv::is_aligned, and MAGS_FLOAT_OF_BFP.
Referenced by ins_float_invariant_wrapper_init().
|
static |
Definition at line 61 of file ins_float_invariant_wrapper.c.
References dev, get_sys_time_usec(), INS_FINV_FILTER_ID, ins_finv_last_stamp, ins_float_inv, InsFloatInv::is_aligned, and val.
Referenced by ins_float_invariant_wrapper_init().
|
static |
Definition at line 49 of file ins_float_invariant_wrapper.c.
References LlaCoor_i::alt, dev, LtpDef_i::ecef, foo, LtpDef_i::hmsl, LlaCoor_i::lat, LtpDef_i::lla, LlaCoor_i::lon, State::ned_initialized_i, State::ned_origin_i, state, EcefCoor_i::x, EcefCoor_i::y, and EcefCoor_i::z.
Referenced by ins_float_invariant_wrapper_init().
|
static |
Definition at line 114 of file ins_float_invariant_wrapper.c.
Referenced by ins_float_invariant_wrapper_init().
|
static |
Definition at line 115 of file ins_float_invariant_wrapper.c.
Referenced by ins_float_invariant_wrapper_init().
|
static |
Definition at line 112 of file ins_float_invariant_wrapper.c.
Referenced by ins_float_invariant_wrapper_init().
|
static |
Definition at line 118 of file ins_float_invariant_wrapper.c.
Referenced by ins_float_invariant_wrapper_init().
|
static |
Definition at line 120 of file ins_float_invariant_wrapper.c.
Referenced by ins_float_invariant_wrapper_init().
|
static |
Definition at line 113 of file ins_float_invariant_wrapper.c.
Referenced by ins_float_invariant_wrapper_init().
|
static |
last accel measurement
Definition at line 1 of file ins_float_invariant_wrapper.c.
Referenced by accel_cb(), and gyro_cb().
|
static |
last gyro msg timestamp
Definition at line 44 of file ins_float_invariant_wrapper.c.
Referenced by gyro_cb(), and send_filter_status().
|
static |
Definition at line 117 of file ins_float_invariant_wrapper.c.
Referenced by ins_float_invariant_wrapper_init().