fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 15:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

adopted ekf2_main.cpp

Browse Source
This commit is contained in:
CarlOlsson
2016-03-30 17:33:37 +02:00
committed by Lorenz Meier
parent c821f41631
commit 3af01a8c5e
1 changed files with 5 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+5 -1
View File
@@ -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);