|
Paparazzi UAS v7.1_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 |
Functions | |
| static const matrix::Vector3f | gravity (0.f, 0.f, 9.81f) |
| float | fx_fuselage (float *skew, float *aoa, float *u) |
| float | fx_elevator (float *elevator_angle, float *V_a) |
| float | fx_wing (float *skew, float *aoa, float *u) |
| float | fy_wing (float *skew, float *aoa, float *u) |
| float | fx_fy_hover (float *RPM_hover_mean, float *V) |
| float | fx_pusher (float *RPM_pusher, float *u) |
| float | fz_fuselage (float *skew, float *aoa, float *V_a) |
| float | fz_elevator (float *elevator_angle, float *V_a) |
| float | fz_wing (float *skew, float *aoa, float *V_a) |
| float | fz_hover (matrix::Vector< float, 4 > RPM_hover, float *V_a) |
| static void | init_ekf_aw_state (void) |
| void | ekf_aw_init (void) |
| void | ekf_aw_update_params (void) |
| void | ekf_aw_reset (void) |
| 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) |
| struct NedCoor_f | ekf_aw_get_speed_body (void) |
| struct NedCoor_f | ekf_aw_get_wind_ned (void) |
| struct NedCoor_f | ekf_aw_get_offset (void) |
| struct ekfHealth | ekf_aw_get_health (void) |
| struct FloatVect3 | ekf_aw_get_innov_V_gnd (void) |
| struct FloatVect3 | ekf_aw_get_innov_accel_filt (void) |
| float | ekf_aw_get_innov_V_pitot (void) |
| void | ekf_aw_get_meas_cov (float meas_cov[7]) |
| void | ekf_aw_get_state_cov (float state_cov[9]) |
| void | ekf_aw_get_process_cov (float process_cov[12]) |
| void | ekf_aw_get_fuselage_force (float force[3]) |
| void | ekf_aw_get_wing_force (float force[3]) |
| void | ekf_aw_get_elevator_force (float force[3]) |
| void | ekf_aw_get_hover_force (float force[3]) |
| void | ekf_aw_get_pusher_force (float force[3]) |
| struct ekfAwParameters * | ekf_aw_get_param_handle (void) |
| void | ekf_aw_set_speed_body (struct NedCoor_f *s) |
| void | ekf_aw_set_wind (struct NedCoor_f *s) |
| void | ekf_aw_set_offset (struct NedCoor_f *s) |
| void | ekf_aw_reset_health (void) |
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 |
Definition at line 50 of file ekf_aw.cpp.
| Data Fields | ||
|---|---|---|
| Vector3f | accel | |
| float | elevator_angle | |
| Vector3f | euler | |
| Vector3f | rates | |
| Vector< float, 4 > | RPM_hover | |
| float | RPM_pusher | |
| float | skew | |
| struct ekfAwMeasurements |
Definition at line 61 of file ekf_aw.cpp.
| Data Fields | ||
|---|---|---|
| Vector3f | accel_filt | |
| Vector3f | V_gnd | |
| float | V_pitot | |
| struct ekfAwForces |
Definition at line 68 of file ekf_aw.cpp.
| Data Fields | ||
|---|---|---|
| Vector3f | elevator | |
| Vector3f | fuselage | |
| Vector3f | hover | |
| Vector3f | pusher | |
| Vector3f | wing | |
| 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.
Referenced by ekf_aw_get_elevator_force(), ekf_aw_get_fuselage_force(), ekf_aw_get_health(), ekf_aw_get_hover_force(), ekf_aw_get_innov_accel_filt(), ekf_aw_get_innov_V_gnd(), ekf_aw_get_innov_V_pitot(), ekf_aw_get_meas_cov(), ekf_aw_get_offset(), ekf_aw_get_process_cov(), ekf_aw_get_pusher_force(), ekf_aw_get_speed_body(), ekf_aw_get_state_cov(), ekf_aw_get_wind_ned(), ekf_aw_get_wing_force(), ekf_aw_init(), ekf_aw_propagate(), ekf_aw_reset_health(), ekf_aw_set_offset(), ekf_aw_set_speed_body(), ekf_aw_set_wind(), ekf_aw_update_params(), and init_ekf_aw_state().
| #define EKF_AW_AOA_MAX_ANGLE 15.0f |
Definition at line 337 of file ekf_aw.cpp.
Referenced by ekf_aw_propagate().
| #define EKF_AW_AOA_MIN_ANGLE -15.0f |
Definition at line 340 of file ekf_aw.cpp.
Referenced by ekf_aw_propagate().
| #define EKF_AW_AX_SCHED_END_DEG 60 |
Definition at line 318 of file ekf_aw.cpp.
Referenced by ekf_aw_propagate().
| #define EKF_AW_AX_SCHED_GAIN 0 |
Definition at line 312 of file ekf_aw.cpp.
Referenced by ekf_aw_propagate().
| #define EKF_AW_AX_SCHED_START_DEG 40 |
Definition at line 315 of file ekf_aw.cpp.
Referenced by ekf_aw_propagate().
| #define EKF_AW_AZ_QUICK_CONV_ACCEL_GAIN 0 |
Definition at line 326 of file ekf_aw.cpp.
Referenced by ekf_aw_propagate().
| #define EKF_AW_AZ_QUICK_CONV_MU_GAIN -2 |
Definition at line 323 of file ekf_aw.cpp.
Referenced by ekf_aw_propagate().
| #define EKF_AW_AZ_SCHED_END_DEG 70 |
Definition at line 309 of file ekf_aw.cpp.
Referenced by ekf_aw_propagate().
| #define EKF_AW_AZ_SCHED_GAIN 0 |
Definition at line 303 of file ekf_aw.cpp.
Referenced by ekf_aw_propagate().
| #define EKF_AW_AZ_SCHED_START_DEG 60 |
Definition at line 306 of file ekf_aw.cpp.
Referenced by ekf_aw_propagate().
| #define EKF_AW_DEBUG false |
Definition at line 356 of file ekf_aw.cpp.
Referenced by ekf_aw_propagate().
| #define EKF_AW_ELEV_MAX_ANGLE 37.0f |
Definition at line 331 of file ekf_aw.cpp.
Referenced by ekf_aw_propagate().
| #define EKF_AW_ELEV_MIN_ANGLE -10.0f |
Definition at line 334 of file ekf_aw.cpp.
Referenced by ekf_aw_propagate().
| #define EKF_AW_K1_FX_DRAG -3.0E-1f |
Definition at line 163 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K1_FX_ELEV 0.0f |
Definition at line 219 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K1_FX_FUSELAGE 0.0f |
Definition at line 170 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K1_FX_HOVER 0.0f |
Definition at line 183 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
Definition at line 209 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K1_FX_WING -6.428638953316000e-03f |
Definition at line 193 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K1_FY_WING 0.0f |
Definition at line 236 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K1_FZ_ELEV 0.0f |
Definition at line 295 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K1_FZ_FUSELAGE 0.0f |
Definition at line 253 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K1_FZ_HOVER -8.738705811080210e-07f |
Definition at line 279 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K1_FZ_WING -1.000778727574050e-01f |
Definition at line 266 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K2_FX_DRAG -4.0E-2f |
Definition at line 166 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K2_FX_ELEV 0.0f |
Definition at line 222 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K2_FX_FUSELAGE -0.04f |
Definition at line 173 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K2_FX_HOVER 0.0f |
Definition at line 186 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
Definition at line 212 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K2_FX_WING 1.671952644901720e-01f |
Definition at line 196 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K2_FY_WING 0.0f |
Definition at line 239 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K2_FZ_ELEV 0.0f |
Definition at line 298 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K2_FZ_FUSELAGE 0.0f |
Definition at line 256 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K2_FZ_HOVER -9.517409386179890e-07f |
Definition at line 282 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K2_FZ_WING -8.696479964371250e-01f |
Definition at line 269 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K3_FX_ELEV 0.0f |
Definition at line 225 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K3_FX_FUSELAGE 0.0f |
Definition at line 176 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K3_FX_HOVER -0.3f |
Definition at line 189 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
Definition at line 215 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K3_FX_WING 5.944103706458780e-01f |
Definition at line 199 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K3_FY_WING 0.0f |
Definition at line 242 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K3_FZ_FUSELAGE 0.0f |
Definition at line 259 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K3_FZ_HOVER -8.946217883362630e-07f |
Definition at line 285 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K3_FZ_WING 1.457831456377660e-01f |
Definition at line 272 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K4_FX_FUSELAGE 0.0f |
Definition at line 179 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K4_FX_WING 3.983889380919000e-03f |
Definition at line 202 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K4_FY_WING 0.0f |
Definition at line 245 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K4_FZ_FUSELAGE 0.0f |
Definition at line 262 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K4_FZ_HOVER -8.520556416144729e-07f |
Definition at line 288 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K4_FZ_WING 2.185394878246410e-01f |
Definition at line 275 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K5_FX_WING 3.532085496834000e-03f |
Definition at line 205 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K5_FY_WING 0.0f |
Definition at line 248 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_K5_FZ_HOVER 0.0f |
Definition at line 291 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
Definition at line 230 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
Definition at line 233 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_P0_MU EKF_AW_Q_MU*1.E1f |
Definition at line 110 of file ekf_aw.cpp.
Referenced by init_ekf_aw_state().
| #define EKF_AW_P0_OFFSET EKF_AW_Q_OFFSET |
Definition at line 113 of file ekf_aw.cpp.
Referenced by init_ekf_aw_state().
Definition at line 107 of file ekf_aw.cpp.
Referenced by init_ekf_aw_state().
| #define EKF_AW_PROPAGATE_OFFSET false |
Definition at line 150 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_Q_ACCEL 1.0E-4f |
Definition at line 93 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_Q_GYRO 1.0E-09f |
Definition at line 96 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_Q_MU 1.0E-6f |
Definition at line 99 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
Definition at line 102 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
Definition at line 121 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
Definition at line 124 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
Definition at line 127 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
Definition at line 118 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
Definition at line 130 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_SKEW_POLY_0 0.0f |
Definition at line 345 of file ekf_aw.cpp.
Referenced by ekf_aw_propagate().
| #define EKF_AW_SKEW_POLY_1 1.0f |
Definition at line 348 of file ekf_aw.cpp.
Referenced by ekf_aw_propagate().
| #define EKF_AW_SKEW_POLY_2 0.0f |
Definition at line 351 of file ekf_aw.cpp.
Referenced by ekf_aw_propagate().
| #define EKF_AW_USE_BETA false |
Definition at line 147 of file ekf_aw.cpp.
| #define EKF_AW_USE_MODEL_BASED_X false |
Definition at line 138 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_USE_MODEL_BASED_Y false |
Definition at line 141 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_USE_MODEL_BASED_Z false |
Definition at line 144 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_USE_PITOT false |
Definition at line 153 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_VEHICLE_MASS 6.5f |
Definition at line 158 of file ekf_aw.cpp.
Referenced by ekf_aw_init().
| #define EKF_AW_WING_INSTALLED false |
Definition at line 135 of file ekf_aw.cpp.
Referenced by ekf_aw_propagate().
| typedef matrix::SquareMatrix<float, EKF_AW_COV_SIZE> EKF_Aw_Cov |
Definition at line 19 of file ekf_aw.cpp.
| typedef matrix::SquareMatrix<float, EKF_AW_Q_SIZE> EKF_Aw_Q |
Definition at line 30 of file ekf_aw.cpp.
| typedef matrix::SquareMatrix<float, EKF_AW_R_SIZE> EKF_Aw_R |
Definition at line 40 of file ekf_aw.cpp.
| enum ekfAwCovVar |
| 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.
| enum ekfAwQVar |
Definition at line 22 of file ekf_aw.cpp.
| enum ekfAwRVar |
| 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.
| void ekf_aw_get_elevator_force | ( | float | force[3] | ) |
Definition at line 1392 of file ekf_aw.cpp.
References eawp.
Referenced by ekf_aw_wrapper_periodic().
| void ekf_aw_get_fuselage_force | ( | float | force[3] | ) |
Definition at line 1374 of file ekf_aw.cpp.
References eawp.
Referenced by ekf_aw_wrapper_periodic().
| struct ekfHealth ekf_aw_get_health | ( | void | ) |
Definition at line 1295 of file ekf_aw.cpp.
References eawp, and ekf_aw_get_health().
Referenced by ekf_aw_get_health(), ekf_aw_get_process_cov(), and ekf_aw_wrapper_periodic().
| void ekf_aw_get_hover_force | ( | float | force[3] | ) |
Definition at line 1401 of file ekf_aw.cpp.
References eawp.
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 ekf_aw_get_innov_accel_filt().
Referenced by ekf_aw_get_innov_accel_filt(), and 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 ekf_aw_get_innov_V_gnd().
Referenced by ekf_aw_get_innov_V_gnd(), and ekf_aw_wrapper_periodic().
| float ekf_aw_get_innov_V_pitot | ( | void | ) |
Definition at line 1323 of file ekf_aw.cpp.
References eawp.
Referenced by ekf_aw_wrapper_periodic().
| void ekf_aw_get_meas_cov | ( | float | meas_cov[7] | ) |
Definition at line 1329 of file ekf_aw.cpp.
References eawp, and EKF_AW_R_SIZE.
Referenced by ekf_aw_wrapper_periodic().
| struct NedCoor_f ekf_aw_get_offset | ( | void | ) |
Definition at line 1285 of file ekf_aw.cpp.
References eawp, and ekf_aw_get_offset().
Referenced by ekf_aw_get_offset(), and 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().
| void ekf_aw_get_process_cov | ( | float | process_cov[12] | ) |
Definition at line 1359 of file ekf_aw.cpp.
References eawp, and EKF_AW_Q_SIZE.
Referenced by ekf_aw_wrapper_periodic().
| void ekf_aw_get_pusher_force | ( | float | force[3] | ) |
Definition at line 1410 of file ekf_aw.cpp.
References eawp.
Referenced by ekf_aw_wrapper_periodic().
| struct NedCoor_f ekf_aw_get_speed_body | ( | void | ) |
Definition at line 1265 of file ekf_aw.cpp.
References eawp, ekf_aw_get_speed_body(), and s.
Referenced by ekf_aw_get_speed_body(), and ekf_aw_wrapper_periodic().
| void ekf_aw_get_state_cov | ( | float | state_cov[9] | ) |
Definition at line 1344 of file ekf_aw.cpp.
References eawp, and EKF_AW_COV_SIZE.
Referenced by ekf_aw_wrapper_periodic().
| struct NedCoor_f ekf_aw_get_wind_ned | ( | void | ) |
Definition at line 1275 of file ekf_aw.cpp.
References eawp, and ekf_aw_get_wind_ned().
Referenced by ekf_aw_get_wind_ned(), and ekf_aw_wrapper_periodic().
| void ekf_aw_get_wing_force | ( | float | force[3] | ) |
Definition at line 1383 of file ekf_aw.cpp.
References eawp.
Referenced by ekf_aw_wrapper_periodic().
| void ekf_aw_init | ( | void | ) |
< 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, and init_ekf_aw_state().
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, 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, FloatRates::p, p, FloatEulers::phi, FloatEulers::psi, FloatRates::q, FloatRates::r, FloatEulers::theta, FloatVect3::x, FloatVect3::y, and FloatVect3::z.
Referenced by ekf_aw_wrapper_periodic().
| void ekf_aw_reset | ( | void | ) |
Definition at line 584 of file ekf_aw.cpp.
References init_ekf_aw_state().
| void ekf_aw_reset_health | ( | void | ) |
Definition at line 1447 of file ekf_aw.cpp.
References eawp.
| void ekf_aw_set_offset | ( | struct NedCoor_f * | s | ) |
Definition at line 1440 of file ekf_aw.cpp.
| void ekf_aw_set_speed_body | ( | struct NedCoor_f * | s | ) |
Definition at line 1426 of file ekf_aw.cpp.
Referenced by ekf_aw_wrapper_periodic().
| void ekf_aw_set_wind | ( | struct NedCoor_f * | s | ) |
Definition at line 1433 of file ekf_aw.cpp.
| void ekf_aw_update_params | ( | void | ) |
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, and EKF_AW_R_V_pitot_index.
Referenced by init_ekf_aw_state().
| float fx_elevator | ( | float * | elevator_angle, |
| float * | V_a ) |
Definition at line 1469 of file ekf_aw.cpp.
References ekf_aw_params.
Referenced by ekf_aw_propagate().
| float fx_fuselage | ( | float * | skew, |
| float * | aoa, | ||
| float * | u ) |
Definition at line 1456 of file ekf_aw.cpp.
References ekf_aw_params, and sign().
Referenced by ekf_aw_propagate().
| float fx_fy_hover | ( | float * | RPM_hover_mean, |
| float * | V ) |
Definition at line 1502 of file ekf_aw.cpp.
References ekf_aw_params, and sign().
Referenced by ekf_aw_propagate().
| float fx_pusher | ( | float * | RPM_pusher, |
| float * | u ) |
Definition at line 1516 of file ekf_aw.cpp.
References ekf_aw_params.
Referenced by ekf_aw_propagate().
| float fx_wing | ( | float * | skew, |
| float * | aoa, | ||
| float * | u ) |
Definition at line 1480 of file ekf_aw.cpp.
References ekf_aw_params, and sign().
Referenced by ekf_aw_propagate().
| float fy_wing | ( | float * | skew, |
| float * | aoa, | ||
| float * | u ) |
Definition at line 1537 of file ekf_aw.cpp.
References ekf_aw_params, and sign().
Referenced by ekf_aw_propagate().
| float fz_elevator | ( | float * | elevator_angle, |
| float * | V_a ) |
Definition at line 1562 of file ekf_aw.cpp.
References ekf_aw_params.
Referenced by ekf_aw_propagate().
| float fz_fuselage | ( | float * | skew, |
| float * | aoa, | ||
| float * | V_a ) |
Definition at line 1550 of file ekf_aw.cpp.
References ekf_aw_params.
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.
Referenced by ekf_aw_propagate().
| float fz_wing | ( | float * | skew, |
| float * | aoa, | ||
| float * | V_a ) |
Definition at line 1572 of file ekf_aw.cpp.
References ekf_aw_params, and sign().
Referenced by ekf_aw_propagate().
|
static |
|
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().
| float deg2rad = M_PI / 180.0 |
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.
| float rad2deg = 180.0 / M_PI |
Definition at line 389 of file ekf_aw.cpp.