Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-05 06:03:02 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

ekf_att_pos_estimator: added logic for airspeed modes

Browse Source 
Signed-off-by: Roman <bapstr@ethz.ch>
This commit is contained in:
Roman
2016-06-23 09:07:46 +02:00
parent c1ba7ab62b
commit 6f6ae78cf2
2 changed files with 22 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+2
View File
@@ -255,6 +255,7 @@ private:
float magb_pnoise; float magb_pnoise;
float eas_noise; float eas_noise;
float pos_stddev_threshold; float pos_stddev_threshold;
int32_t airspeed_mode;
} _parameters; /**< local copies of interesting parameters */ } _parameters; /**< local copies of interesting parameters */
struct { struct {
@@ -276,6 +277,7 @@ private:
param_t magb_pnoise; param_t magb_pnoise;
param_t eas_noise; param_t eas_noise;
param_t pos_stddev_threshold; param_t pos_stddev_threshold;
param_t airspeed_mode;
} _parameter_handles; /**< handles for interesting parameters */ } _parameter_handles; /**< handles for interesting parameters */
AttPosEKF *_ekf; AttPosEKF *_ekf;
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+16 -6
View File
@@ -257,6 +257,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_parameter_handles.magb_pnoise = param_find("PE_MAGB_PNOISE"); _parameter_handles.magb_pnoise = param_find("PE_MAGB_PNOISE");
_parameter_handles.eas_noise = param_find("PE_EAS_NOISE"); _parameter_handles.eas_noise = param_find("PE_EAS_NOISE");
_parameter_handles.pos_stddev_threshold = param_find("PE_POSDEV_INIT"); _parameter_handles.pos_stddev_threshold = param_find("PE_POSDEV_INIT");
_parameter_handles.airspeed_mode = param_find("FW_AIRSPD_MODE");
/* indicate consumers that the current position data is not valid */ /* indicate consumers that the current position data is not valid */
_gps.eph = 10000.0f; _gps.eph = 10000.0f;
@@ -324,6 +325,7 @@ int AttitudePositionEstimatorEKF::parameters_update()
param_get(_parameter_handles.magb_pnoise, &(_parameters.magb_pnoise)); param_get(_parameter_handles.magb_pnoise, &(_parameters.magb_pnoise));
param_get(_parameter_handles.eas_noise, &(_parameters.eas_noise)); param_get(_parameter_handles.eas_noise, &(_parameters.eas_noise));
param_get(_parameter_handles.pos_stddev_threshold, &(_parameters.pos_stddev_threshold)); param_get(_parameter_handles.pos_stddev_threshold, &(_parameters.pos_stddev_threshold));
param_get(_parameter_handles.airspeed_mode, &_parameters.airspeed_mode);
if (_ekf) { if (_ekf) {
// _ekf->yawVarScale = 1.0f; // _ekf->yawVarScale = 1.0f;
@@ -917,17 +919,25 @@ void AttitudePositionEstimatorEKF::publishControlState()
_ctrl_state.q[2] = _ekf->states[2]; _ctrl_state.q[2] = _ekf->states[2];
_ctrl_state.q[3] = _ekf->states[3]; _ctrl_state.q[3] = _ekf->states[3];
/* Airspeed (Groundspeed - Windspeed) */ // use estimated velocity for airspeed estimate
//_ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2)); if (_parameters.airspeed_mode == control_state_s::AIRSPD_MODE_MEAS) {
// the line above was introduced by the control state PR. The airspeed it gives is totally wrong and leads to horrible flight performance in SITL // use measured airspeed
// and in outdoor tests
if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6 if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6
&& _airspeed.timestamp > 0) { && _airspeed.timestamp > 0) {
_ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s; _ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s;
_ctrl_state.airspeed_valid = true; _ctrl_state.airspeed_valid = true;
}
} else { } else if (_parameters.airspeed_mode == control_state_s::AIRSPD_MODE_EST) {
_ctrl_state.airspeed_valid = false; if (_local_pos.v_xy_valid && _local_pos.v_z_valid) {
_ctrl_state.airspeed = sqrtf(_ekf->states[4] * _ekf->states[4]
+ _ekf->states[5] * _ekf->states[5] + _ekf->states[6] * _ekf->states[6]);
_ctrl_state.airspeed_valid = true;
}
} else if (_parameters.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED) {
// do nothing, airspeed has been declared as non-valid above, controllers
// will handle this assuming always trim airspeed
} }
/* Attitude Rates */ /* Attitude Rates */