From 3af01a8c5eba8e8c78988b940c29d5646c153f6f Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Mon, 14 Mar 2016 18:26:19 +0100 Subject: [PATCH] adopted ekf2_main.cpp --- src/modules/ekf2/ekf2_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index c05998ff3b..5c1dc10322 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -185,6 +185,7 @@ private: control::BlockParamFloat *_mag_heading_noise; // measurement noise used for simple heading fusion control::BlockParamFloat *_mag_noise; // measurement noise used for 3-axis magnetoemter fusion (Gauss) + control::BlockParamFloat *_eas_noise; // measurement noise used for airspeed fusion (std m/s) control::BlockParamFloat *_mag_declination_deg; // magnetic declination in degrees control::BlockParamFloat *_heading_innov_gate; // innovation gate for heading innovation test control::BlockParamFloat *_mag_innov_gate; // innovation gate for magnetometer innovation test @@ -259,6 +260,7 @@ Ekf2::Ekf2(): _vel_innov_gate(new control::BlockParamFloat(this, "EKF2_GPS_V_GATE", false, &_params->vel_innov_gate)), _mag_heading_noise(new control::BlockParamFloat(this, "EKF2_HEAD_NOISE", false, &_params->mag_heading_noise)), _mag_noise(new control::BlockParamFloat(this, "EKF2_MAG_NOISE", false, &_params->mag_noise)), + _eas_noise(new control::BlockParamFloat(this, "EKF2_EAS_NOISE", false, &_params->eas_noise)), _mag_declination_deg(new control::BlockParamFloat(this, "EKF2_MAG_DECL", false, &_params->mag_declination_deg)), _heading_innov_gate(new control::BlockParamFloat(this, "EKF2_HDG_GATE", false, &_params->heading_innov_gate)), _mag_innov_gate(new control::BlockParamFloat(this, "EKF2_MAG_GATE", false, &_params->mag_innov_gate)), @@ -433,7 +435,7 @@ void Ekf2::task_main() // read airspeed data if available if (airspeed_updated) { - _ekf->setAirspeedData(airspeed.timestamp, &airspeed.indicated_airspeed_m_s); + _ekf->setAirspeedData(airspeed.timestamp, &airspeed.true_airspeed_m_s); // Only TAS is now fed into the estimator } if (optical_flow_updated) { @@ -636,12 +638,14 @@ void Ekf2::task_main() _ekf->get_vel_pos_innov(&innovations.vel_pos_innov[0]); _ekf->get_mag_innov(&innovations.mag_innov[0]); _ekf->get_heading_innov(&innovations.heading_innov); + _ekf->get_airspeed_innov(&innovations.airspeed_innov); _ekf->get_flow_innov(&innovations.flow_innov[0]); _ekf->get_hagl_innov(&innovations.hagl_innov); _ekf->get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]); _ekf->get_mag_innov_var(&innovations.mag_innov_var[0]); _ekf->get_heading_innov_var(&innovations.heading_innov_var); + _ekf->get_airspeed_innov_var(&innovations.airspeed_innov_var); _ekf->get_flow_innov_var(&innovations.flow_innov_var[0]); _ekf->get_hagl_innov_var(&innovations.hagl_innov_var);