diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-10 23:14:57 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-10 23:14:57 +0200 |
commit | 5602d5dfa3033b0364a80d10aff794f2a92d62c9 (patch) | |
tree | bb6684a1978bcb4c85b0cb617ac81fdd33e17c55 /src/modules/ekf_att_pos_estimator/estimator_23states.h | |
parent | 2219dd3fc6586e7da8a1bc7b7514b97261ff8b23 (diff) | |
download | px4-firmware-5602d5dfa3033b0364a80d10aff794f2a92d62c9.tar.gz px4-firmware-5602d5dfa3033b0364a80d10aff794f2a92d62c9.tar.bz2 px4-firmware-5602d5dfa3033b0364a80d10aff794f2a92d62c9.zip |
New gyro offset based divergence detection and protection. Pending flight tests
Diffstat (limited to 'src/modules/ekf_att_pos_estimator/estimator_23states.h')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_23states.h | 13 |
1 files changed, 11 insertions, 2 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index 851e46371..32c514004 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -72,7 +72,7 @@ public: accelProcessNoise = 0.5f; } - struct { + struct mag_state_struct { unsigned obsIndex; float MagPred[3]; float SH_MAG[9]; @@ -88,7 +88,12 @@ public: float magZbias; float R_MAG; Mat3f DCM; - } magstate; + }; + + struct mag_state_struct magstate; + struct mag_state_struct resetMagState; + + // Global variables @@ -97,6 +102,7 @@ public: float P[n_states][n_states]; // covariance matrix float Kfusion[n_states]; // Kalman gains float states[n_states]; // state matrix + float resetStates[n_states]; float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored @@ -114,6 +120,7 @@ public: float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) + Vector3f lastGyroOffset; // Last gyro offset Mat3f Tbn; // transformation matrix from body to NED coordinates Mat3f Tnb; // transformation amtrix from NED to body coordinates @@ -272,6 +279,8 @@ protected: bool FilterHealthy(); +bool GyroOffsetsDiverged(); + void ResetHeight(void); void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); |