dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Rename mag ambiguous mag abbreviations with magnitude

This commit is contained in:
kamilritz 2019-12-25 17:50:34 +01:00 committed by Paul Riseborough
parent a24aaad861
commit 1c68709949
5 changed files with 15 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
EKF/control.cpp
View File

@ -415,7 +415,7 @@ void Ekf::controlOpticalFlowFusion()
const bool motion_is_excessive = ((accel_norm > (CONSTANTS_ONE_G * 1.5f)) // upper g limit
|| (accel_norm < (CONSTANTS_ONE_G * 0.5f)) // lower g limit
|| (_ang_rate_mag_filt > _flow_max_rate) // angular rate exceeds flow sensor limit
|| (_ang_rate_magnitude_filt > _flow_max_rate) // angular rate exceeds flow sensor limit
|| (_R_to_earth(2,2) < cosf(math::radians(30.0f)))); // tilted excessively
if (motion_is_excessive) {
@ -1136,7 +1136,7 @@ void Ekf::controlHeightFusion()
_hgt_sensor_offset = 0.0f;
}
}
// TODO: Add EV normal case here
// determine if we should use the vertical position observation
if (_control_status.flags.ev_hgt) {
_fuse_height = true;

8
EKF/covariance.cpp
View File

@ -161,12 +161,12 @@ void Ekf::predictCovariance()
// inhibit learning of imu accel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected
float alpha = math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f);
float beta = 1.0f - alpha;
_ang_rate_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_ang.norm(), beta * _ang_rate_mag_filt);
_accel_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_vel.norm(), beta * _accel_mag_filt);
_ang_rate_magnitude_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_ang.norm(), beta * _ang_rate_magnitude_filt);
_accel_magnitude_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_vel.norm(), beta * _accel_magnitude_filt);
_accel_vec_filt = alpha * dt_inv * _imu_sample_delayed.delta_vel + beta * _accel_vec_filt;
if (_ang_rate_mag_filt > _params.acc_bias_learn_gyr_lim
|| _accel_mag_filt > _params.acc_bias_learn_acc_lim
if (_ang_rate_magnitude_filt > _params.acc_bias_learn_gyr_lim
|| _accel_magnitude_filt > _params.acc_bias_learn_acc_lim
|| _bad_vert_accel_detected) {
// store the bias state variances to be reinstated later

4
EKF/ekf.cpp
View File

@ -95,8 +95,8 @@ void Ekf::reset(uint64_t timestamp)
_fault_status.value = 0;
_innov_check_fail_status.value = 0;
_accel_mag_filt = 0.0f;
_ang_rate_mag_filt = 0.0f;
_accel_magnitude_filt = 0.0f;
_ang_rate_magnitude_filt = 0.0f;
_prev_dvel_bias_var.zero();
}

10
EKF/ekf.h
View File

@ -336,11 +336,9 @@ private:
bool _tas_data_ready{false}; ///< true when new true airspeed data has fallen behind the fusion time horizon and is available to be fused
bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator
uint64_t _time_last_fake_pos{0}; ///< last time we faked position measurements to constrain tilt errors during operation without external aiding (uSec)
uint64_t _time_ins_deadreckon_start{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec)
bool _using_synthetic_position{false}; ///< true if we are using a synthetic position to constrain drift
// TODO: Split those into sensor and measurement specifics
uint64_t _time_last_hor_pos_fuse{0}; ///< time the last fusion of horizontal position measurements was performed (uSec)
uint64_t _time_last_hgt_fuse{0}; ///< time the last fusion of vertical position measurements was performed (uSec)
uint64_t _time_last_hor_vel_fuse{0}; ///< time the last fusion of horizontal velocity measurements was performed (uSec)
@ -350,6 +348,9 @@ private:
uint64_t _time_last_arsp_fuse{0}; ///< time the last fusion of airspeed measurements were performed (uSec)
uint64_t _time_last_beta_fuse{0}; ///< time the last fusion of synthetic sideslip measurements were performed (uSec)
uint64_t _time_last_rng_ready{0}; ///< time the last range finder measurement was ready (uSec)
uint64_t _time_last_mag{0}; ///< measurement time of last magnetomter sample (uSec)
uint64_t _time_last_fake_pos{0}; ///< last time we faked position measurements to constrain tilt errors during operation without external aiding (uSec)
Vector2f _last_known_posNE; ///< last known local NE position vector (m)
float _imu_collection_time_adj{0.0f}; ///< the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec)
@ -461,7 +462,6 @@ private:
bool _is_first_imu_sample{true};
uint32_t _baro_counter{0}; ///< number of baro samples read during initialisation
uint32_t _mag_counter{0}; ///< number of magnetometer samples read during initialisation
uint64_t _time_last_mag{0}; ///< measurement time of last magnetomter sample (uSec)
AlphaFilterVector3f _mag_lpf; ///< filtered magnetometer measurement for instant reset(Gauss)
AlphaFilterVector3f _accel_lpf; ///< filtered accelerometer measurement for instant reset(Gauss)
float _hgt_sensor_offset{0.0f}; ///< set as necessary if desired to maintain the same height after a height reset (m)
@ -481,8 +481,8 @@ private:
// variables used to inhibit accel bias learning
bool _accel_bias_inhibit{false}; ///< true when the accel bias learning is being inhibited
Vector3f _accel_vec_filt; ///< acceleration vector after application of a low pass filter (m/sec**2)
float _accel_mag_filt{0.0f}; ///< acceleration magnitude after application of a decaying envelope filter (rad/sec)
float _ang_rate_mag_filt{0.0f}; ///< angular rate magnitude after application of a decaying envelope filter (rad/sec)
float _accel_magnitude_filt{0.0f}; ///< acceleration magnitude after application of a decaying envelope filter (rad/sec)
float _ang_rate_magnitude_filt{0.0f}; ///< angular rate magnitude after application of a decaying envelope filter (rad/sec)
Vector3f _prev_dvel_bias_var; ///< saved delta velocity XYZ bias variances (m/sec)**2
// Terrain height state estimation