![]() |
Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
#include "modules/meteo/ekf_aw.h"
#include <iostream>
#include <stdio.h>
#include "std.h"
#include <math.h>
#include "mcu_periph/sys_time.h"
#include <matrix/math.hpp>
Go to the source code of this file.
Data Structures | |
struct | ekfAwState |
struct | ekfAwInputs |
struct | ekfAwMeasurements |
struct | ekfAwForces |
struct | ekfAwPrivate |
Typedefs | |
typedef matrix::SquareMatrix< float, EKF_AW_COV_SIZE > | EKF_Aw_Cov |
typedef matrix::SquareMatrix< float, EKF_AW_Q_SIZE > | EKF_Aw_Q |
typedef matrix::SquareMatrix< float, EKF_AW_R_SIZE > | EKF_Aw_R |
Variables | |
struct ekfAwParameters | ekf_aw_params |
static struct ekfAwPrivate | ekf_aw_private |
float | deg2rad = M_PI / 180.0 |
float | rad2deg = 180.0 / M_PI |
struct ekfAwState |
Definition at line 43 of file ekf_aw.cpp.
Data Fields | ||
---|---|---|
Vector3f | offset | |
Vector3f | V_body | |
Vector3f | wind |
struct ekfAwInputs |
struct ekfAwMeasurements |
Definition at line 61 of file ekf_aw.cpp.
Data Fields | ||
---|---|---|
Vector3f | accel_filt | |
Vector3f | V_gnd | |
float | V_pitot |
struct ekfAwForces |
struct ekfAwPrivate |
Definition at line 77 of file ekf_aw.cpp.
Data Fields | ||
---|---|---|
struct ekfAwForces | forces | |
struct ekfHealth | health | |
struct ekfAwMeasurements | innovations | |
struct ekfAwInputs | inputs | |
struct ekfAwMeasurements | measurements | |
EKF_Aw_Cov | P | |
EKF_Aw_Q | Q | |
EKF_Aw_R | R | |
struct ekfAwState | state |
#define eawp ekf_aw_private |
Definition at line 382 of file ekf_aw.cpp.
#define EKF_AW_AOA_MAX_ANGLE 15.0f |
Definition at line 337 of file ekf_aw.cpp.
#define EKF_AW_AOA_MIN_ANGLE -15.0f |
Definition at line 340 of file ekf_aw.cpp.
#define EKF_AW_AX_SCHED_END_DEG 60 |
Definition at line 318 of file ekf_aw.cpp.
#define EKF_AW_AX_SCHED_GAIN 0 |
Definition at line 312 of file ekf_aw.cpp.
#define EKF_AW_AX_SCHED_START_DEG 40 |
Definition at line 315 of file ekf_aw.cpp.
#define EKF_AW_AZ_QUICK_CONV_ACCEL_GAIN 0 |
Definition at line 326 of file ekf_aw.cpp.
#define EKF_AW_AZ_QUICK_CONV_MU_GAIN -2 |
Definition at line 323 of file ekf_aw.cpp.
#define EKF_AW_AZ_SCHED_END_DEG 70 |
Definition at line 309 of file ekf_aw.cpp.
#define EKF_AW_AZ_SCHED_GAIN 0 |
Definition at line 303 of file ekf_aw.cpp.
#define EKF_AW_AZ_SCHED_START_DEG 60 |
Definition at line 306 of file ekf_aw.cpp.
Definition at line 356 of file ekf_aw.cpp.
#define EKF_AW_ELEV_MAX_ANGLE 37.0f |
Definition at line 331 of file ekf_aw.cpp.
#define EKF_AW_ELEV_MIN_ANGLE -10.0f |
Definition at line 334 of file ekf_aw.cpp.
Definition at line 163 of file ekf_aw.cpp.
#define EKF_AW_K1_FX_ELEV 0.0f |
Definition at line 219 of file ekf_aw.cpp.
#define EKF_AW_K1_FX_FUSELAGE 0.0f |
Definition at line 170 of file ekf_aw.cpp.
#define EKF_AW_K1_FX_HOVER 0.0f |
Definition at line 183 of file ekf_aw.cpp.
Definition at line 209 of file ekf_aw.cpp.
Definition at line 193 of file ekf_aw.cpp.
#define EKF_AW_K1_FY_WING 0.0f |
Definition at line 236 of file ekf_aw.cpp.
#define EKF_AW_K1_FZ_ELEV 0.0f |
Definition at line 295 of file ekf_aw.cpp.
#define EKF_AW_K1_FZ_FUSELAGE 0.0f |
Definition at line 253 of file ekf_aw.cpp.
Definition at line 279 of file ekf_aw.cpp.
Definition at line 266 of file ekf_aw.cpp.
Definition at line 166 of file ekf_aw.cpp.
#define EKF_AW_K2_FX_ELEV 0.0f |
Definition at line 222 of file ekf_aw.cpp.
Definition at line 173 of file ekf_aw.cpp.
#define EKF_AW_K2_FX_HOVER 0.0f |
Definition at line 186 of file ekf_aw.cpp.
Definition at line 212 of file ekf_aw.cpp.
Definition at line 196 of file ekf_aw.cpp.
#define EKF_AW_K2_FY_WING 0.0f |
Definition at line 239 of file ekf_aw.cpp.
#define EKF_AW_K2_FZ_ELEV 0.0f |
Definition at line 298 of file ekf_aw.cpp.
#define EKF_AW_K2_FZ_FUSELAGE 0.0f |
Definition at line 256 of file ekf_aw.cpp.
Definition at line 282 of file ekf_aw.cpp.
Definition at line 269 of file ekf_aw.cpp.
#define EKF_AW_K3_FX_ELEV 0.0f |
Definition at line 225 of file ekf_aw.cpp.
#define EKF_AW_K3_FX_FUSELAGE 0.0f |
Definition at line 176 of file ekf_aw.cpp.
Definition at line 189 of file ekf_aw.cpp.
Definition at line 215 of file ekf_aw.cpp.
Definition at line 199 of file ekf_aw.cpp.
#define EKF_AW_K3_FY_WING 0.0f |
Definition at line 242 of file ekf_aw.cpp.
#define EKF_AW_K3_FZ_FUSELAGE 0.0f |
Definition at line 259 of file ekf_aw.cpp.
Definition at line 285 of file ekf_aw.cpp.
Definition at line 272 of file ekf_aw.cpp.
#define EKF_AW_K4_FX_FUSELAGE 0.0f |
Definition at line 179 of file ekf_aw.cpp.
Definition at line 202 of file ekf_aw.cpp.
#define EKF_AW_K4_FY_WING 0.0f |
Definition at line 245 of file ekf_aw.cpp.
#define EKF_AW_K4_FZ_FUSELAGE 0.0f |
Definition at line 262 of file ekf_aw.cpp.
Definition at line 288 of file ekf_aw.cpp.
Definition at line 275 of file ekf_aw.cpp.
Definition at line 205 of file ekf_aw.cpp.
#define EKF_AW_K5_FY_WING 0.0f |
Definition at line 248 of file ekf_aw.cpp.
#define EKF_AW_K5_FZ_HOVER 0.0f |
Definition at line 291 of file ekf_aw.cpp.
Definition at line 230 of file ekf_aw.cpp.
Definition at line 233 of file ekf_aw.cpp.
#define EKF_AW_P0_MU EKF_AW_Q_MU*1.E1f |
Definition at line 110 of file ekf_aw.cpp.
#define EKF_AW_P0_OFFSET EKF_AW_Q_OFFSET |
Definition at line 113 of file ekf_aw.cpp.
Definition at line 107 of file ekf_aw.cpp.
Definition at line 150 of file ekf_aw.cpp.
Definition at line 93 of file ekf_aw.cpp.
Definition at line 96 of file ekf_aw.cpp.
Definition at line 99 of file ekf_aw.cpp.
Definition at line 102 of file ekf_aw.cpp.
Definition at line 121 of file ekf_aw.cpp.
Definition at line 124 of file ekf_aw.cpp.
Definition at line 127 of file ekf_aw.cpp.
Definition at line 118 of file ekf_aw.cpp.
Definition at line 130 of file ekf_aw.cpp.
#define EKF_AW_SKEW_POLY_0 0.0f |
Definition at line 345 of file ekf_aw.cpp.
#define EKF_AW_SKEW_POLY_1 1.0f |
Definition at line 348 of file ekf_aw.cpp.
#define EKF_AW_SKEW_POLY_2 0.0f |
Definition at line 351 of file ekf_aw.cpp.
Definition at line 147 of file ekf_aw.cpp.
Definition at line 138 of file ekf_aw.cpp.
Definition at line 141 of file ekf_aw.cpp.
Definition at line 144 of file ekf_aw.cpp.
Definition at line 153 of file ekf_aw.cpp.
Definition at line 158 of file ekf_aw.cpp.
Definition at line 135 of file ekf_aw.cpp.
Definition at line 19 of file ekf_aw.cpp.
Definition at line 30 of file ekf_aw.cpp.
Definition at line 40 of file ekf_aw.cpp.
Enumerator | |
---|---|
EKF_AW_u_index | |
EKF_AW_v_index | |
EKF_AW_w_index | |
EKF_AW_mu_x_index | |
EKF_AW_mu_y_index | |
EKF_AW_mu_z_index | |
EKF_AW_k_x_index | |
EKF_AW_k_y_index | |
EKF_AW_k_z_index | |
EKF_AW_COV_SIZE |
Definition at line 12 of file ekf_aw.cpp.
Definition at line 22 of file ekf_aw.cpp.
Enumerator | |
---|---|
EKF_AW_R_V_gnd_x_index | |
EKF_AW_R_V_gnd_y_index | |
EKF_AW_R_V_gnd_z_index | |
EKF_AW_R_a_x_filt_index | |
EKF_AW_R_a_y_filt_index | |
EKF_AW_R_a_z_filt_index | |
EKF_AW_R_V_pitot_index | |
EKF_AW_R_SIZE |
Definition at line 33 of file ekf_aw.cpp.
Definition at line 1392 of file ekf_aw.cpp.
Referenced by ekf_aw_wrapper_periodic().
Definition at line 1374 of file ekf_aw.cpp.
Referenced by ekf_aw_wrapper_periodic().
Definition at line 1295 of file ekf_aw.cpp.
References eawp, and ekfHealth::healthy.
Referenced by ekf_aw_wrapper_periodic().
Definition at line 1401 of file ekf_aw.cpp.
Referenced by ekf_aw_wrapper_periodic().
struct FloatVect3 ekf_aw_get_innov_accel_filt | ( | void | ) |
Definition at line 1313 of file ekf_aw.cpp.
References eawp, and FloatVect3::x.
Referenced by ekf_aw_wrapper_periodic().
struct FloatVect3 ekf_aw_get_innov_V_gnd | ( | void | ) |
Definition at line 1304 of file ekf_aw.cpp.
References eawp, and FloatVect3::x.
Referenced by ekf_aw_wrapper_periodic().
Definition at line 1323 of file ekf_aw.cpp.
References eawp.
Referenced by ekf_aw_wrapper_periodic().
Definition at line 1329 of file ekf_aw.cpp.
References eawp, EKF_AW_R_SIZE, and foo.
Referenced by ekf_aw_wrapper_periodic().
Definition at line 1285 of file ekf_aw.cpp.
References eawp, and NedCoor_f::x.
Referenced by ekf_aw_wrapper_periodic().
struct ekfAwParameters * ekf_aw_get_param_handle | ( | void | ) |
Definition at line 1419 of file ekf_aw.cpp.
References ekf_aw_params.
Referenced by ekf_aw_wrapper_init().
Definition at line 1359 of file ekf_aw.cpp.
References eawp, EKF_AW_Q_SIZE, and foo.
Referenced by ekf_aw_wrapper_periodic().
Definition at line 1410 of file ekf_aw.cpp.
Referenced by ekf_aw_wrapper_periodic().
Definition at line 1265 of file ekf_aw.cpp.
References eawp, s, and NedCoor_f::x.
Referenced by ekf_aw_wrapper_periodic().
Definition at line 1344 of file ekf_aw.cpp.
References eawp, EKF_AW_COV_SIZE, and foo.
Referenced by ekf_aw_wrapper_periodic().
Definition at line 1275 of file ekf_aw.cpp.
References eawp, and NedCoor_f::x.
Referenced by ekf_aw_wrapper_periodic().
Definition at line 1383 of file ekf_aw.cpp.
Referenced by ekf_aw_wrapper_periodic().
< accel process noise
< gyro process noise
< wind process noise
< offset process noise
< speed measurement noise
< filtered accel measurement noise
< airspeed measurement noise
Definition at line 457 of file ekf_aw.cpp.
References eawp, EKF_AW_K1_FX_DRAG, EKF_AW_K1_FX_ELEV, EKF_AW_K1_FX_FUSELAGE, EKF_AW_K1_FX_HOVER, EKF_AW_K1_FX_PUSH, EKF_AW_K1_FX_WING, EKF_AW_K1_FY_WING, EKF_AW_K1_FZ_ELEV, EKF_AW_K1_FZ_FUSELAGE, EKF_AW_K1_FZ_HOVER, EKF_AW_K1_FZ_WING, EKF_AW_K2_FX_DRAG, EKF_AW_K2_FX_ELEV, EKF_AW_K2_FX_FUSELAGE, EKF_AW_K2_FX_HOVER, EKF_AW_K2_FX_PUSH, EKF_AW_K2_FX_WING, EKF_AW_K2_FY_WING, EKF_AW_K2_FZ_ELEV, EKF_AW_K2_FZ_FUSELAGE, EKF_AW_K2_FZ_HOVER, EKF_AW_K2_FZ_WING, EKF_AW_K3_FX_ELEV, EKF_AW_K3_FX_FUSELAGE, EKF_AW_K3_FX_HOVER, EKF_AW_K3_FX_PUSH, EKF_AW_K3_FX_WING, EKF_AW_K3_FY_WING, EKF_AW_K3_FZ_FUSELAGE, EKF_AW_K3_FZ_HOVER, EKF_AW_K3_FZ_WING, EKF_AW_K4_FX_FUSELAGE, EKF_AW_K4_FX_WING, EKF_AW_K4_FY_WING, EKF_AW_K4_FZ_FUSELAGE, EKF_AW_K4_FZ_HOVER, EKF_AW_K4_FZ_WING, EKF_AW_K5_FX_WING, EKF_AW_K5_FY_WING, EKF_AW_K5_FZ_HOVER, EKF_AW_K_FY_BETA, EKF_AW_K_FY_V, ekf_aw_params, EKF_AW_PROPAGATE_OFFSET, EKF_AW_Q_ACCEL, EKF_AW_Q_GYRO, EKF_AW_Q_MU, EKF_AW_Q_OFFSET, EKF_AW_R_ACCEL_FILT_X, EKF_AW_R_ACCEL_FILT_Y, EKF_AW_R_ACCEL_FILT_Z, EKF_AW_R_V_GND, EKF_AW_R_V_PITOT, EKF_AW_USE_MODEL_BASED_X, EKF_AW_USE_MODEL_BASED_Y, EKF_AW_USE_MODEL_BASED_Z, EKF_AW_USE_PITOT, EKF_AW_VEHICLE_MASS, init_ekf_aw_state(), ekfAwParameters::k_fx_drag, ekfAwParameters::k_fx_elev, ekfAwParameters::k_fx_fuselage, ekfAwParameters::k_fx_hover, ekfAwParameters::k_fx_push, ekfAwParameters::k_fx_wing, ekfAwParameters::k_fy_beta, ekfAwParameters::k_fy_v, ekfAwParameters::k_fy_wing, ekfAwParameters::k_fz_elev, ekfAwParameters::k_fz_fuselage, ekfAwParameters::k_fz_hover, ekfAwParameters::k_fz_wing, ekfAwParameters::propagate_offset, ekfAwParameters::Q_accel, ekfAwParameters::Q_gyro, ekfAwParameters::Q_k, ekfAwParameters::Q_mu, ekfAwParameters::quick_convergence, ekfAwParameters::R_accel_filt, ekfAwParameters::R_V_gnd, ekfAwParameters::R_V_pitot, ekfAwParameters::use_model, ekfAwParameters::use_pitot, and ekfAwParameters::vehicle_mass.
Referenced by ekf_aw_wrapper_init().
void ekf_aw_propagate | ( | struct FloatVect3 * | acc, |
struct FloatRates * | gyro, | ||
struct FloatEulers * | euler, | ||
float * | pusher_RPM, | ||
float * | hover_RPM_array, | ||
float * | skew, | ||
float * | elevator_angle, | ||
FloatVect3 * | V_gnd, | ||
FloatVect3 * | acc_filt, | ||
float * | V_pitot, | ||
float | dt | ||
) |
Definition at line 591 of file ekf_aw.cpp.
References deg2rad, E, eawp, EKF_AW_AOA_MAX_ANGLE, EKF_AW_AOA_MIN_ANGLE, EKF_AW_AX_SCHED_END_DEG, EKF_AW_AX_SCHED_GAIN, EKF_AW_AX_SCHED_START_DEG, EKF_AW_AZ_QUICK_CONV_ACCEL_GAIN, EKF_AW_AZ_QUICK_CONV_MU_GAIN, EKF_AW_AZ_SCHED_END_DEG, EKF_AW_AZ_SCHED_GAIN, EKF_AW_AZ_SCHED_START_DEG, EKF_AW_COV_SIZE, EKF_AW_DEBUG, EKF_AW_ELEV_MAX_ANGLE, EKF_AW_ELEV_MIN_ANGLE, ekf_aw_params, EKF_AW_Q_mu_x_index, EKF_AW_Q_mu_y_index, EKF_AW_R_a_x_filt_index, EKF_AW_R_a_y_filt_index, EKF_AW_R_a_z_filt_index, EKF_AW_R_SIZE, EKF_AW_SKEW_POLY_0, EKF_AW_SKEW_POLY_1, EKF_AW_SKEW_POLY_2, EKF_AW_WING_INSTALLED, foo, fx_elevator(), fx_fuselage(), fx_fy_hover(), fx_pusher(), fx_wing(), fy_wing(), fz_elevator(), fz_fuselage(), fz_hover(), fz_wing(), get_sys_time_usec(), gravity(), K, ekfAwParameters::k_fx_drag, ekfAwParameters::k_fx_elev, ekfAwParameters::k_fx_fuselage, ekfAwParameters::k_fx_hover, ekfAwParameters::k_fx_push, ekfAwParameters::k_fx_wing, ekfAwParameters::k_fy_beta, ekfAwParameters::k_fy_v, ekfAwParameters::k_fy_wing, ekfAwParameters::k_fz_elev, ekfAwParameters::k_fz_fuselage, ekfAwParameters::k_fz_hover, ekfAwParameters::k_fz_wing, FloatRates::p, p, FloatEulers::phi, ekfAwParameters::propagate_offset, FloatEulers::psi, FloatRates::q, ekfAwParameters::Q_mu, ekfAwParameters::quick_convergence, FloatRates::r, ekfAwParameters::R_accel_filt, FloatEulers::theta, ekfAwParameters::use_model, ekfAwParameters::use_pitot, ekfAwParameters::vehicle_mass, FloatVect3::x, FloatVect3::y, and FloatVect3::z.
Referenced by ekf_aw_wrapper_periodic().
Definition at line 584 of file ekf_aw.cpp.
References init_ekf_aw_state().
Definition at line 1447 of file ekf_aw.cpp.
References eawp.
Definition at line 1440 of file ekf_aw.cpp.
Definition at line 1426 of file ekf_aw.cpp.
Referenced by ekf_aw_wrapper_periodic().
Definition at line 1433 of file ekf_aw.cpp.
Definition at line 552 of file ekf_aw.cpp.
References eawp, ekf_aw_params, EKF_AW_Q_accel_x_index, EKF_AW_Q_accel_y_index, EKF_AW_Q_accel_z_index, EKF_AW_Q_gyro_x_index, EKF_AW_Q_gyro_y_index, EKF_AW_Q_gyro_z_index, EKF_AW_Q_k_x_index, EKF_AW_Q_k_y_index, EKF_AW_Q_k_z_index, EKF_AW_Q_mu_x_index, EKF_AW_Q_mu_y_index, EKF_AW_Q_mu_z_index, EKF_AW_R_a_x_filt_index, EKF_AW_R_a_y_filt_index, EKF_AW_R_a_z_filt_index, EKF_AW_R_V_gnd_x_index, EKF_AW_R_V_gnd_y_index, EKF_AW_R_V_gnd_z_index, EKF_AW_R_V_pitot_index, ekfAwParameters::Q_accel, ekfAwParameters::Q_gyro, ekfAwParameters::Q_k, ekfAwParameters::Q_mu, ekfAwParameters::R_accel_filt, ekfAwParameters::R_V_gnd, and ekfAwParameters::R_V_pitot.
Referenced by init_ekf_aw_state().
Definition at line 1469 of file ekf_aw.cpp.
References ekf_aw_params, foo, and ekfAwParameters::k_fx_elev.
Referenced by ekf_aw_propagate().
Definition at line 1456 of file ekf_aw.cpp.
References ekf_aw_params, foo, ekfAwParameters::k_fx_fuselage, and sign().
Referenced by ekf_aw_propagate().
Definition at line 1502 of file ekf_aw.cpp.
References ekf_aw_params, foo, ekfAwParameters::k_fx_hover, and sign().
Referenced by ekf_aw_propagate().
Definition at line 1516 of file ekf_aw.cpp.
References ekf_aw_params, and ekfAwParameters::k_fx_push.
Referenced by ekf_aw_propagate().
Definition at line 1480 of file ekf_aw.cpp.
References ekf_aw_params, foo, ekfAwParameters::k_fx_wing, and sign().
Referenced by ekf_aw_propagate().
Definition at line 1537 of file ekf_aw.cpp.
References ekf_aw_params, foo, ekfAwParameters::k_fy_wing, and sign().
Referenced by ekf_aw_propagate().
Definition at line 1562 of file ekf_aw.cpp.
References ekf_aw_params, foo, and ekfAwParameters::k_fz_elev.
Referenced by ekf_aw_propagate().
Definition at line 1550 of file ekf_aw.cpp.
References ekf_aw_params, foo, and ekfAwParameters::k_fz_fuselage.
Referenced by ekf_aw_propagate().
float fz_hover | ( | matrix::Vector< float, 4 > | RPM_hover, |
float * | V_a | ||
) |
Definition at line 1592 of file ekf_aw.cpp.
References ekf_aw_params, foo, and ekfAwParameters::k_fz_hover.
Referenced by ekf_aw_propagate().
Definition at line 1572 of file ekf_aw.cpp.
References ekf_aw_params, foo, ekfAwParameters::k_fz_wing, and sign().
Referenced by ekf_aw_propagate().
|
static |
Definition at line 404 of file ekf_aw.cpp.
References eawp, EKF_AW_k_x_index, EKF_AW_k_y_index, EKF_AW_k_z_index, EKF_AW_mu_x_index, EKF_AW_mu_y_index, EKF_AW_mu_z_index, EKF_AW_P0_MU, EKF_AW_P0_OFFSET, EKF_AW_P0_V_BODY, EKF_AW_u_index, ekf_aw_update_params(), EKF_AW_v_index, and EKF_AW_w_index.
Referenced by ekf_aw_init(), and ekf_aw_reset().
Definition at line 388 of file ekf_aw.cpp.
Referenced by ekf_aw_propagate().
struct ekfAwParameters ekf_aw_params |
Definition at line 377 of file ekf_aw.cpp.
Referenced by ekf_aw_get_param_handle(), ekf_aw_init(), ekf_aw_propagate(), ekf_aw_update_params(), fx_elevator(), fx_fuselage(), fx_fy_hover(), fx_pusher(), fx_wing(), fy_wing(), fz_elevator(), fz_fuselage(), fz_hover(), and fz_wing().
|
static |
Definition at line 380 of file ekf_aw.cpp.
Definition at line 389 of file ekf_aw.cpp.