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

added static gps mode and init mag state correctly

This commit is contained in:
Roman 2015-12-22 11:22:17 +01:00
parent f153a7cb44
commit 5e5d6f432a
3 changed files with 14 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
EKF/ekf.cpp
View File

@ -48,7 +48,8 @@ Ekf::Ekf():
_fuse_height(false),
_fuse_pos(false),
_fuse_vel(false),
_mag_fuse_index(0)
_mag_fuse_index(0),
_time_last_fake_gps(0)
{
_earth_rate_NED.setZero();
_R_prev = matrix::Dcm<float>();
@ -96,6 +97,9 @@ bool Ekf::update()
if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) {
_fuse_pos = true;
_fuse_vel = true;
} else if (_time_last_imu - _time_last_gps > 2000000 && _time_last_imu - _time_last_fake_gps > 70000) {
_fuse_vel = true;
_gps_sample_delayed.vel.setZero();
}
@ -142,14 +146,17 @@ bool Ekf::initialiseFilter(void)
roll = -asinf(accel_init(1) / cosf(pitch));
}
matrix::Euler<float> euler_init(roll, pitch, 0.0f);
_state.quat_nominal = Quaternion(euler_init);
magSample mag_init = _mag_buffer.get_newest();
if (mag_init.time_us == 0) {
return false;
}
float yaw_init = atan2f(mag_init.mag(1), mag_init.mag(0));
matrix::Euler<float> euler_init(roll, pitch, yaw_init);
_state.quat_nominal = Quaternion(euler_init);
matrix::Dcm<float> R_to_earth(euler_init);
_state.mag_I = R_to_earth * mag_init.mag;

2
EKF/ekf.h
View File

@ -66,6 +66,8 @@ private:
uint8_t _mag_fuse_index; // counter for sequential mag axis fusion
uint64_t _time_last_fake_gps;
Vector3f _earth_rate_NED;
matrix::Dcm<float> _R_prev;

2
EKF/estimator_base.cpp
View File

@ -69,7 +69,7 @@ void EstimatorBase::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64
_time_last_imu = time_usec;
if (_imu_time_last > 0) {
if (_time_last_imu > 0) {
_dt_imu_avg = 0.8f * _dt_imu_avg + 0.2f * dt;
}