18#ifndef EKF_AW_WRAPPER_ROTWING
19#define EKF_AW_WRAPPER_ROTWING false
21#ifndef EKF_AW_WRAPPER_RANDOM_INPUTS
22#define EKF_AW_WRAPPER_RANDOM_INPUTS false
24#ifndef EKF_AW_QUICK_CONVERGENCE
25#define EKF_AW_QUICK_CONVERGENCE false
27#ifndef EKF_AW_QUICK_CONVERGENCE_TIME
28#define EKF_AW_QUICK_CONVERGENCE_TIME 10.0f
31#define EKF_AW_DEBUG false
34#if EKF_AW_WRAPPER_ROTWING
35#include "modules/rotwing_drone/wing_rotation_controller_servo.h"
49 pprz_msg_send_AIRSPEED_WIND_ESTIMATOR_EKF(trans,
dev, AC_ID,
63static void debug_vect(
struct transport_tx *trans,
struct link_device *
dev,
char *name,
float *data,
int datasize)
65 pprz_msg_send_DEBUG_VECT(trans,
dev, AC_ID,
86 rw_state[1] =
ekf_aw.elevator_angle;
87 rw_state[2] =
ekf_aw.RPM_pusher;
88 rw_state[3] =
ekf_aw.RPM_hover[0];
128#ifndef PERIODIC_FREQUENCY_AIRSPEED_EKF_FETCH
129#define PERIODIC_FREQUENCY_AIRSPEED_EKF_FETCH 100
132#ifndef PERIODIC_FREQUENCY_AIRSPEED_EKF
133#define PERIODIC_FREQUENCY_AIRSPEED_EKF 25
145 for (
int8_t i = 0; i < 3; i++) {
155 ekf_aw.last_RPM_hover[i] = 0;
163#if PERIODIC_TELEMETRY
180 ekf_aw.internal_clock = 0;
181 ekf_aw.time_last_on_gnd = 0;
184 ekf_aw.override_start =
false;
185 ekf_aw.override_quick_convergence =
false;
189 ekf_aw.last_RPM_hover[i] = 0;
191 ekf_aw.last_RPM_pusher = 0;
196 ekf_aw.health.healthy =
true;
ekf_aw.health.crashes_n = 0.0f;
197 ekf_aw.innov_V_gnd.x = 0.0f;
ekf_aw.innov_V_gnd.y = 0.0f;
ekf_aw.innov_V_gnd.z = 0.0f;
198 ekf_aw.innov_acc_filt.y = 0.0f;
ekf_aw.innov_acc_filt.y = 0.0f;
ekf_aw.innov_acc_filt.z = 0.0f;
199 ekf_aw.innov_V_pitot = 0.0f;
201 ekf_aw.fuselage_force[0] = 0.0f;
ekf_aw.fuselage_force[1] = 0.0f;
ekf_aw.fuselage_force[2] = 0.0f;
202 ekf_aw.wing_force[0] = 0.0f;
ekf_aw.wing_force[1] = 0.0f;
ekf_aw.wing_force[2] = 0.0f;
203 ekf_aw.elevator_force[0] = 0.0f;
ekf_aw.elevator_force[1] = 0.0f;
ekf_aw.elevator_force[2] = 0.0f;
204 ekf_aw.pusher_force[0] = 0.0f;
ekf_aw.pusher_force[1] = 0.0f;
ekf_aw.pusher_force[2] = 0.0f;
205 ekf_aw.hover_force[0] = 0.0f;
ekf_aw.hover_force[1] = 0.0f;
ekf_aw.hover_force[2] = 0.0f;
208 ekf_aw.elevator_angle = 0.0f;
211 ekf_aw.RPM_hover[i] = 0.0f;
227 ekf_aw.acc.x = 1.0f * rand() / RAND_MAX;
228 ekf_aw.acc.y = 1.0f * rand() / RAND_MAX;
229 ekf_aw.acc.z = 1.0f * rand() / RAND_MAX;
230 ekf_aw.gyro.p = 0.1f * rand() / RAND_MAX;
231 ekf_aw.gyro.q = 0.1f * rand() / RAND_MAX;
232 ekf_aw.gyro.r = 0.1f * rand() / RAND_MAX;
233 ekf_aw.euler.phi = 1.0f * rand() / RAND_MAX;
234 ekf_aw.euler.theta = 1.0f * rand() / RAND_MAX;
235 ekf_aw.euler.psi = 1.0f * rand() / RAND_MAX;
237 ekf_aw.RPM_hover[i] = 1000.0f * rand() / RAND_MAX;
239 ekf_aw.RPM_pusher = 1000.0f * rand() / RAND_MAX;
240 ekf_aw.skew = 10.0f * rand() / RAND_MAX;
241 ekf_aw.elevator_angle = 10.0f * rand() / RAND_MAX;
243 ekf_aw.Vg_NED.x = 10.0f * rand() / RAND_MAX;
244 ekf_aw.Vg_NED.y = 10.0f * rand() / RAND_MAX;
245 ekf_aw.Vg_NED.z = 10.0f * rand() / RAND_MAX;
247 ekf_aw.acc_filt.x = 1.0f * rand() / RAND_MAX;
248 ekf_aw.acc_filt.y = 1.0f * rand() / RAND_MAX;
249 ekf_aw.acc_filt.z = 1.0f * rand() / RAND_MAX;
251 ekf_aw.V_pitot = 10.0f * rand() / RAND_MAX;
351#if EKF_AW_WRAPPER_ROTWING
390#if EKF_AW_WRAPPER_ROTWING
397 de = (-0.004885417f * *elev_pprz + 36.6f) * 3.14f / 180.0f;
416 for (
int i = 0; i < num_act_message; i++) {
420 switch (rpm_message[i].
idx) {
422 ekf_aw.last_RPM_hover[0] = rpm_message[i].
rpm;
425 ekf_aw.last_RPM_hover[1] = rpm_message[i].
rpm;
428 ekf_aw.last_RPM_hover[2] = rpm_message[i].
rpm;
431 ekf_aw.last_RPM_hover[3] = rpm_message[i].
rpm;
434 ekf_aw.last_RPM_pusher = rpm_message[i].
rpm;
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
#define ACT_FEEDBACK_RPM_SENSOR_ID
bool autopilot_in_flight(void)
get in_flight flag
Core autopilot interface common to all firmwares.
static struct uart_periph * dev
struct ekfAwParameters * ekf_aw_get_param_handle(void)
void ekf_aw_get_hover_force(float force[3])
float ekf_aw_get_innov_V_pitot(void)
struct FloatVect3 ekf_aw_get_innov_accel_filt(void)
void ekf_aw_get_meas_cov(float meas_cov[7])
void ekf_aw_set_speed_body(struct NedCoor_f *s)
void ekf_aw_get_state_cov(float state_cov[9])
struct NedCoor_f ekf_aw_get_speed_body(void)
struct ekfHealth ekf_aw_get_health(void)
void ekf_aw_get_fuselage_force(float force[3])
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_wind_ned(void)
void ekf_aw_get_elevator_force(float force[3])
struct FloatVect3 ekf_aw_get_innov_V_gnd(void)
void ekf_aw_get_pusher_force(float force[3])
void ekf_aw_get_process_cov(float process_cov[12])
void ekf_aw_get_wing_force(float force[3])
struct NedCoor_f ekf_aw_get_offset(void)
#define EKF_AW_QUICK_CONVERGENCE
static void send_airspeed_wind_ekf_debug(struct transport_tx *trans, struct link_device *dev)
Butterworth2LowPass filt_hover_prop_rpm[EKF_AW_RPM_HOVER_NUM]
static void rpm_cb(uint8_t sender_id, struct act_feedback_t *rpm_message, uint8_t num_act)
Butterworth2LowPass filt_groundspeed[3]
Butterworth2LowPass filt_acc_low[3]
void ekf_aw_wrapper_fetch(void)
#define EKF_AW_WRAPPER_RANDOM_INPUTS
struct NedCoor_f zero_speed
void set_in_air_status(bool in_air)
Butterworth2LowPass filt_airspeed_pitot
static void debug_vect(struct transport_tx *trans, struct link_device *dev, char *name, float *data, int datasize)
static void send_airspeed_wind_ekf(struct transport_tx *trans, struct link_device *dev)
Butterworth2LowPass filt_skew
Butterworth2LowPass filt_pusher_prop_rpm
#define EKF_AW_QUICK_CONVERGENCE_TIME
Butterworth2LowPass filt_euler[3]
Butterworth2LowPass filt_acc[3]
#define PERIODIC_FREQUENCY_AIRSPEED_EKF_FETCH
void ekf_aw_wrapper_periodic(void)
#define PERIODIC_FREQUENCY_AIRSPEED_EKF
Butterworth2LowPass filt_rate[3]
void ekf_aw_wrapper_init(void)
Butterworth2LowPass filt_elevator_pprz
#define EKF_AW_RPM_HOVER_NUM
Euler angles.
#define VECT3_COPY(_a, _b)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
#define ACCEL_BFP_OF_REAL(_af)
void int32_rmat_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_a2b, struct Int32Vect3 *va)
rotate 3D vector by rotation matrix.
vector in North East Down coordinates
static struct NedCoor_i * stateGetAccelNed_i(void)
Get acceleration in NED coordinates (int).
static struct Int32RMat * stateGetNedToBodyRMat_i(void)
Get vehicle body attitude rotation matrix (int).
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
static struct Int32Vect3 * stateGetAccelBody_i(void)
Get acceleration in Body coordinates (int).
static float stateGetAirspeed_f(void)
Get airspeed (float).
static parameters * ekf_params
The EKF parameters.
Simple first order low pass filter with bilinear transform.
struct SecondOrderLowPass Butterworth2LowPass
Second order Butterworth low pass filter.
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, const float tau, const float sample_time, const float value)
Init a second order Butterworth filter.
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, const float value)
Update second order Butterworth low pass filter state with a new value.
Hardware independent API for actuators (servos, motor controllers).
Paparazzi generic algebra macros.
vector in North East Down coordinates Units: meters
struct FloatVect3 body_accel_f
int16_t actuators_pprz[INDI_NUM_ACT+1]
PPRZ command to each actuator Can be used to directly control actuators from the control algorithm if...
API to get/set the generic vehicle states.
Architecture independent timing functions.
static float get_sys_time_float(void)
Get the time in seconds since startup.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
signed char int8_t
Typedef defining 8 bit char type.