|
Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
#include "modules/meteo/ekf_aw_wrapper.h"#include "modules/meteo/ekf_aw.h"#include <stdio.h>#include "state.h"#include "filters/low_pass_filter.h"#include "math/pprz_algebra.h"#include "modules/core/abi.h"#include "autopilot.h"#include "modules/actuators/actuators.h"#include "mcu_periph/sys_time.h"#include "modules/datalink/telemetry.h"Go to the source code of this file.
Macros | |
| #define | EKF_AW_WRAPPER_ROTWING false |
| #define | EKF_AW_WRAPPER_RANDOM_INPUTS false |
| #define | EKF_AW_QUICK_CONVERGENCE false |
| #define | EKF_AW_QUICK_CONVERGENCE_TIME 10.0f |
| #define | EKF_AW_DEBUG false |
| #define | PERIODIC_FREQUENCY_AIRSPEED_EKF_FETCH 100 |
| #define | PERIODIC_FREQUENCY_AIRSPEED_EKF 25 |
Functions | |
| static void | send_airspeed_wind_ekf (struct transport_tx *trans, struct link_device *dev) |
| static void | debug_vect (struct transport_tx *trans, struct link_device *dev, char *name, float *data, int datasize) |
| static void | send_airspeed_wind_ekf_debug (struct transport_tx *trans, struct link_device *dev) |
| static void | rpm_cb (uint8_t sender_id, struct act_feedback_t *rpm_message, uint8_t num_act) |
| void | ekf_aw_wrapper_init (void) |
| void | ekf_aw_wrapper_periodic (void) |
| void | ekf_aw_wrapper_fetch (void) |
| void | set_in_air_status (bool in_air) |
Variables | |
| abi_event | RPM_ev |
| float | time_of_rpm = 0.0f |
| struct ekfAw | ekf_aw |
| static struct ekfAwParameters * | ekf_params |
| The EKF parameters. | |
| float | tau_filter_high = 25.0f |
| float | tau_filter_low = 0.2f |
| bool | reset_filter = false |
| struct NedCoor_f | zero_speed |
| Butterworth2LowPass | filt_groundspeed [3] |
| Butterworth2LowPass | filt_acc [3] |
| Butterworth2LowPass | filt_acc_low [3] |
| Butterworth2LowPass | filt_rate [3] |
| Butterworth2LowPass | filt_euler [3] |
| Butterworth2LowPass | filt_hover_prop_rpm [EKF_AW_RPM_HOVER_NUM] |
| Butterworth2LowPass | filt_pusher_prop_rpm |
| Butterworth2LowPass | filt_skew |
| Butterworth2LowPass | filt_elevator_pprz |
| Butterworth2LowPass | filt_airspeed_pitot |
| #define EKF_AW_DEBUG false |
Definition at line 31 of file ekf_aw_wrapper.c.
| #define EKF_AW_QUICK_CONVERGENCE false |
Definition at line 25 of file ekf_aw_wrapper.c.
Referenced by ekf_aw_wrapper_periodic().
| #define EKF_AW_QUICK_CONVERGENCE_TIME 10.0f |
Definition at line 28 of file ekf_aw_wrapper.c.
Referenced by ekf_aw_wrapper_periodic().
| #define EKF_AW_WRAPPER_RANDOM_INPUTS false |
Definition at line 22 of file ekf_aw_wrapper.c.
Referenced by ekf_aw_wrapper_periodic().
| #define EKF_AW_WRAPPER_ROTWING false |
Definition at line 19 of file ekf_aw_wrapper.c.
| #define PERIODIC_FREQUENCY_AIRSPEED_EKF 25 |
Definition at line 133 of file ekf_aw_wrapper.c.
Referenced by ekf_aw_wrapper_periodic().
| #define PERIODIC_FREQUENCY_AIRSPEED_EKF_FETCH 100 |
Definition at line 129 of file ekf_aw_wrapper.c.
Referenced by ekf_aw_wrapper_init(), and ekf_aw_wrapper_periodic().
|
static |
Definition at line 63 of file ekf_aw_wrapper.c.
References dev.
Referenced by send_airspeed_wind_ekf_debug().
| void ekf_aw_wrapper_fetch | ( | void | ) |
Definition at line 338 of file ekf_aw_wrapper.c.
References ACCEL_BFP_OF_REAL, ACCELS_FLOAT_OF_BFP, actuators_pprz, body_accel_f, ekf_aw, EKF_AW_RPM_HOVER_NUM, filt_acc, filt_acc_low, filt_airspeed_pitot, filt_elevator_pprz, filt_euler, filt_groundspeed, filt_hover_prop_rpm, filt_pusher_prop_rpm, filt_rate, filt_skew, int32_rmat_vmult(), p, stateGetAccelBody_i(), stateGetAccelNed_i(), stateGetAirspeed_f(), stateGetBodyRates_f(), stateGetNedToBodyEulers_f(), stateGetNedToBodyRMat_i(), stateGetSpeedNed_f(), update_butterworth_2_low_pass(), VECT3_COPY, NedCoor_f::x, NedCoor_f::y, Int32Vect3::z, and NedCoor_f::z.
| void ekf_aw_wrapper_init | ( | void | ) |
Definition at line 136 of file ekf_aw_wrapper.c.
References ACT_FEEDBACK_RPM_SENSOR_ID, DefaultPeriodic, ekf_aw, ekf_aw_get_param_handle(), ekf_aw_init(), EKF_AW_RPM_HOVER_NUM, ekf_params, filt_acc, filt_acc_low, filt_airspeed_pitot, filt_elevator_pprz, filt_euler, filt_groundspeed, filt_hover_prop_rpm, filt_pusher_prop_rpm, filt_rate, filt_skew, init_butterworth_2_low_pass(), PERIODIC_FREQUENCY_AIRSPEED_EKF_FETCH, register_periodic_telemetry(), rpm_cb(), RPM_ev, send_airspeed_wind_ekf(), send_airspeed_wind_ekf_debug(), tau_filter_high, and tau_filter_low.
| void ekf_aw_wrapper_periodic | ( | void | ) |
Definition at line 215 of file ekf_aw_wrapper.c.
References autopilot_in_flight(), ekf_aw, 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_propagate(), EKF_AW_QUICK_CONVERGENCE, EKF_AW_QUICK_CONVERGENCE_TIME, EKF_AW_RPM_HOVER_NUM, ekf_aw_set_speed_body(), EKF_AW_WRAPPER_RANDOM_INPUTS, ekf_params, filt_acc, filt_acc_low, filt_airspeed_pitot, filt_elevator_pprz, filt_euler, filt_groundspeed, filt_hover_prop_rpm, filt_pusher_prop_rpm, filt_rate, filt_skew, PERIODIC_FREQUENCY_AIRSPEED_EKF, PERIODIC_FREQUENCY_AIRSPEED_EKF_FETCH, FloatEulers::psi, set_in_air_status(), stateGetNedToBodyEulers_f(), stateGetPositionNed_f(), NedCoor_f::z, and zero_speed.
|
static |
Definition at line 413 of file ekf_aw_wrapper.c.
References ekf_aw, EKF_AW_RPM_HOVER_NUM, get_sys_time_float(), idx, act_feedback_t::rpm, and time_of_rpm.
Referenced by ekf_aw_wrapper_init().
|
static |
Definition at line 45 of file ekf_aw_wrapper.c.
Referenced by ekf_aw_wrapper_init().
|
static |
Definition at line 72 of file ekf_aw_wrapper.c.
References debug_vect(), dev, and ekf_aw.
Referenced by ekf_aw_wrapper_init().
| void set_in_air_status | ( | bool | in_air | ) |
Definition at line 445 of file ekf_aw_wrapper.c.
References ekf_aw.
Referenced by ekf_aw_wrapper_periodic().
| struct ekfAw ekf_aw |
Definition at line 100 of file ekf_aw_wrapper.c.
Referenced by ekf_aw_wrapper_fetch(), ekf_aw_wrapper_init(), ekf_aw_wrapper_periodic(), rpm_cb(), send_airspeed_wind_ekf(), send_airspeed_wind_ekf_debug(), and set_in_air_status().
|
static |
The EKF parameters.
Definition at line 101 of file ekf_aw_wrapper.c.
| Butterworth2LowPass filt_acc[3] |
Definition at line 118 of file ekf_aw_wrapper.c.
Referenced by ekf_aw_wrapper_fetch(), ekf_aw_wrapper_init(), and ekf_aw_wrapper_periodic().
| Butterworth2LowPass filt_acc_low[3] |
Definition at line 119 of file ekf_aw_wrapper.c.
Referenced by ekf_aw_wrapper_fetch(), ekf_aw_wrapper_init(), and ekf_aw_wrapper_periodic().
| Butterworth2LowPass filt_airspeed_pitot |
Definition at line 126 of file ekf_aw_wrapper.c.
Referenced by ekf_aw_wrapper_fetch(), ekf_aw_wrapper_init(), and ekf_aw_wrapper_periodic().
| Butterworth2LowPass filt_elevator_pprz |
Definition at line 125 of file ekf_aw_wrapper.c.
Referenced by ekf_aw_wrapper_fetch(), ekf_aw_wrapper_init(), and ekf_aw_wrapper_periodic().
| Butterworth2LowPass filt_euler[3] |
Definition at line 121 of file ekf_aw_wrapper.c.
Referenced by ekf_aw_wrapper_fetch(), ekf_aw_wrapper_init(), and ekf_aw_wrapper_periodic().
| Butterworth2LowPass filt_groundspeed[3] |
Definition at line 117 of file ekf_aw_wrapper.c.
Referenced by ekf_aw_wrapper_fetch(), ekf_aw_wrapper_init(), and ekf_aw_wrapper_periodic().
| Butterworth2LowPass filt_hover_prop_rpm[EKF_AW_RPM_HOVER_NUM] |
Definition at line 122 of file ekf_aw_wrapper.c.
Referenced by ekf_aw_wrapper_fetch(), ekf_aw_wrapper_init(), and ekf_aw_wrapper_periodic().
| Butterworth2LowPass filt_pusher_prop_rpm |
Definition at line 123 of file ekf_aw_wrapper.c.
Referenced by ekf_aw_wrapper_fetch(), ekf_aw_wrapper_init(), and ekf_aw_wrapper_periodic().
| Butterworth2LowPass filt_rate[3] |
Definition at line 120 of file ekf_aw_wrapper.c.
Referenced by ekf_aw_wrapper_fetch(), ekf_aw_wrapper_init(), and ekf_aw_wrapper_periodic().
| Butterworth2LowPass filt_skew |
Definition at line 124 of file ekf_aw_wrapper.c.
Referenced by ekf_aw_wrapper_fetch(), ekf_aw_wrapper_init(), and ekf_aw_wrapper_periodic().
| bool reset_filter = false |
Definition at line 108 of file ekf_aw_wrapper.c.
| abi_event RPM_ev |
Definition at line 95 of file ekf_aw_wrapper.c.
Referenced by ekf_aw_wrapper_init().
| float tau_filter_high = 25.0f |
Definition at line 104 of file ekf_aw_wrapper.c.
Referenced by ekf_aw_wrapper_init().
| float tau_filter_low = 0.2f |
Definition at line 105 of file ekf_aw_wrapper.c.
Referenced by ekf_aw_wrapper_init().
| float time_of_rpm = 0.0f |
Definition at line 96 of file ekf_aw_wrapper.c.
Referenced by rpm_cb().
| struct NedCoor_f zero_speed |
Definition at line 110 of file ekf_aw_wrapper.c.
Referenced by ekf_aw_wrapper_periodic().