From c75cc75d25060ec86eb1d013b76e4bc78d3d705c Mon Sep 17 00:00:00 2001
From: Roman <bapstr@ethz.ch>
Date: Sun, 31 Jan 2016 22:28:19 +0100
Subject: [PATCH] ekf2 module: be consistent in constructor
---
src/modules/ekf2/ekf2_main.cpp | 75 ++++++++++++++++------------------
1 file changed, 36 insertions(+), 39 deletions(-)
diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index aaf8d93e5e..8e6d17da5f 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -138,12 +138,15 @@ private:
orb_advert_t _estimator_status_pub;
orb_advert_t _estimator_innovations_pub;
-
/* Low pass filter for attitude rates */
math::LowPassFilter2p _lp_roll_rate;
math::LowPassFilter2p _lp_pitch_rate;
math::LowPassFilter2p _lp_yaw_rate;
+ EstimatorInterface *_ekf;
+
+ parameters *_params; // pointer to ekf parameter struct (located in _ekf class instance)
+
control::BlockParamFloat *_mag_delay_ms;
control::BlockParamFloat *_baro_delay_ms;
control::BlockParamFloat *_gps_delay_ms;
@@ -180,8 +183,6 @@ private:
control::BlockParamFloat *_requiredHdrift; // maximum acceptable horizontal drift speed (m/s)
control::BlockParamFloat *_requiredVdrift; // maximum acceptable vertical drift speed (m/s)
- EstimatorInterface *_ekf;
-
int update_subscriptions();
};
@@ -197,43 +198,39 @@ Ekf2::Ekf2():
_lp_roll_rate(250.0f, 30.0f),
_lp_pitch_rate(250.0f, 30.0f),
_lp_yaw_rate(250.0f, 20.0f),
- _ekf(new Ekf())
+ _ekf(new Ekf()),
+ _params(_ekf->getParamHandle()),
+ _mag_delay_ms(new control::BlockParamFloat(this, "EKF2_MAG_DELAY", false, &_params->mag_delay_ms)),
+ _baro_delay_ms(new control::BlockParamFloat(this, "EKF2_BARO_DELAY", false, &_params->baro_delay_ms)),
+ _gps_delay_ms(new control::BlockParamFloat(this, "EKF2_GPS_DELAY", false, &_params->gps_delay_ms)),
+ _airspeed_delay_ms(new control::BlockParamFloat(this, "EKF2_ASP_DELAY", false, &_params->airspeed_delay_ms)),
+ _gyro_noise(new control::BlockParamFloat(this, "EKF2_G_NOISE", false, &_params->gyro_noise)),
+ _accel_noise(new control::BlockParamFloat(this, "EKF2_ACC_NOISE", false, &_params->accel_noise)),
+ _gyro_bias_p_noise(new control::BlockParamFloat(this, "EKF2_GB_NOISE", false, &_params->gyro_bias_p_noise)),
+ _accel_bias_p_noise(new control::BlockParamFloat(this, "EKF2_ACCB_NOISE", false, &_params->accel_bias_p_noise)),
+ _gyro_scale_p_noise(new control::BlockParamFloat(this, "EKF2_GS_NOISE", false, &_params->gyro_scale_p_noise)),
+ _mag_p_noise(new control::BlockParamFloat(this, "EKF2_MAG_NOISE", false, &_params->mag_p_noise)),
+ _wind_vel_p_noise(new control::BlockParamFloat(this, "EKF2_WIND_NOISE", false, &_params->wind_vel_p_noise)),
+ _gps_vel_noise(new control::BlockParamFloat(this, "EKF2_GPS_V_NOISE", false, &_params->gps_vel_noise)),
+ _gps_pos_noise(new control::BlockParamFloat(this, "EKF2_GPS_P_NOISE", false, &_params->gps_pos_noise)),
+ _baro_noise(new control::BlockParamFloat(this, "EKF2_BARO_NOISE", false, &_params->baro_noise)),
+ _baro_innov_gate(new control::BlockParamFloat(this, "EKF2_BARO_GATE", false, &_params->baro_innov_gate)),
+ _posNE_innov_gate(new control::BlockParamFloat(this, "EKF2_GPS_P_GATE", false, &_params->posNE_innov_gate)),
+ _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_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)),
+ _gps_check_mask(new control::BlockParamInt(this, "EKF2_GPS_CHECK", false, &_params->gps_check_mask)),
+ _requiredEph(new control::BlockParamFloat(this, "EKF2_REQ_EPH", false, &_params->req_hacc)),
+ _requiredEpv(new control::BlockParamFloat(this, "EKF2_REQ_EPV", false, &_params->req_vacc)),
+ _requiredSacc(new control::BlockParamFloat(this, "EKF2_REQ_SACC", false, &_params->req_sacc)),
+ _requiredNsats(new control::BlockParamInt(this, "EKF2_REQ_NSATS", false, &_params->req_nsats)),
+ _requiredGDoP(new control::BlockParamFloat(this, "EKF2_REQ_GDOP", false, &_params->req_gdop)),
+ _requiredHdrift(new control::BlockParamFloat(this, "EKF2_REQ_HDRIFT", false, &_params->req_hdrift)),
+ _requiredVdrift(new control::BlockParamFloat(this, "EKF2_REQ_VDRIFT", false, &_params->req_vdrift))
+
{
- parameters *params = _ekf->getParamHandle();
- _mag_delay_ms = new control::BlockParamFloat(this, "EKF2_MAG_DELAY", false, ¶ms->mag_delay_ms);
- _baro_delay_ms = new control::BlockParamFloat(this, "EKF2_BARO_DELAY", false, ¶ms->baro_delay_ms);
- _gps_delay_ms = new control::BlockParamFloat(this, "EKF2_GPS_DELAY", false, ¶ms->gps_delay_ms);
- _airspeed_delay_ms = new control::BlockParamFloat(this, "EKF2_ASP_DELAY", false, ¶ms->airspeed_delay_ms);
-
- _gyro_noise = new control::BlockParamFloat(this, "EKF2_G_NOISE", false, ¶ms->gyro_noise);
- _accel_noise = new control::BlockParamFloat(this, "EKF2_ACC_NOISE", false, ¶ms->accel_noise);
-
- _gyro_bias_p_noise = new control::BlockParamFloat(this, "EKF2_GB_NOISE", false, ¶ms->gyro_bias_p_noise);
- _accel_bias_p_noise = new control::BlockParamFloat(this, "EKF2_ACCB_NOISE", false, ¶ms->accel_bias_p_noise);
- _gyro_scale_p_noise = new control::BlockParamFloat(this, "EKF2_GS_NOISE", false, ¶ms->gyro_scale_p_noise);
- _mag_p_noise = new control::BlockParamFloat(this, "EKF2_MAG_NOISE", false, ¶ms->mag_p_noise);
- _wind_vel_p_noise = new control::BlockParamFloat(this, "EKF2_WIND_NOISE", false, ¶ms->wind_vel_p_noise);
-
- _gps_vel_noise = new control::BlockParamFloat(this, "EKF2_GPS_V_NOISE", false, ¶ms->gps_vel_noise);
- _gps_pos_noise = new control::BlockParamFloat(this, "EKF2_GPS_P_NOISE", false, ¶ms->gps_pos_noise);
- _baro_noise = new control::BlockParamFloat(this, "EKF2_BARO_NOISE", false, ¶ms->baro_noise);
- _baro_innov_gate = new control::BlockParamFloat(this, "EKF2_BARO_GATE", false, ¶ms->baro_innov_gate);
- _posNE_innov_gate = new control::BlockParamFloat(this, "EKF2_GPS_P_GATE", false, ¶ms->posNE_innov_gate);
- _vel_innov_gate = new control::BlockParamFloat(this, "EKF2_GPS_V_GATE", false, ¶ms->vel_innov_gate);
-
- _mag_heading_noise = new control::BlockParamFloat(this, "EKF2_HEAD_NOISE", false, ¶ms->mag_heading_noise);
- _mag_declination_deg = new control::BlockParamFloat(this, "EKF2_MAG_DECL", false, ¶ms->mag_declination_deg);
- _heading_innov_gate = new control::BlockParamFloat(this, "EKF2_HDG_GATE", false, ¶ms->heading_innov_gate);
- _mag_innov_gate = new control::BlockParamFloat(this, "EKF2_MAG_GATE", false, ¶ms->mag_innov_gate);
-
- _gps_check_mask = new control::BlockParamInt(this, "EKF2_GPS_CHECK", false, ¶ms->gps_check_mask);
- _requiredEph = new control::BlockParamFloat(this, "EKF2_REQ_EPH", false, ¶ms->req_hacc);
- _requiredEpv = new control::BlockParamFloat(this, "EKF2_REQ_EPV", false, ¶ms->req_vacc);
- _requiredSacc = new control::BlockParamFloat(this, "EKF2_REQ_SACC", false, ¶ms->req_sacc);
- _requiredNsats = new control::BlockParamInt(this, "EKF2_REQ_NSATS", false, ¶ms->req_nsats);
- _requiredGDoP = new control::BlockParamFloat(this, "EKF2_REQ_GDOP", false, ¶ms->req_gdop);
- _requiredHdrift = new control::BlockParamFloat(this, "EKF2_REQ_HDRIFT", false, ¶ms->req_hdrift);
- _requiredVdrift = new control::BlockParamFloat(this, "EKF2_REQ_VDRIFT", false, ¶ms->req_vdrift);
}
--
GitLab