Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Data Structures | Macros | Typedefs | Enumerations | Functions | Variables
ekf_aw.cpp File Reference
#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>
+ Include dependency graph for ekf_aw.cpp:

Go to the source code of this file.

Data Structures

struct  ekfAwState
 
struct  ekfAwInputs
 
struct  ekfAwMeasurements
 
struct  ekfAwForces
 
struct  ekfAwPrivate
 

Macros

#define EKF_AW_Q_ACCEL   1.0E-4f
 
#define EKF_AW_Q_GYRO   1.0E-09f
 
#define EKF_AW_Q_MU   1.0E-6f
 
#define EKF_AW_Q_OFFSET   1.E-8f
 
#define EKF_AW_P0_V_BODY   1.E-2f
 
#define EKF_AW_P0_MU   EKF_AW_Q_MU*1.E1f
 
#define EKF_AW_P0_OFFSET   EKF_AW_Q_OFFSET
 
#define EKF_AW_R_V_GND   1.E-5f
 
#define EKF_AW_R_ACCEL_FILT_X   1.E-5f
 
#define EKF_AW_R_ACCEL_FILT_Y   1.E-5f
 
#define EKF_AW_R_ACCEL_FILT_Z   1.E-5f
 
#define EKF_AW_R_V_PITOT   1.E-7f
 
#define EKF_AW_WING_INSTALLED   false
 
#define EKF_AW_USE_MODEL_BASED_X   false
 
#define EKF_AW_USE_MODEL_BASED_Y   false
 
#define EKF_AW_USE_MODEL_BASED_Z   false
 
#define EKF_AW_USE_BETA   false
 
#define EKF_AW_PROPAGATE_OFFSET   false
 
#define EKF_AW_USE_PITOT   false
 
#define EKF_AW_VEHICLE_MASS   6.5f
 
#define EKF_AW_K1_FX_DRAG   -3.0E-1f
 
#define EKF_AW_K2_FX_DRAG   -4.0E-2f
 
#define EKF_AW_K1_FX_FUSELAGE   0.0f
 
#define EKF_AW_K2_FX_FUSELAGE   -0.04f
 
#define EKF_AW_K3_FX_FUSELAGE   0.0f
 
#define EKF_AW_K4_FX_FUSELAGE   0.0f
 
#define EKF_AW_K1_FX_HOVER   0.0f
 
#define EKF_AW_K2_FX_HOVER   0.0f
 
#define EKF_AW_K3_FX_HOVER   -0.3f
 
#define EKF_AW_K1_FX_WING   -6.428638953316000e-03f
 
#define EKF_AW_K2_FX_WING   1.671952644901720e-01f
 
#define EKF_AW_K3_FX_WING   5.944103706458780e-01f
 
#define EKF_AW_K4_FX_WING   3.983889380919000e-03f
 
#define EKF_AW_K5_FX_WING   3.532085496834000e-03f
 
#define EKF_AW_K1_FX_PUSH   3.96222948E-07f
 
#define EKF_AW_K2_FX_PUSH   -5.2930351318E-05f
 
#define EKF_AW_K3_FX_PUSH   -2.68843366027904E-01f
 
#define EKF_AW_K1_FX_ELEV   0.0f
 
#define EKF_AW_K2_FX_ELEV   0.0f
 
#define EKF_AW_K3_FX_ELEV   0.0f
 
#define EKF_AW_K_FY_BETA   -2.19E-1f
 
#define EKF_AW_K_FY_V   -3.2E-1f
 
#define EKF_AW_K1_FY_WING   0.0f
 
#define EKF_AW_K2_FY_WING   0.0f
 
#define EKF_AW_K3_FY_WING   0.0f
 
#define EKF_AW_K4_FY_WING   0.0f
 
#define EKF_AW_K5_FY_WING   0.0f
 
#define EKF_AW_K1_FZ_FUSELAGE   0.0f
 
#define EKF_AW_K2_FZ_FUSELAGE   0.0f
 
#define EKF_AW_K3_FZ_FUSELAGE   0.0f
 
#define EKF_AW_K4_FZ_FUSELAGE   0.0f
 
#define EKF_AW_K1_FZ_WING   -1.000778727574050e-01f
 
#define EKF_AW_K2_FZ_WING   -8.696479964371250e-01f
 
#define EKF_AW_K3_FZ_WING   1.457831456377660e-01f
 
#define EKF_AW_K4_FZ_WING   2.185394878246410e-01f
 
#define EKF_AW_K1_FZ_HOVER   -8.738705811080210e-07f
 
#define EKF_AW_K2_FZ_HOVER   -9.517409386179890e-07f
 
#define EKF_AW_K3_FZ_HOVER   -8.946217883362630e-07f
 
#define EKF_AW_K4_FZ_HOVER   -8.520556416144729e-07f
 
#define EKF_AW_K5_FZ_HOVER   0.0f
 
#define EKF_AW_K1_FZ_ELEV   0.0f
 
#define EKF_AW_K2_FZ_ELEV   0.0f
 
#define EKF_AW_AZ_SCHED_GAIN   0
 
#define EKF_AW_AZ_SCHED_START_DEG   60
 
#define EKF_AW_AZ_SCHED_END_DEG   70
 
#define EKF_AW_AX_SCHED_GAIN   0
 
#define EKF_AW_AX_SCHED_START_DEG   40
 
#define EKF_AW_AX_SCHED_END_DEG   60
 
#define EKF_AW_AZ_QUICK_CONV_MU_GAIN   -2
 
#define EKF_AW_AZ_QUICK_CONV_ACCEL_GAIN   0
 
#define EKF_AW_ELEV_MAX_ANGLE   37.0f
 
#define EKF_AW_ELEV_MIN_ANGLE   -10.0f
 
#define EKF_AW_AOA_MAX_ANGLE   15.0f
 
#define EKF_AW_AOA_MIN_ANGLE   -15.0f
 
#define EKF_AW_SKEW_POLY_0   0.0f
 
#define EKF_AW_SKEW_POLY_1   1.0f
 
#define EKF_AW_SKEW_POLY_2   0.0f
 
#define EKF_AW_DEBUG   false
 
#define eawp   ekf_aw_private
 

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
 

Enumerations

enum  ekfAwCovVar {
  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
}
 
enum  ekfAwQVar {
  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_mu_x_index , EKF_AW_Q_mu_y_index ,
  EKF_AW_Q_mu_z_index , EKF_AW_Q_k_x_index , EKF_AW_Q_k_y_index , EKF_AW_Q_k_z_index ,
  EKF_AW_Q_SIZE
}
 
enum  ekfAwRVar {
  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
}
 

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
 

Data Structure Documentation

◆ ekfAwState

struct ekfAwState

Definition at line 43 of file ekf_aw.cpp.

Data Fields
Vector3f offset
Vector3f V_body
Vector3f wind

◆ ekfAwInputs

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

◆ ekfAwMeasurements

struct ekfAwMeasurements

Definition at line 61 of file ekf_aw.cpp.

Data Fields
Vector3f accel_filt
Vector3f V_gnd
float V_pitot

◆ ekfAwForces

struct ekfAwForces

Definition at line 68 of file ekf_aw.cpp.

Data Fields
Vector3f elevator
Vector3f fuselage
Vector3f hover
Vector3f pusher
Vector3f wing

◆ ekfAwPrivate

struct ekfAwPrivate

Definition at line 77 of file ekf_aw.cpp.

+ Collaboration diagram for ekfAwPrivate:
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

Macro Definition Documentation

◆ eawp

#define eawp   ekf_aw_private

Definition at line 382 of file ekf_aw.cpp.

◆ EKF_AW_AOA_MAX_ANGLE

#define EKF_AW_AOA_MAX_ANGLE   15.0f

Definition at line 337 of file ekf_aw.cpp.

◆ EKF_AW_AOA_MIN_ANGLE

#define EKF_AW_AOA_MIN_ANGLE   -15.0f

Definition at line 340 of file ekf_aw.cpp.

◆ EKF_AW_AX_SCHED_END_DEG

#define EKF_AW_AX_SCHED_END_DEG   60

Definition at line 318 of file ekf_aw.cpp.

◆ EKF_AW_AX_SCHED_GAIN

#define EKF_AW_AX_SCHED_GAIN   0

Definition at line 312 of file ekf_aw.cpp.

◆ EKF_AW_AX_SCHED_START_DEG

#define EKF_AW_AX_SCHED_START_DEG   40

Definition at line 315 of file ekf_aw.cpp.

◆ EKF_AW_AZ_QUICK_CONV_ACCEL_GAIN

#define EKF_AW_AZ_QUICK_CONV_ACCEL_GAIN   0

Definition at line 326 of file ekf_aw.cpp.

◆ EKF_AW_AZ_QUICK_CONV_MU_GAIN

#define EKF_AW_AZ_QUICK_CONV_MU_GAIN   -2

Definition at line 323 of file ekf_aw.cpp.

◆ EKF_AW_AZ_SCHED_END_DEG

#define EKF_AW_AZ_SCHED_END_DEG   70

Definition at line 309 of file ekf_aw.cpp.

◆ EKF_AW_AZ_SCHED_GAIN

#define EKF_AW_AZ_SCHED_GAIN   0

Definition at line 303 of file ekf_aw.cpp.

◆ EKF_AW_AZ_SCHED_START_DEG

#define EKF_AW_AZ_SCHED_START_DEG   60

Definition at line 306 of file ekf_aw.cpp.

◆ EKF_AW_DEBUG

#define EKF_AW_DEBUG   false

Definition at line 356 of file ekf_aw.cpp.

◆ EKF_AW_ELEV_MAX_ANGLE

#define EKF_AW_ELEV_MAX_ANGLE   37.0f

Definition at line 331 of file ekf_aw.cpp.

◆ EKF_AW_ELEV_MIN_ANGLE

#define EKF_AW_ELEV_MIN_ANGLE   -10.0f

Definition at line 334 of file ekf_aw.cpp.

◆ EKF_AW_K1_FX_DRAG

#define EKF_AW_K1_FX_DRAG   -3.0E-1f

Definition at line 163 of file ekf_aw.cpp.

◆ EKF_AW_K1_FX_ELEV

#define EKF_AW_K1_FX_ELEV   0.0f

Definition at line 219 of file ekf_aw.cpp.

◆ EKF_AW_K1_FX_FUSELAGE

#define EKF_AW_K1_FX_FUSELAGE   0.0f

Definition at line 170 of file ekf_aw.cpp.

◆ EKF_AW_K1_FX_HOVER

#define EKF_AW_K1_FX_HOVER   0.0f

Definition at line 183 of file ekf_aw.cpp.

◆ EKF_AW_K1_FX_PUSH

#define EKF_AW_K1_FX_PUSH   3.96222948E-07f

Definition at line 209 of file ekf_aw.cpp.

◆ EKF_AW_K1_FX_WING

#define EKF_AW_K1_FX_WING   -6.428638953316000e-03f

Definition at line 193 of file ekf_aw.cpp.

◆ EKF_AW_K1_FY_WING

#define EKF_AW_K1_FY_WING   0.0f

Definition at line 236 of file ekf_aw.cpp.

◆ EKF_AW_K1_FZ_ELEV

#define EKF_AW_K1_FZ_ELEV   0.0f

Definition at line 295 of file ekf_aw.cpp.

◆ EKF_AW_K1_FZ_FUSELAGE

#define EKF_AW_K1_FZ_FUSELAGE   0.0f

Definition at line 253 of file ekf_aw.cpp.

◆ EKF_AW_K1_FZ_HOVER

#define EKF_AW_K1_FZ_HOVER   -8.738705811080210e-07f

Definition at line 279 of file ekf_aw.cpp.

◆ EKF_AW_K1_FZ_WING

#define EKF_AW_K1_FZ_WING   -1.000778727574050e-01f

Definition at line 266 of file ekf_aw.cpp.

◆ EKF_AW_K2_FX_DRAG

#define EKF_AW_K2_FX_DRAG   -4.0E-2f

Definition at line 166 of file ekf_aw.cpp.

◆ EKF_AW_K2_FX_ELEV

#define EKF_AW_K2_FX_ELEV   0.0f

Definition at line 222 of file ekf_aw.cpp.

◆ EKF_AW_K2_FX_FUSELAGE

#define EKF_AW_K2_FX_FUSELAGE   -0.04f

Definition at line 173 of file ekf_aw.cpp.

◆ EKF_AW_K2_FX_HOVER

#define EKF_AW_K2_FX_HOVER   0.0f

Definition at line 186 of file ekf_aw.cpp.

◆ EKF_AW_K2_FX_PUSH

#define EKF_AW_K2_FX_PUSH   -5.2930351318E-05f

Definition at line 212 of file ekf_aw.cpp.

◆ EKF_AW_K2_FX_WING

#define EKF_AW_K2_FX_WING   1.671952644901720e-01f

Definition at line 196 of file ekf_aw.cpp.

◆ EKF_AW_K2_FY_WING

#define EKF_AW_K2_FY_WING   0.0f

Definition at line 239 of file ekf_aw.cpp.

◆ EKF_AW_K2_FZ_ELEV

#define EKF_AW_K2_FZ_ELEV   0.0f

Definition at line 298 of file ekf_aw.cpp.

◆ EKF_AW_K2_FZ_FUSELAGE

#define EKF_AW_K2_FZ_FUSELAGE   0.0f

Definition at line 256 of file ekf_aw.cpp.

◆ EKF_AW_K2_FZ_HOVER

#define EKF_AW_K2_FZ_HOVER   -9.517409386179890e-07f

Definition at line 282 of file ekf_aw.cpp.

◆ EKF_AW_K2_FZ_WING

#define EKF_AW_K2_FZ_WING   -8.696479964371250e-01f

Definition at line 269 of file ekf_aw.cpp.

◆ EKF_AW_K3_FX_ELEV

#define EKF_AW_K3_FX_ELEV   0.0f

Definition at line 225 of file ekf_aw.cpp.

◆ EKF_AW_K3_FX_FUSELAGE

#define EKF_AW_K3_FX_FUSELAGE   0.0f

Definition at line 176 of file ekf_aw.cpp.

◆ EKF_AW_K3_FX_HOVER

#define EKF_AW_K3_FX_HOVER   -0.3f

Definition at line 189 of file ekf_aw.cpp.

◆ EKF_AW_K3_FX_PUSH

#define EKF_AW_K3_FX_PUSH   -2.68843366027904E-01f

Definition at line 215 of file ekf_aw.cpp.

◆ EKF_AW_K3_FX_WING

#define EKF_AW_K3_FX_WING   5.944103706458780e-01f

Definition at line 199 of file ekf_aw.cpp.

◆ EKF_AW_K3_FY_WING

#define EKF_AW_K3_FY_WING   0.0f

Definition at line 242 of file ekf_aw.cpp.

◆ EKF_AW_K3_FZ_FUSELAGE

#define EKF_AW_K3_FZ_FUSELAGE   0.0f

Definition at line 259 of file ekf_aw.cpp.

◆ EKF_AW_K3_FZ_HOVER

#define EKF_AW_K3_FZ_HOVER   -8.946217883362630e-07f

Definition at line 285 of file ekf_aw.cpp.

◆ EKF_AW_K3_FZ_WING

#define EKF_AW_K3_FZ_WING   1.457831456377660e-01f

Definition at line 272 of file ekf_aw.cpp.

◆ EKF_AW_K4_FX_FUSELAGE

#define EKF_AW_K4_FX_FUSELAGE   0.0f

Definition at line 179 of file ekf_aw.cpp.

◆ EKF_AW_K4_FX_WING

#define EKF_AW_K4_FX_WING   3.983889380919000e-03f

Definition at line 202 of file ekf_aw.cpp.

◆ EKF_AW_K4_FY_WING

#define EKF_AW_K4_FY_WING   0.0f

Definition at line 245 of file ekf_aw.cpp.

◆ EKF_AW_K4_FZ_FUSELAGE

#define EKF_AW_K4_FZ_FUSELAGE   0.0f

Definition at line 262 of file ekf_aw.cpp.

◆ EKF_AW_K4_FZ_HOVER

#define EKF_AW_K4_FZ_HOVER   -8.520556416144729e-07f

Definition at line 288 of file ekf_aw.cpp.

◆ EKF_AW_K4_FZ_WING

#define EKF_AW_K4_FZ_WING   2.185394878246410e-01f

Definition at line 275 of file ekf_aw.cpp.

◆ EKF_AW_K5_FX_WING

#define EKF_AW_K5_FX_WING   3.532085496834000e-03f

Definition at line 205 of file ekf_aw.cpp.

◆ EKF_AW_K5_FY_WING

#define EKF_AW_K5_FY_WING   0.0f

Definition at line 248 of file ekf_aw.cpp.

◆ EKF_AW_K5_FZ_HOVER

#define EKF_AW_K5_FZ_HOVER   0.0f

Definition at line 291 of file ekf_aw.cpp.

◆ EKF_AW_K_FY_BETA

#define EKF_AW_K_FY_BETA   -2.19E-1f

Definition at line 230 of file ekf_aw.cpp.

◆ EKF_AW_K_FY_V

#define EKF_AW_K_FY_V   -3.2E-1f

Definition at line 233 of file ekf_aw.cpp.

◆ EKF_AW_P0_MU

#define EKF_AW_P0_MU   EKF_AW_Q_MU*1.E1f

Definition at line 110 of file ekf_aw.cpp.

◆ EKF_AW_P0_OFFSET

#define EKF_AW_P0_OFFSET   EKF_AW_Q_OFFSET

Definition at line 113 of file ekf_aw.cpp.

◆ EKF_AW_P0_V_BODY

#define EKF_AW_P0_V_BODY   1.E-2f

Definition at line 107 of file ekf_aw.cpp.

◆ EKF_AW_PROPAGATE_OFFSET

#define EKF_AW_PROPAGATE_OFFSET   false

Definition at line 150 of file ekf_aw.cpp.

◆ EKF_AW_Q_ACCEL

#define EKF_AW_Q_ACCEL   1.0E-4f

Definition at line 93 of file ekf_aw.cpp.

◆ EKF_AW_Q_GYRO

#define EKF_AW_Q_GYRO   1.0E-09f

Definition at line 96 of file ekf_aw.cpp.

◆ EKF_AW_Q_MU

#define EKF_AW_Q_MU   1.0E-6f

Definition at line 99 of file ekf_aw.cpp.

◆ EKF_AW_Q_OFFSET

#define EKF_AW_Q_OFFSET   1.E-8f

Definition at line 102 of file ekf_aw.cpp.

◆ EKF_AW_R_ACCEL_FILT_X

#define EKF_AW_R_ACCEL_FILT_X   1.E-5f

Definition at line 121 of file ekf_aw.cpp.

◆ EKF_AW_R_ACCEL_FILT_Y

#define EKF_AW_R_ACCEL_FILT_Y   1.E-5f

Definition at line 124 of file ekf_aw.cpp.

◆ EKF_AW_R_ACCEL_FILT_Z

#define EKF_AW_R_ACCEL_FILT_Z   1.E-5f

Definition at line 127 of file ekf_aw.cpp.

◆ EKF_AW_R_V_GND

#define EKF_AW_R_V_GND   1.E-5f

Definition at line 118 of file ekf_aw.cpp.

◆ EKF_AW_R_V_PITOT

#define EKF_AW_R_V_PITOT   1.E-7f

Definition at line 130 of file ekf_aw.cpp.

◆ EKF_AW_SKEW_POLY_0

#define EKF_AW_SKEW_POLY_0   0.0f

Definition at line 345 of file ekf_aw.cpp.

◆ EKF_AW_SKEW_POLY_1

#define EKF_AW_SKEW_POLY_1   1.0f

Definition at line 348 of file ekf_aw.cpp.

◆ EKF_AW_SKEW_POLY_2

#define EKF_AW_SKEW_POLY_2   0.0f

Definition at line 351 of file ekf_aw.cpp.

◆ EKF_AW_USE_BETA

#define EKF_AW_USE_BETA   false

Definition at line 147 of file ekf_aw.cpp.

◆ EKF_AW_USE_MODEL_BASED_X

#define EKF_AW_USE_MODEL_BASED_X   false

Definition at line 138 of file ekf_aw.cpp.

◆ EKF_AW_USE_MODEL_BASED_Y

#define EKF_AW_USE_MODEL_BASED_Y   false

Definition at line 141 of file ekf_aw.cpp.

◆ EKF_AW_USE_MODEL_BASED_Z

#define EKF_AW_USE_MODEL_BASED_Z   false

Definition at line 144 of file ekf_aw.cpp.

◆ EKF_AW_USE_PITOT

#define EKF_AW_USE_PITOT   false

Definition at line 153 of file ekf_aw.cpp.

◆ EKF_AW_VEHICLE_MASS

#define EKF_AW_VEHICLE_MASS   6.5f

Definition at line 158 of file ekf_aw.cpp.

◆ EKF_AW_WING_INSTALLED

#define EKF_AW_WING_INSTALLED   false

Definition at line 135 of file ekf_aw.cpp.

Typedef Documentation

◆ EKF_Aw_Cov

typedef matrix::SquareMatrix<float, EKF_AW_COV_SIZE> EKF_Aw_Cov

Definition at line 19 of file ekf_aw.cpp.

◆ EKF_Aw_Q

typedef matrix::SquareMatrix<float, EKF_AW_Q_SIZE> EKF_Aw_Q

Definition at line 30 of file ekf_aw.cpp.

◆ EKF_Aw_R

typedef matrix::SquareMatrix<float, EKF_AW_R_SIZE> EKF_Aw_R

Definition at line 40 of file ekf_aw.cpp.

Enumeration Type Documentation

◆ ekfAwCovVar

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.

◆ ekfAwQVar

enum ekfAwQVar
Enumerator
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_mu_x_index 
EKF_AW_Q_mu_y_index 
EKF_AW_Q_mu_z_index 
EKF_AW_Q_k_x_index 
EKF_AW_Q_k_y_index 
EKF_AW_Q_k_z_index 
EKF_AW_Q_SIZE 

Definition at line 22 of file ekf_aw.cpp.

◆ ekfAwRVar

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.

Function Documentation

◆ ekf_aw_get_elevator_force()

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().

+ Here is the caller graph for this function:

◆ ekf_aw_get_fuselage_force()

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().

+ Here is the caller graph for this function:

◆ ekf_aw_get_health()

struct ekfHealth ekf_aw_get_health ( void  )

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_USE_BETA, 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, 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().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ ekf_aw_get_hover_force()

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().

+ Here is the caller graph for this function:

◆ ekf_aw_get_innov_accel_filt()

struct FloatVect3 ekf_aw_get_innov_accel_filt ( void  )

Definition at line 591 of file ekf_aw.cpp.

Referenced by ekf_aw_wrapper_periodic().

+ Here is the caller graph for this function:

◆ ekf_aw_get_innov_V_gnd()

struct FloatVect3 ekf_aw_get_innov_V_gnd ( void  )

Definition at line 591 of file ekf_aw.cpp.

Referenced by ekf_aw_wrapper_periodic().

+ Here is the caller graph for this function:

◆ ekf_aw_get_innov_V_pitot()

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().

+ Here is the caller graph for this function:

◆ ekf_aw_get_meas_cov()

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().

+ Here is the caller graph for this function:

◆ ekf_aw_get_offset()

struct NedCoor_f ekf_aw_get_offset ( void  )

Definition at line 591 of file ekf_aw.cpp.

Referenced by ekf_aw_wrapper_periodic().

+ Here is the caller graph for this function:

◆ ekf_aw_get_param_handle()

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().

+ Here is the caller graph for this function:

◆ ekf_aw_get_process_cov()

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().

+ Here is the caller graph for this function:

◆ ekf_aw_get_pusher_force()

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().

+ Here is the caller graph for this function:

◆ ekf_aw_get_speed_body()

struct NedCoor_f ekf_aw_get_speed_body ( void  )

Definition at line 591 of file ekf_aw.cpp.

Referenced by ekf_aw_wrapper_periodic().

+ Here is the caller graph for this function:

◆ ekf_aw_get_state_cov()

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().

+ Here is the caller graph for this function:

◆ ekf_aw_get_wind_ned()

struct NedCoor_f ekf_aw_get_wind_ned ( void  )

Definition at line 591 of file ekf_aw.cpp.

Referenced by ekf_aw_wrapper_periodic().

+ Here is the caller graph for this function:

◆ ekf_aw_get_wing_force()

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().

+ Here is the caller graph for this function:

◆ ekf_aw_init()

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, 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().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ ekf_aw_propagate()

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.

Referenced by ekf_aw_wrapper_periodic().

+ Here is the caller graph for this function:

◆ ekf_aw_reset()

void ekf_aw_reset ( void  )

Definition at line 584 of file ekf_aw.cpp.

References init_ekf_aw_state().

+ Here is the call graph for this function:

◆ ekf_aw_reset_health()

void ekf_aw_reset_health ( void  )

Definition at line 1447 of file ekf_aw.cpp.

References eawp.

◆ ekf_aw_set_offset()

void ekf_aw_set_offset ( struct NedCoor_f *  s)

Definition at line 1440 of file ekf_aw.cpp.

References eawp, and s.

◆ ekf_aw_set_speed_body()

void ekf_aw_set_speed_body ( struct NedCoor_f *  s)

Definition at line 1426 of file ekf_aw.cpp.

References eawp, and s.

Referenced by ekf_aw_wrapper_periodic().

+ Here is the caller graph for this function:

◆ ekf_aw_set_wind()

void ekf_aw_set_wind ( struct NedCoor_f *  s)

Definition at line 1433 of file ekf_aw.cpp.

References eawp, and s.

◆ ekf_aw_update_params()

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, 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().

+ Here is the caller graph for this function:

◆ fx_elevator()

float fx_elevator ( float *  elevator_angle,
float *  V_a 
)

Definition at line 1469 of file ekf_aw.cpp.

References ekf_aw_params, and ekfAwParameters::k_fx_elev.

Referenced by ekf_aw_get_health().

+ Here is the caller graph for this function:

◆ fx_fuselage()

float fx_fuselage ( float *  skew,
float *  aoa,
float *  u 
)

Definition at line 1456 of file ekf_aw.cpp.

References ekf_aw_params, ekfAwParameters::k_fx_fuselage, and sign().

Referenced by ekf_aw_get_health().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ fx_fy_hover()

float fx_fy_hover ( float *  RPM_hover_mean,
float *  V 
)

Definition at line 1502 of file ekf_aw.cpp.

References ekf_aw_params, ekfAwParameters::k_fx_hover, and sign().

Referenced by ekf_aw_get_health().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ fx_pusher()

float fx_pusher ( float *  RPM_pusher,
float *  u 
)

Definition at line 1516 of file ekf_aw.cpp.

References ekf_aw_params, and ekfAwParameters::k_fx_push.

Referenced by ekf_aw_get_health().

+ Here is the caller graph for this function:

◆ fx_wing()

float fx_wing ( float *  skew,
float *  aoa,
float *  u 
)

Definition at line 1480 of file ekf_aw.cpp.

References ekf_aw_params, ekfAwParameters::k_fx_wing, and sign().

Referenced by ekf_aw_get_health().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ fy_wing()

float fy_wing ( float *  skew,
float *  aoa,
float *  u 
)

Definition at line 1537 of file ekf_aw.cpp.

References ekf_aw_params, ekfAwParameters::k_fy_wing, and sign().

Referenced by ekf_aw_get_health().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ fz_elevator()

float fz_elevator ( float *  elevator_angle,
float *  V_a 
)

Definition at line 1562 of file ekf_aw.cpp.

References ekf_aw_params, and ekfAwParameters::k_fz_elev.

Referenced by ekf_aw_get_health().

+ Here is the caller graph for this function:

◆ fz_fuselage()

float fz_fuselage ( float *  skew,
float *  aoa,
float *  V_a 
)

Definition at line 1550 of file ekf_aw.cpp.

References ekf_aw_params, and ekfAwParameters::k_fz_fuselage.

Referenced by ekf_aw_get_health().

+ Here is the caller graph for this function:

◆ fz_hover()

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, and ekfAwParameters::k_fz_hover.

Referenced by ekf_aw_get_health().

+ Here is the caller graph for this function:

◆ fz_wing()

float fz_wing ( float *  skew,
float *  aoa,
float *  V_a 
)

Definition at line 1572 of file ekf_aw.cpp.

References ekf_aw_params, ekfAwParameters::k_fz_wing, and sign().

Referenced by ekf_aw_get_health().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ gravity()

static const matrix::Vector3f gravity ( 0.  f,
0.  f,
9.  81f 
)
static

Referenced by ekf_aw_get_health().

+ Here is the caller graph for this function:

◆ init_ekf_aw_state()

static void init_ekf_aw_state ( void  )
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().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

Variable Documentation

◆ deg2rad

float deg2rad = M_PI / 180.0

Definition at line 388 of file ekf_aw.cpp.

Referenced by ekf_aw_get_health().

◆ ekf_aw_params

struct ekfAwParameters ekf_aw_params

Definition at line 40 of file ekf_aw.cpp.

Referenced by ekf_aw_get_health(), ekf_aw_get_param_handle(), ekf_aw_init(), 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().

◆ ekf_aw_private

struct ekfAwPrivate ekf_aw_private
static

Definition at line 40 of file ekf_aw.cpp.

◆ rad2deg

float rad2deg = 180.0 / M_PI

Definition at line 389 of file ekf_aw.cpp.