|
Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
Paparazzi specific wrapper to run MEKF-Wind INS filter. More...
#include "modules/ins/ins_mekf_wind_wrapper.h"#include "modules/ins/ins_mekf_wind.h"#include "modules/air_data/air_data.h"#include "modules/ahrs/ahrs_float_utils.h"#include "modules/ahrs/ahrs_aligner.h"#include "modules/ins/ins.h"#include "modules/core/abi.h"#include "math/pprz_isa.h"#include "state.h"#include "generated/airframe.h"#include "generated/flight_plan.h"#include "modules/datalink/telemetry.h"#include "mcu_periph/sys_time.h"Go to the source code of this file.
Macros | |
| #define | INS_MEKF_WIND_FILTER_ID 3 |
| #define | INS_MEKF_WIND_AIRSPEED_ID AIR_DATA_SENDER_ID |
| airspeed (Pitot tube) | |
| #define | INS_MEKF_WIND_INCIDENCE_ID ABI_BROADCAST |
| incidence angles | |
| #define | INS_MEKF_WIND_BARO_ID AIR_DATA_SENDER_ID |
| baro | |
| #define | INS_MEKF_WIND_IMU_ID ABI_BROADCAST |
| IMU (gyro, accel). | |
| #define | INS_MEKF_WIND_MAG_ID ABI_BROADCAST |
| magnetometer | |
| #define | INS_MEKF_WIND_GPS_ID GPS_MULTI_ID |
| ABI binding for gps data. | |
Functions | |
| static void | set_state_from_ins (void) |
| update state interface | |
| static void | send_euler (struct transport_tx *trans, struct link_device *dev) |
| logging functions | |
| static void | send_wind (struct transport_tx *trans, struct link_device *dev) |
| static void | send_filter_status (struct transport_tx *trans, struct link_device *dev) |
| static void | send_inv_filter (struct transport_tx *trans, struct link_device *dev) |
| static void | reset_cb (uint8_t sender_id, uint8_t flag) |
| static void | baro_cb (uint8_t sender_id, uint32_t stamp, float pressure) |
| static void | pressure_diff_cb (uint8_t sender_id, float pdyn) |
| static void | airspeed_cb (uint8_t sender_id, float airspeed) |
| static void | incidence_cb (uint8_t sender_id, uint8_t flag, float aoa, float sideslip) |
| static void | gyro_cb (uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro) |
| Call ins_mekf_wind_propagate on new gyro measurements. | |
| static void | accel_cb (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel) |
| static void | mag_cb (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag) |
| 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 | 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_mekf_wind_wrapper_init (void) |
| Init function. | |
| static void | reset_ref (void) |
| static void | reset_vertical_ref (void) |
Variables | |
| struct InsMekfWind | ins_mekf_wind |
| static struct FloatVect3 | ins_mekf_wind_accel |
| last accel measurement | |
| static uint32_t | last_imu_stamp = 0 |
| static abi_event | pressure_diff_ev |
| static abi_event | airspeed_ev |
| static abi_event | incidence_ev |
| static abi_event | baro_ev |
| static abi_event | mag_ev |
| static abi_event | gyro_ev |
| static abi_event | accel_ev |
| static abi_event | aligner_ev |
| static abi_event | geo_mag_ev |
| static abi_event | gps_ev |
| static abi_event | reset_ev |
Paparazzi specific wrapper to run MEKF-Wind INS filter.
Definition in file ins_mekf_wind_wrapper.c.
| #define INS_MEKF_WIND_AIRSPEED_ID AIR_DATA_SENDER_ID |
airspeed (Pitot tube)
Definition at line 160 of file ins_mekf_wind_wrapper.c.
Referenced by ins_mekf_wind_wrapper_init().
| #define INS_MEKF_WIND_BARO_ID AIR_DATA_SENDER_ID |
baro
Definition at line 172 of file ins_mekf_wind_wrapper.c.
Referenced by ins_mekf_wind_wrapper_init().
| #define INS_MEKF_WIND_FILTER_ID 3 |
Definition at line 47 of file ins_mekf_wind_wrapper.c.
Referenced by send_euler(), and send_filter_status().
| #define INS_MEKF_WIND_GPS_ID GPS_MULTI_ID |
ABI binding for gps data.
Used for GPS ABI messages.
Definition at line 192 of file ins_mekf_wind_wrapper.c.
Referenced by ins_mekf_wind_wrapper_init().
| #define INS_MEKF_WIND_IMU_ID ABI_BROADCAST |
IMU (gyro, accel).
Definition at line 178 of file ins_mekf_wind_wrapper.c.
Referenced by ins_mekf_wind_wrapper_init().
| #define INS_MEKF_WIND_INCIDENCE_ID ABI_BROADCAST |
incidence angles
Definition at line 166 of file ins_mekf_wind_wrapper.c.
Referenced by ins_mekf_wind_wrapper_init().
| #define INS_MEKF_WIND_MAG_ID ABI_BROADCAST |
magnetometer
Definition at line 184 of file ins_mekf_wind_wrapper.c.
Referenced by ins_mekf_wind_wrapper_init().
|
static |
Definition at line 375 of file ins_mekf_wind_wrapper.c.
References ACCELS_FLOAT_OF_BFP, and ins_mekf_wind_accel.
|
static |
Definition at line 266 of file ins_mekf_wind_wrapper.c.
References get_sys_time_float(), ins_mekf_wind, ins_mekf_wind_update_airspeed(), LogFileIsOpen, pprzLogFile, and tas_from_eas().
|
static |
Definition at line 404 of file ins_mekf_wind_wrapper.c.
References ACCELS_FLOAT_OF_BFP, ahrs_float_get_quat_from_accel_mag(), ins_mekf_wind, ins_mekf_wind_align(), MAGS_FLOAT_OF_BFP, RATES_FLOAT_OF_BFP, and set_state_from_ins().
Definition at line 210 of file ins_mekf_wind_wrapper.c.
References alpha, baro_cb(), if(), ins_mekf_wind, and PPRZ_ISA_SEA_LEVEL_PRESSURE.
|
static |
Definition at line 430 of file ins_mekf_wind_wrapper.c.
References h(), and ins_mekf_wind_set_mag_h().
Definition at line 435 of file ins_mekf_wind_wrapper.c.
References UtmCoor_f::alt, UtmCoor_f::east, ecef_int_from_gps(), ecef_vel_float_from_gps(), GpsState::fix, get_sys_time_float(), GPS_FIX_3D, ins_mekf_wind, ins_mekf_wind_set_pos_ned(), ins_mekf_wind_set_speed_ned(), ins_mekf_wind_update_pos_speed(), INT32_POS_OF_CM_DEN, INT32_POS_OF_CM_NUM, INT32_VECT3_SCALE_2, LogFileIsOpen, nav_utm_zone0, NED_FLOAT_OF_BFP, ned_of_ecef_point_i(), ned_of_ecef_vect_f(), ned_vel_float_from_gps(), UtmCoor_f::north, pprzLogFile, state, stateGetHmslOrigin_f(), stateGetNedOrigin_f(), stateGetNedOrigin_i(), stateGetUtmOrigin_f(), utm_float_from_gps(), NedCoor_f::x, NedCoor_f::y, and NedCoor_f::z.
|
static |
Call ins_mekf_wind_propagate on new gyro measurements.
Since acceleration measurement is also needed for propagation, use the last stored accel from #ins_mekfw_accel.
Definition at line 297 of file ins_mekf_wind_wrapper.c.
References float_eulers_of_quat(), get_sys_time_float(), ins_mekf_wind, ins_mekf_wind_accel, ins_mekf_wind_get_accel_bias(), ins_mekf_wind_get_accel_ned(), ins_mekf_wind_get_airspeed_norm(), ins_mekf_wind_get_baro_bias(), ins_mekf_wind_get_body_rates(), ins_mekf_wind_get_pos_ned(), ins_mekf_wind_get_quat(), ins_mekf_wind_get_rates_bias(), ins_mekf_wind_get_speed_ned(), ins_mekf_wind_get_wind_ned(), ins_mekf_wind_propagate(), ins_mekf_wind_propagate_ahrs(), last_imu_stamp, LogFileIsOpen, FloatRates::p, FloatEulers::phi, pprzLogFile, PRINT_CONFIG_MSG(), PRINT_CONFIG_VAR(), FloatEulers::psi, FloatRates::q, FloatRates::r, RATES_FLOAT_OF_BFP, set_state_from_ins(), FloatEulers::theta, FloatVect3::x, NedCoor_f::x, FloatVect3::y, NedCoor_f::y, FloatVect3::z, and NedCoor_f::z.
Definition at line 279 of file ins_mekf_wind_wrapper.c.
References get_sys_time_float(), ins_mekf_wind, ins_mekf_wind_update_incidence(), LogFileIsOpen, and pprzLogFile.
| void ins_mekf_wind_wrapper_init | ( | void | ) |
Init function.
Definition at line 517 of file ins_mekf_wind_wrapper.c.
References ABI_BROADCAST, accel_cb(), accel_ev, ahrs_aligner_init(), airspeed_cb(), airspeed_ev, aligner_cb(), aligner_ev, LlaCoor_i::alt, UtmCoor_f::alt, baro_cb(), baro_ev, counter, DefaultPeriodic, UtmCoor_f::east, ecef_of_lla_i(), geo_mag_cb(), geo_mag_ev, gps_cb(), gps_ev, gyro_cb(), gyro_ev, incidence_cb(), incidence_ev, ins_mekf_wind, INS_MEKF_WIND_AIRSPEED_ID, INS_MEKF_WIND_BARO_ID, INS_MEKF_WIND_GPS_ID, INS_MEKF_WIND_IMU_ID, INS_MEKF_WIND_INCIDENCE_ID, ins_mekf_wind_init(), INS_MEKF_WIND_MAG_ID, ins_mekf_wind_set_mag_h(), LlaCoor_i::lat, LlaCoor_i::lon, ltp_def, ltp_def_from_ecef_i(), mag_cb(), mag_ev, nav_utm_east0, nav_utm_north0, nav_utm_zone0, UtmCoor_f::north, pprzLogFile, pressure_diff_cb(), pressure_diff_ev, register_periodic_telemetry(), reset_cb(), reset_ev, send_euler(), send_filter_status(), send_inv_filter(), send_wind(), stateSetLocalOrigin_i(), stateSetLocalUtmOrigin_f(), stateSetPositionUtm_f(), and UtmCoor_f::zone.
|
static |
Definition at line 382 of file ins_mekf_wind_wrapper.c.
References get_sys_time_float(), ins_mekf_wind, ins_mekf_wind_update_mag(), LogFileIsOpen, MAGS_FLOAT_OF_BFP, pprzLogFile, FloatVect3::x, FloatVect3::y, and FloatVect3::z.
|
static |
Definition at line 252 of file ins_mekf_wind_wrapper.c.
References get_sys_time_float(), ins_mekf_wind, ins_mekf_wind_update_airspeed(), LogFileIsOpen, pprzLogFile, and tas_from_dynamic_pressure().
Referenced by ins_mekf_wind_wrapper_init().
Definition at line 638 of file ins_mekf_wind_wrapper.c.
References INS_RESET_REF, INS_RESET_VERTICAL_REF, reset_ref(), reset_vertical_ref(), and UNUSED.
|
static |
Definition at line 600 of file ins_mekf_wind_wrapper.c.
References LlaCoor_i::alt, ecef_int_from_gps(), gps, ins_mekf_wind_wrapper_Reset, lla_int_from_gps(), ltp_def, ltp_def_from_ecef_i(), stateSetLocalOrigin_i(), stateSetLocalUtmOrigin_f(), and utm_float_from_gps().
Referenced by reset_cb().
|
static |
Definition at line 619 of file ins_mekf_wind_wrapper.c.
References UtmCoor_f::alt, gps, LlaCoor_i::lat, LtpDef_i::lla, LlaCoor_i::lon, ltp_def, ltp_def_from_lla_i(), stateGetNedOrigin_i(), stateGetUtmOrigin_f(), stateSetLocalOrigin_i(), and stateSetLocalUtmOrigin_f().
Referenced by reset_cb().
|
static |
logging functions
telemetry functions
Definition at line 78 of file ins_mekf_wind_wrapper.c.
References dev, float_eulers_of_quat(), INS_MEKF_WIND_FILTER_ID, ins_mekf_wind_get_quat(), FloatEulers::phi, FloatEulers::psi, and FloatEulers::theta.
|
static |
Definition at line 102 of file ins_mekf_wind_wrapper.c.
References dev, get_sys_time_usec(), ins_mekf_wind, INS_MEKF_WIND_FILTER_ID, last_imu_stamp, and val.
|
static |
Definition at line 120 of file ins_mekf_wind_wrapper.c.
References dev, float_eulers_of_quat(), ins_mekf_wind_get_accel_bias(), ins_mekf_wind_get_airspeed_norm(), ins_mekf_wind_get_pos_ned(), ins_mekf_wind_get_quat(), ins_mekf_wind_get_rates_bias(), ins_mekf_wind_get_speed_ned(), FloatRates::p, FloatEulers::phi, FloatEulers::psi, FloatRates::q, FloatQuat::qi, FloatRates::r, FloatEulers::theta, FloatVect3::x, NedCoor_f::x, FloatVect3::y, NedCoor_f::y, FloatVect3::z, and NedCoor_f::z.
|
static |
Definition at line 91 of file ins_mekf_wind_wrapper.c.
References dev, ENU_OF_TO_NED, ins_mekf_wind_get_airspeed_norm(), ins_mekf_wind_get_wind_ned(), EnuCoor_f::x, EnuCoor_f::y, and EnuCoor_f::z.
Referenced by ins_mekf_wind_wrapper_init(), and wind_estimation_quadrotor_init().
|
static |
update state interface
Set current state (ltp to body orientation/rates and ned pos/speed/accel.
Definition at line 496 of file ins_mekf_wind_wrapper.c.
References ins_mekf_wind_get_accel_ned(), ins_mekf_wind_get_body_rates(), ins_mekf_wind_get_pos_ned(), ins_mekf_wind_get_quat(), ins_mekf_wind_get_speed_ned(), stateSetAccelNed_f(), stateSetBodyRates_f(), stateSetNedToBodyQuat_f(), stateSetPositionNed_f(), and stateSetSpeedNed_f().
Referenced by aligner_cb(), and gyro_cb().
|
static |
Definition at line 202 of file ins_mekf_wind_wrapper.c.
|
static |
Definition at line 197 of file ins_mekf_wind_wrapper.c.
|
static |
Definition at line 203 of file ins_mekf_wind_wrapper.c.
|
static |
Definition at line 199 of file ins_mekf_wind_wrapper.c.
|
static |
Definition at line 204 of file ins_mekf_wind_wrapper.c.
|
static |
Definition at line 205 of file ins_mekf_wind_wrapper.c.
|
static |
Definition at line 201 of file ins_mekf_wind_wrapper.c.
|
static |
Definition at line 198 of file ins_mekf_wind_wrapper.c.
| struct InsMekfWind ins_mekf_wind |
Definition at line 50 of file ins_mekf_wind_wrapper.c.
Referenced by airspeed_cb(), aligner_cb(), baro_cb(), gps_cb(), gyro_cb(), incidence_cb(), ins_mekf_wind_wrapper_init(), mag_cb(), pressure_diff_cb(), and send_filter_status().
|
static |
last accel measurement
Definition at line 53 of file ins_mekf_wind_wrapper.c.
Referenced by accel_cb(), and gyro_cb().
|
static |
Definition at line 54 of file ins_mekf_wind_wrapper.c.
Referenced by gyro_cb(), and send_filter_status().
|
static |
Definition at line 200 of file ins_mekf_wind_wrapper.c.
|
static |
Definition at line 196 of file ins_mekf_wind_wrapper.c.
Referenced by ins_mekf_wind_wrapper_init().
|
static |
Definition at line 206 of file ins_mekf_wind_wrapper.c.