fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-03 01:00:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2 module: be consistent in constructor

Browse Source
This commit is contained in:
Roman
2016-01-31 22:28:19 +01:00
parent 904319f889
commit c75cc75d25
1 changed files with 36 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+36 -39
View File
@@ -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, &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);
}