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

EKF angle constants in degrees for readability (#465)

Browse Source 
* EKF angle constants in degrees for readability

* EKF make FILTER_UPDATE_PERIOD_MS static constexpr and add FILTER_UPDATE_PERIOD_S

* EKF controlOpticalFlowFusion() use constants and update comments

* EKF controlMagFusion() use angle in degrees

* EKF move earth spin rate to geo and update usage

* EKF: Fix numerical constant error and clean up comments

Comments do not need to contain numerical values when the code makes these clear.
This commit is contained in:
Daniel Agar
2018-07-05 08:55:22 +10:00
committed by Paul Riseborough
parent cebdc3d829
commit 41953ab582
7 changed files with 29 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
+4 -4
View File
@@ -85,7 +85,7 @@ bool Ekf::init(uint64_t timestamp)
_control_status.value = 0;
_control_status_prev.value = 0;
_dt_ekf_avg = 0.001f * (float)(FILTER_UPDATE_PERIOD_MS);
_dt_ekf_avg = FILTER_UPDATE_PERIOD_S;
_fault_status.value = 0;
_innov_check_fail_status.value = 0;
@@ -307,7 +307,7 @@ void Ekf::predictState()
{
if (!_earth_rate_initialised) {
if (_NED_origin_initialised) {
calcEarthRateNED(_earth_rate_NED, _pos_ref.lat_rad);
calcEarthRateNED(_earth_rate_NED, (float)_pos_ref.lat_rad);
_earth_rate_initialised = true;
}
}
@@ -359,7 +359,7 @@ void Ekf::predictState()
float input = 0.5f * (_imu_sample_delayed.delta_vel_dt + _imu_sample_delayed.delta_ang_dt);
// filter and limit input between -50% and +100% of nominal value
input = math::constrain(input, 0.0005f * (float)(FILTER_UPDATE_PERIOD_MS), 0.002f * (float)(FILTER_UPDATE_PERIOD_MS));
input = math::constrain(input, 0.5f * FILTER_UPDATE_PERIOD_S, 2.0f * FILTER_UPDATE_PERIOD_S);
_dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * input;
}
@@ -391,7 +391,7 @@ bool Ekf::collect_imu(const imuSample &imu)
// if the target time delta between filter prediction steps has been exceeded
// write the accumulated IMU data to the ring buffer
const float target_dt = FILTER_UPDATE_PERIOD_MS / 1000.0f;
const float target_dt = FILTER_UPDATE_PERIOD_S;
if (_imu_down_sampled.delta_ang_dt >= target_dt - _imu_collection_time_adj) {