151 changes: 151 additions & 0 deletions conf/modules/ekf_aw.xml
View file
@@ -0,0 +1,151 @@
<!DOCTYPE module SYSTEM "module.dtd">

<module name="ekf_aw" dir="meteo">
<doc>
<description>
Airspeed and Wind Estimator with EKF
Frédéric Larocque
</description>
<define name="PERIODIC_FREQUENCY_AIRSPEED_EKF" value="25" description="Airspeed periodic frequency"/>
<define name="PERIODIC_FREQUENCY_AIRSPEED_EKF_FETCH" value="100" description="Airspeed periodic fetch frequency"/>

<define name="EKF_AW_P0_V_BODY" value="1e-00f" description="Initial covariance body velocity"/>
<define name="EKF_AW_P0_MU" value="1e-00f" description="Initial covariance wind"/>
<define name="EKF_AW_P0_OFFSET" value="1e-00f" description="Initial covariance offset"/>

<define name="EKF_AW_Q_ACCEL" value="1e-00f" description="Accel process noise"/>
<define name="EKF_AW_Q_GYRO" value="1e-00f" description="Gyro process noise"/>
<define name="EKF_AW_Q_MU" value="1e-00f" description="Wind process noise"/>
<define name="EKF_AW_Q_OFFSET" value="1e-00f" description="Offset process noise"/>

<define name="EKF_AW_R_V_GND" value="1e-00f" description="GPS Velocity measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_X" value="1e-00f" description="Filtered x accel measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_Y" value="1e-00f" description="Filtered y accel measurement noise"/>
<define name="EKF_AW_R_ACCEL_FILT_Z" value="1e-00f" description="Filtered z accel measurement noise"/>
<define name="EKF_AW_R_V_PITOT" value="1e-00f" description="Pitot Tube Velocity measurement noise"/>

<define name="EKF_AW_WING_INSTALLED" value="false" description="Use wing contribution"/>
<define name="EKF_AW_USE_MODEL_BASED" value="false" description="Use model based to augment filter"/>
<define name="EKF_AW_USE_BETA" value="false" description="Use beta to estimate sideforce"/>
<define name="EKF_AW_PROPAGATE_OFFSET" value="false" description="Propagate the offset state"/>

<define name="EKF_AW_VEHICLE_MASS" value="0.0" description="Mass of the vehicle"/>

<define name="EKF_AW_K1_FX_DRAG" value="0.f" description="Drag coefficient linear to u used if use model based is set to false"/>
<define name="EKF_AW_K2_FX_DRAG" value="0.f" description="Drag coefficient linear to u^2 used if use model based is set to false"/>

<define name="EKF_AW_K_FY_BETA" value="1e-00f" description="Fy v (side velocity) coefficient"/>
<define name="EKF_AW_K_FY_V" value="1e-00f" description="Fy beta coefficient"/>

<define name="EKF_AW_K1_FX_FUSELAGE" value="1e-00f" description="Fx Fuselage Coeff"/>
<define name="EKF_AW_K2_FX_FUSELAGE" value="1e-00f" description="Fx Fuselage Coeff"/>
<define name="EKF_AW_K3_FX_FUSELAGE" value="1e-00f" description="Fx Fuselage Coeff"/>
<define name="EKF_AW_K4_FX_FUSELAGE" value="1e-00f" description="Fx Fuselage Coeff"/>

<define name="EKF_AW_K1_FX_HOVER" value="1e-00f" description="Fx Hover Prop Coeff"/>
<define name="EKF_AW_K2_FX_HOVER" value="1e-00f" description="Fx Hover Prop Coeff"/>

<define name="EKF_AW_K1_FX_WING" value="1e-00f" description="Fx Wing Coeff"/>
<define name="EKF_AW_K2_FX_WING" value="1e-00f" description="Fx Wing Coeff"/>
<define name="EKF_AW_K3_FX_WING" value="1e-00f" description="Fx Wing Coeff"/>
<define name="EKF_AW_K4_FX_WING" value="1e-00f" description="Fx Wing Coeff"/>
<define name="EKF_AW_K5_FX_WING" value="1e-00f" description="Fx Wing Coeff"/>

<define name="EKF_AW_K1_FX_PUSH" value="1e-00f" description="Fx Pusher Coeff"/>
<define name="EKF_AW_K2_FX_PUSH" value="1e-00f" description="Fx Pusher Coeff"/>
<define name="EKF_AW_K3_FX_PUSH" value="1e-00f" description="Fx Pusher Coeff"/>

<define name="EKF_AW_K1_FX_ELEV" value="1e-00f" description="Fx Elevator Coeff"/>
<define name="EKF_AW_K2_FX_ELEV" value="1e-00f" description="Fx Elevator Coeff"/>
<define name="EKF_AW_K3_FX_ELEV" value="1e-00f" description="Fx Elevator Coeff"/>

<define name="EKF_AW_K1_FZ_FUSELAGE" value="1e-00f" description="Fz Fuselage Coeff"/>
<define name="EKF_AW_K2_FZ_FUSELAGE" value="1e-00f" description="Fz Fuselage Coeff"/>
<define name="EKF_AW_K3_FZ_FUSELAGE" value="1e-00f" description="Fz Fuselage Coeff"/>
<define name="EKF_AW_K4_FZ_FUSELAGE" value="1e-00f" description="Fz Fuselage Coeff"/>

<define name="EKF_AW_K1_FZ_HOVER" value="1e-00f" description="Fz Hover Prop Coeff"/>
<define name="EKF_AW_K2_FZ_HOVER" value="1e-00f" description="Fz Hover Prop Coeff"/>
<define name="EKF_AW_K3_FZ_HOVER" value="1e-00f" description="Fz Hover Prop Coeff"/>
<define name="EKF_AW_K4_FZ_HOVER" value="1e-00f" description="Fz Hover Prop Coeff"/>

<define name="EKF_AW_K1_Fz_WING" value="1e-00f" description="Fz Wing Coeff"/>
<define name="EKF_AW_K2_Fz_WING" value="1e-00f" description="Fz Wing Coeff"/>
<define name="EKF_AW_K3_Fz_WING" value="1e-00f" description="Fz Wing Coeff"/>
<define name="EKF_AW_K4_Fz_WING" value="1e-00f" description="Fz Wing Coeff"/>
<define name="EKF_AW_K5_Fz_WING" value="1e-00f" description="Fz Wing Coeff"/>

<define name="EKF_AW_K1_FZ_ELEV" value="1e-00f" description="Fz Elevator Coeff"/>
<define name="EKF_AW_K2_FZ_ELEV" value="1e-00f" description="Fz Elevator Coeff"/>

<define name="EKF_AW_ELEV_MAX_ANGLE" value="1e-00f" description="Maximum elevator angle"/>
<define name="EKF_AW_ELEV_MIN_ANGLE" value="1e-00f" description="Minimum elevator angle"/>
<define name="EKF_AW_AOA_MAX_ANGLE" value="1e-00f" description="Maximum angle of attack"/>
<define name="EKF_AW_AOA_MIN_ANGLE" value="1e-00f" description="Minimum angle of attack"/>
</doc>
<settings>
<dl_settings>
<dl_settings name="EKF_Airspeed_Wind">
<dl_setting min="0" max="1" step="1" var="ekf_aw.override_start" module="meteo/ekf_aw_wrapper" shortname="Override Start"/>
<dl_setting min="0" max="1" step="1" var="ekf_aw.override_quick_convergence" module="meteo/ekf_aw_wrapper" shortname="Override Quick Convergence"/>
<dl_setting min="0" max="1" step="1" var="ekf_aw.reset" module="meteo/ekf_aw_wrapper" shortname="Reset States and Health" handler="reset"/>
<dl_setting min="0" max="1" step="1" var="ekf_aw_params.propagate_offset" module="meteo/ekf_aw" shortname="propagate offset"/>

<dl_setting min="1E-1" max="8" step="1E-1" var="ekf_aw_params.vehicle_mass" module="meteo/ekf_aw" shortname="Vehicle Mass"/>

<dl_setting min="-1" max="0" step="1E-3" var="ekf_aw_params.k_fx_drag[0]" module="meteo/ekf_aw" shortname="k_drag_lin"/>
<dl_setting min="-1" max="0" step="1E-3" var="ekf_aw_params.k_fx_drag[1]" module="meteo/ekf_aw" shortname="k_drag_quad"/>

<dl_setting min="-5E-2" max="0" step="1E-4" var="ekf_aw_params.k_fy_beta" module="meteo/ekf_aw" shortname="k_beta"/>
<dl_setting min="-5E-2" max="0" step="1E-4" var="ekf_aw_params.k_fy_v" module="meteo/ekf_aw" shortname="k_v"/>

<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.Q_accel" module="meteo/ekf_aw" shortname="Q accel" handler="update_Q_accel"/>
<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.Q_gyro" module="meteo/ekf_aw" shortname="Q gyro" handler="update_Q_gyro"/>
<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.Q_mu" module="meteo/ekf_aw" shortname="Q wind" handler="update_Q_mu"/>
<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.Q_k" module="meteo/ekf_aw" shortname="Q offset" handler="update_Q_k"/>

<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.R_V_gnd" module="meteo/ekf_aw" shortname="R V_gnd" handler="update_R_V_gnd"/>
<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.R_accel_filt[0]" module="meteo/ekf_aw" shortname="R Accel Filt x" handler="update_R_accel_filt_x"/>
<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.R_accel_filt[1]" module="meteo/ekf_aw" shortname="R Accel Filt y" handler="update_R_accel_filt_y"/>
<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.R_accel_filt[2]" module="meteo/ekf_aw" shortname="R Accel Filt z" handler="update_R_accel_filt_z"/>
<dl_setting min="1E-9" max="1E-2" step="1E-8" var="ekf_aw_params.R_V_pitot" module="meteo/ekf_aw" shortname="R Pitot" handler="update_R_V_pitot"/>

<dl_setting min="-10" max="10" step="0.1" var="ekf_aw.wind_guess.x" module="meteo/ekf_aw_wrapper" shortname="Wind Guess N" handler="set_wind_N"/>
<dl_setting min="-10" max="10" step="0.1" var="ekf_aw.wind_guess.y" module="meteo/ekf_aw_wrapper" shortname="Wind Guess E" handler="set_wind_E"/>
<dl_setting min="-10" max="10" step="0.1" var="ekf_aw.wind_guess.z" module="meteo/ekf_aw_wrapper" shortname="Wind Guess D" handler="set_wind_D"/>

<dl_setting min="-1" max="1" step="1E-3" var="ekf_aw.offset_guess.x" module="meteo/ekf_aw_wrapper" shortname="Offset Guess x" handler="set_offset_x"/>
<dl_setting min="-1" max="1" step="1E-3" var="ekf_aw.offset_guess.y" module="meteo/ekf_aw_wrapper" shortname="Offset Guess y" handler="set_offset_y"/>
<dl_setting min="-1" max="1" step="1E-3" var="ekf_aw.offset_guess.z" module="meteo/ekf_aw_wrapper" shortname="Offset Guess z" handler="set_offset_z"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="ekf_aw_wrapper.h"/>
</header>
<init fun="ekf_aw_wrapper_init()"/>
<periodic fun="ekf_aw_wrapper_periodic()" freq="25"/>
<periodic fun="ekf_aw_wrapper_fetch()" freq="100"/>
<makefile target="ap|nps">
<file name="ekf_aw.cpp"/>
<file name="ekf_aw_wrapper.c"/>
<include name="$(PAPARAZZI_SRC)/sw/ext/matrix/"/>
</makefile>
</module>

<!--Your comment
Target NPS or AP
<configure name="CXXSTANDARD" value="-std=c++14"/>
<flag name="LDFLAGS" value="lstdc++" />
Target AP
<define name="EIGEN_NO_DEBUG"/> Removes debug checks and should lead to faster code execution
<flag name="CXXFLAGS" value="Wno-bool-compare"/>
<flag name="CXXFLAGS" value="Wno-logical-not-parentheses"/>
<datalink message="PING" fun="datalink_parse_PING(dev, trans, buf)"/>
-->

2 changes: 2 additions & 0 deletions conf/modules/imu_common.xml
View file
Expand Up @@ -15,6 +15,8 @@
<define name="GYRO_ABI_SEND_ID" value="ABI_BROADCAST" description="The gyro ABI ID which is send over telemetry/logging"/>
<define name="ACCEL_ABI_SEND_ID" value="ABI_BROADCAST" description="The accel ABI ID which is send over telemetry/logging"/>
<define name="MAG_ABI_SEND_ID" value="ABI_BROADCAST" description="The mag ABI ID which is send over telemetry/logging"/>
<define name="LOG_HIGHSPEED" value="FALSE" description="Log all the accel/gyro measurements at the IMU sampling rates in floats"/>
<define name="LOG_HIGHSPEED_DEVICE" value="flightrecorder_sdlog" description="The device to log all the highspeeds measurements"/>
</section>
</doc>
<settings>
Expand Down
7 changes: 6 additions & 1 deletion conf/telemetry/highspeed_rotorcraft.xml
View file
Expand Up @@ -45,8 +45,10 @@
<message name="HYBRID_GUIDANCE" period="0.4"/>
<message name="ESC" period="0.5"/>
<message name="AHRS_REF_QUAT" period="0.1"/>
<message name="INS_FLOW_INFO" period="0.1"/>
<message name="INS_FLOW_INFO" period="0.1"/>
<message name="GPS_RTK" period="0.1"/>
<message name="AIRSPEED_RAW" period="0.1"/>
<message name="ROTATING_WING_STATE" period="0.2"/>
</mode>

<mode name="calibration">
Expand Down Expand Up @@ -82,6 +84,7 @@
<message name="IMU_MAG_RAW" period=".05"/>
<message name="BARO_RAW" period=".1"/>
<message name="ARDRONE_NAVDATA" period=".05"/>
<message name="AIRSPEED_RAW" period="0.1"/>
</mode>

<mode name="scaled_sensors">
Expand Down Expand Up @@ -149,6 +152,7 @@
<mode name="vel_guidance" key_press="q">
<message name="ALIVE" period="0.9"/>
<message name="HYBRID_GUIDANCE" period="0.062"/>
<message name="GUIDANCE_INDI_HYBRID" period="0.1"/>
<!--<message name="STAB_ATTITUDE_REF" period="0.4"/>-->
<message name="ROTORCRAFT_FP" period="0.8"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
Expand Down Expand Up @@ -242,6 +246,7 @@
<message name="ACTUATORS" period="0.002"/>
<message name="GPS_RTK" period="0.1"/>
<message name="IMU_HEATER" period="1.0"/>
<message name="AIRSPEED_RAW" period="0.01"/>
</mode>
</process>

Expand Down
15 changes: 12 additions & 3 deletions sw/airborne/modules/computer_vision/lib/vision/act_fast.c
View file
Expand Up @@ -27,17 +27,21 @@ All rights reserved.
#include "math.h"
#include "image.h"
#include "../../opticflow/opticflow_calculator.h"
#include "generated/airframe.h"

// ACT-FAST agents arrays
// equal to the maximal number of corners defined by fast9_rsize in opticflow_calculator.c
// TODO Currently hardcoded to two cameras
#define MAX_AGENTS FAST9_MAX_CORNERS
#ifndef OPTICFLOW_CAMERA2
struct agent_t agents[1][MAX_AGENTS];
#ifdef OPTICFLOW_CAMERA2
#define FAST9_MAX_CAMERAS 2
#else
struct agent_t agents[2][MAX_AGENTS];
#define FAST9_MAX_CAMERAS 1
#endif

struct agent_t agents[FAST9_MAX_CAMERAS][MAX_AGENTS];


/**
* Do an ACT-FAST corner detection.
* @param[in] *img The image to do the corner detection on
Expand All @@ -55,6 +59,11 @@ void act_fast(struct image_t *img, uint8_t fast_threshold, uint16_t *num_corners
uint16_t n_agents, uint16_t n_time_steps, float long_step, float short_step, int min_gradient,
int gradient_method, int camera_id)
{
// Input protection:
if (camera_id >= FAST9_MAX_CAMERAS) {
*num_corners = 0;
return;
}

/*
* Procedure:
Expand Down
1 change: 1 addition & 0 deletions sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
View file
Expand Up @@ -45,6 +45,7 @@
#include "size_divergence.h"
#include "linear_flow_fit.h"
#include "modules/sonar/agl_dist.h"
#include "generated/airframe.h"

// to get the definition of front_camera / bottom_camera
#include BOARD_CONFIG
Expand Down
1 change: 1 addition & 0 deletions sw/airborne/modules/computer_vision/opticflow_module.c
View file
Expand Up @@ -39,6 +39,7 @@
#include "errno.h"

#include "cv.h"
#include "generated/airframe.h"

uint16_t fps_OF;

Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/modules/ctrl/ctrl_eff_sched_rot_wing.c
View file
Expand Up @@ -121,7 +121,7 @@ static abi_event wing_position_ev;
static void wing_position_cb(uint8_t sender_id UNUSED, struct act_feedback_t *pos_msg, uint8_t num_act)
{
for (int i=0; i<num_act; i++){
if (pos_msg[i].set.position && (pos_msg[i].idx = SERVO_ROTATION_MECH))
if (pos_msg[i].set.position && (pos_msg[i].idx == SERVO_ROTATION_MECH))
{
// Get wing rotation angle from sensor
eff_sched_var.wing_rotation_rad = 0.5 * M_PI - pos_msg[i].position;
Expand Down
12 changes: 12 additions & 0 deletions sw/airborne/modules/energy/electrical.c
View file
Expand Up @@ -73,6 +73,9 @@ static struct {
#if defined ADC_CHANNEL_CURRENT && !defined SITL
struct adc_buf current_adc_buf;
#endif
#if defined ADC_CHANNEL_CURRENT2 && !defined SITL
struct adc_buf current2_adc_buf;
#endif
#ifdef MILLIAMP_AT_FULL_THROTTLE
float nonlin_factor;
#endif
Expand Down Expand Up @@ -116,6 +119,10 @@ void electrical_init(void)
/* measure current if available, otherwise estimate it */
#if defined ADC_CHANNEL_CURRENT && !defined SITL
adc_buf_channel(ADC_CHANNEL_CURRENT, &electrical_priv.current_adc_buf, DEFAULT_AV_NB_SAMPLE);

#if defined ADC_CHANNEL_CURRENT2 && !defined SITL
adc_buf_channel(ADC_CHANNEL_CURRENT2, &electrical_priv.current2_adc_buf, DEFAULT_AV_NB_SAMPLE);
#endif
#elif defined MILLIAMP_AT_FULL_THROTTLE
PRINT_CONFIG_VAR(CURRENT_ESTIMATION_NONLINEARITY)
electrical_priv.nonlin_factor = CURRENT_ESTIMATION_NONLINEARITY;
Expand All @@ -137,6 +144,11 @@ void electrical_periodic(void)
#ifndef SITL
int32_t current_adc = electrical_priv.current_adc_buf.sum / electrical_priv.current_adc_buf.av_nb_sample;
electrical.current = MilliAmpereOfAdc(current_adc) / 1000.f;

#ifdef ADC_CHANNEL_CURRENT2
current_adc = electrical_priv.current2_adc_buf.sum / electrical_priv.current2_adc_buf.av_nb_sample;
electrical.current += MilliAmpereOfAdc2(current_adc) / 1000.f;
#endif
#endif
#elif defined MILLIAMP_AT_FULL_THROTTLE && defined COMMAND_CURRENT_ESTIMATION
/*
Expand Down
47 changes: 41 additions & 6 deletions sw/airborne/modules/imu/imu.c
View file
Expand Up @@ -173,6 +173,11 @@ PRINT_CONFIG_VAR(IMU_ACCEL_ABI_SEND_ID)
#endif
PRINT_CONFIG_VAR(IMU_MAG_ABI_SEND_ID)

/** By default log highspeed on the flightrecorder */
#ifndef IMU_LOG_HIGHSPEED_DEVICE
#define IMU_LOG_HIGHSPEED_DEVICE flightrecorder_sdlog
#endif


#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
Expand Down Expand Up @@ -590,9 +595,18 @@ static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates

// Add all the other samples
for(uint8_t i = 0; i < samples-1; i++) {
integrated_sensor.p += RATE_FLOAT_OF_BFP((data[i].p - gyro->neutral.p) * gyro->scale[0].p / gyro->scale[1].p);
integrated_sensor.q += RATE_FLOAT_OF_BFP((data[i].q - gyro->neutral.q) * gyro->scale[0].q / gyro->scale[1].q);
integrated_sensor.r += RATE_FLOAT_OF_BFP((data[i].r - gyro->neutral.r) * gyro->scale[0].r / gyro->scale[1].r);
struct FloatRates f_sample;
f_sample.p = RATE_FLOAT_OF_BFP((data[i].p - gyro->neutral.p) * gyro->scale[0].p / gyro->scale[1].p);
f_sample.q = RATE_FLOAT_OF_BFP((data[i].q - gyro->neutral.q) * gyro->scale[0].q / gyro->scale[1].q);
f_sample.r = RATE_FLOAT_OF_BFP((data[i].r - gyro->neutral.r) * gyro->scale[0].r / gyro->scale[1].r);

#if IMU_LOG_HIGHSPEED
pprz_msg_send_IMU_GYRO(&pprzlog_tp.trans_tx, &(IMU_LOG_HIGHSPEED_DEVICE).device, AC_ID, &sender_id, &f_sample.p, &f_sample.q, &f_sample.r);
#endif

integrated_sensor.p += f_sample.p;
integrated_sensor.q += f_sample.q;
integrated_sensor.r += f_sample.r;
}

// Rotate to body frame
Expand All @@ -612,6 +626,12 @@ static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates
(void)rate; // Surpress compile warning not used
#endif

#if IMU_LOG_HIGHSPEED
struct FloatRates f_sample;
RATES_FLOAT_OF_BFP(f_sample, scaled);
pprz_msg_send_IMU_GYRO(&pprzlog_tp.trans_tx, &(IMU_LOG_HIGHSPEED_DEVICE).device, AC_ID, &sender_id, &f_sample.p, &f_sample.q, &f_sample.r);
#endif

// Copy and send
gyro->temperature = temp;
RATES_COPY(gyro->scaled, scaled_rot);
Expand Down Expand Up @@ -658,9 +678,18 @@ static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect

// Add all the other samples
for(uint8_t i = 0; i < samples-1; i++) {
integrated_sensor.x += ACCEL_FLOAT_OF_BFP((data[i].x - accel->neutral.x) * accel->scale[0].x / accel->scale[1].x);
integrated_sensor.y += ACCEL_FLOAT_OF_BFP((data[i].y - accel->neutral.y) * accel->scale[0].y / accel->scale[1].y);
integrated_sensor.z += ACCEL_FLOAT_OF_BFP((data[i].z - accel->neutral.z) * accel->scale[0].z / accel->scale[1].z);
struct FloatVect3 f_sample;
f_sample.x = ACCEL_FLOAT_OF_BFP((data[i].x - accel->neutral.x) * accel->scale[0].x / accel->scale[1].x);
f_sample.y = ACCEL_FLOAT_OF_BFP((data[i].y - accel->neutral.y) * accel->scale[0].y / accel->scale[1].y);
f_sample.z = ACCEL_FLOAT_OF_BFP((data[i].z - accel->neutral.z) * accel->scale[0].z / accel->scale[1].z);

#if IMU_LOG_HIGHSPEED
pprz_msg_send_IMU_ACCEL(&pprzlog_tp.trans_tx, &(IMU_LOG_HIGHSPEED_DEVICE).device, AC_ID, &sender_id, &f_sample.x, &f_sample.y, &f_sample.z);
#endif

integrated_sensor.x += f_sample.x;
integrated_sensor.y += f_sample.y;
integrated_sensor.z += f_sample.z;
}

// Rotate to body frame
Expand All @@ -680,6 +709,12 @@ static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect
(void)rate; // Surpress compile warning not used
#endif

#if IMU_LOG_HIGHSPEED
struct FloatVect3 f_sample;
ACCELS_FLOAT_OF_BFP(f_sample, scaled);
pprz_msg_send_IMU_ACCEL(&pprzlog_tp.trans_tx, &(IMU_LOG_HIGHSPEED_DEVICE).device, AC_ID, &sender_id, &f_sample.x, &f_sample.y, &f_sample.z);
#endif

// Copy and send
accel->temperature = temp;
VECT3_COPY(accel->scaled, scaled_rot);
Expand Down
1,559 changes: 1,559 additions & 0 deletions sw/airborne/modules/meteo/ekf_aw.cpp
View file

Large diffs are not rendered by default.

145 changes: 145 additions & 0 deletions sw/airborne/modules/meteo/ekf_aw.h
View file
@@ -0,0 +1,145 @@
#ifndef EKF_AW_H
#define EKF_AW_H

#ifdef __cplusplus
extern "C" {
#endif

#include "std.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_geodetic_float.h"

// Settings
struct ekfAwParameters {
// Q
float Q_accel; ///< accel process noise
float Q_gyro; ///< gyro process noise
float Q_mu; ///< wind process noise
float Q_k; ///< offset process noise

// R
float R_V_gnd; ///< speed measurement noise
float R_accel_filt[3]; ///< filtered accel measurement noise
float R_V_pitot; ///< airspeed measurement noise

// Model Based Parameters
float vehicle_mass;

// X Axis
float k_fx_drag[2]; // Temporary setting for fuselage + hover prop
float k_fx_fuselage[4];
float k_fx_hover[3];
float k_fx_wing[5];
float k_fx_push[3];
float k_fx_elev[3];

// Y Axis
float k_fy_beta;
float k_fy_v;
float k_fy_wing[5];

// Z Axis
float k_fz_fuselage[4];
float k_fz_wing[4];
float k_fz_hover[5];
float k_fz_elev[2];

// Other options
bool use_model[3];
bool use_pitot;
bool propagate_offset;
bool quick_convergence;
};

struct ekfHealth{
bool healthy;
uint16_t crashes_n;
};

extern struct ekfAwParameters ekf_aw_params;

// Init functions
extern void ekf_aw_init(void);
extern void ekf_aw_reset(void);

// Filtering functions
extern 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, struct FloatVect3 * V_gnd, struct FloatVect3 *acc_filt, float *V_pitot,float dt);

// Getter/Setter functions
extern struct NedCoor_f ekf_aw_get_speed_body(void);
extern struct NedCoor_f ekf_aw_get_wind_ned(void);
extern struct NedCoor_f ekf_aw_get_offset(void);
extern struct FloatVect3 ekf_aw_get_innov_V_gnd(void);
extern struct FloatVect3 ekf_aw_get_innov_accel_filt(void);
extern float ekf_aw_get_innov_V_pitot(void);
extern void ekf_aw_get_meas_cov(float meas_cov[7]);
extern void ekf_aw_get_state_cov(float state_cov[9]);
extern void ekf_aw_get_process_cov(float process_cov[9]);
extern void ekf_aw_get_fuselage_force(float force[3]);
extern void ekf_aw_get_wing_force(float force[3]);
extern void ekf_aw_get_elevator_force(float force[3]);
extern void ekf_aw_get_hover_force(float force[3]);
extern void ekf_aw_get_pusher_force(float force[3]);
extern struct ekfAwParameters *ekf_aw_get_param_handle(void);

extern void ekf_aw_set_speed_body(struct NedCoor_f *s);
extern void ekf_aw_set_wind(struct NedCoor_f *s);
extern void ekf_aw_set_offset(struct NedCoor_f *s);
extern struct ekfHealth ekf_aw_get_health(void);

// Settings handlers
extern void ekf_aw_update_params(void);
extern void ekf_aw_reset_health(void);


// Handlers
#define ekf_aw_update_Q_accel(_v) { \
ekf_aw_params.Q_accel = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_Q_gyro(_v) { \
ekf_aw_params.Q_gyro = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_Q_mu(_v) { \
ekf_aw_params.Q_mu = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_Q_k(_v) { \
ekf_aw_params.Q_k = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_R_V_gnd(_v) { \
ekf_aw_params.R_V_gnd = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_R_accel_filt_x(_v) { \
ekf_aw_params.R_accel_filt[0] = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_R_accel_filt_y(_v) { \
ekf_aw_params.R_accel_filt[1] = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_R_accel_filt_z(_v) { \
ekf_aw_params.R_accel_filt[2] = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_R_V_pitot(_v) { \
ekf_aw_params.R_V_pitot = _v; \
ekf_aw_update_params(); \
}

#ifdef __cplusplus
}
#endif

#endif /* EKF_AW_H */
465 changes: 465 additions & 0 deletions sw/airborne/modules/meteo/ekf_aw_wrapper.c
View file

Large diffs are not rendered by default.

118 changes: 118 additions & 0 deletions sw/airborne/modules/meteo/ekf_aw_wrapper.h
View file
@@ -0,0 +1,118 @@
#ifndef EKF_AW_WRAPPER_H
#define EKF_AW_WRAPPER_H

#include "std.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_geodetic_float.h"
#include "modules/meteo/ekf_aw.h"


// EKF structure
struct ekfAw {

// States
struct NedCoor_f wind;
struct NedCoor_f V_body;
struct NedCoor_f offset;

// Inputs
struct FloatVect3 acc; ///< Last accelerometer measurements
struct FloatRates gyro; ///< Last gyroscope measurements
struct FloatEulers euler; /// Euler angles

#define EKF_AW_RPM_HOVER_NUM 4
int32_t last_RPM_hover[EKF_AW_RPM_HOVER_NUM]; // Value obtained from ABI Callback
int32_t last_RPM_pusher; // Value obtained from ABI Callback
float RPM_hover[EKF_AW_RPM_HOVER_NUM]; /// Hover motor RPM
float RPM_pusher; /// Pusher motor RPM
float skew; /// Skew
float elevator_angle;


// Measurements
struct FloatVect3 Vg_NED; /// Ground Speed
struct FloatVect3 acc_filt; ///< Last accelerometer measurements
float V_pitot; /// Pitot tube airspeed

// Innovation
struct FloatVect3 innov_V_gnd;
struct FloatVect3 innov_acc_filt;
float innov_V_pitot;

// Covariance
float meas_cov[7];
float state_cov[9];
float process_cov[12];

// Forces
float fuselage_force[3];
float wing_force[3];
float elevator_force[3];
float hover_force[3];
float pusher_force[3];

// Other
bool reset;
bool in_air;
struct NedCoor_f wind_guess;
struct NedCoor_f offset_guess;
struct ekfHealth health;
uint64_t internal_clock;
uint64_t time_last_on_gnd;
uint64_t time_last_in_air;
bool override_start;
bool override_quick_convergence;

};

extern void ekf_aw_wrapper_init(void);
extern void ekf_aw_wrapper_periodic(void);
extern void ekf_aw_wrapper_fetch(void);

extern void set_in_air_status(bool);


extern float tau_filter_high;
extern float tau_filter_low;

extern struct ekfAw ekf_aw;

// Handlers
#define ekf_aw_wrapper_reset(_v) { \
ekf_aw.reset = false; \
ekf_aw_reset(); \
ekf_aw_reset_health(); \
}

#define ekf_aw_wrapper_set_wind_N(_v) { \
ekf_aw.wind_guess.x = _v; \
ekf_aw_set_wind(&ekf_aw.wind_guess); \
}

#define ekf_aw_wrapper_set_wind_E(_v) { \
ekf_aw.wind_guess.y = _v; \
ekf_aw_set_wind(&ekf_aw.wind_guess); \
}

#define ekf_aw_wrapper_set_wind_D(_v) { \
ekf_aw.wind_guess.z = _v; \
ekf_aw_set_wind(&ekf_aw.wind_guess); \
}

#define ekf_aw_wrapper_set_offset_x(_v) { \
ekf_aw.offset_guess.x = _v; \
ekf_aw_set_offset(&ekf_aw.offset_guess); \
}

#define ekf_aw_wrapper_set_offset_y(_v) { \
ekf_aw.offset_guess.y = _v; \
ekf_aw_set_offset(&ekf_aw.offset_guess); \
}

#define ekf_aw_wrapper_set_offset_z(_v) { \
ekf_aw.offset_guess.z = _v; \
ekf_aw_set_offset(&ekf_aw.offset_guess); \
}

#endif /* EKF_AW_WRAPPER_H */
2 changes: 1 addition & 1 deletion sw/airborne/modules/sensors/baro_MS5534A.c
View file
Expand Up @@ -218,7 +218,7 @@ static void calibration(void)
#ifndef BARO_NO_DOWNLINK
float debug[4];
for (int i=0; i<4; i++){
debug[0] = words[0];
debug[i] = words[i];
}
DOWNLINK_SEND_DEBUG_VECT(DefaultChannel, DefaultDevice, "baro_calib", 4, debug);
#endif
Expand Down