added static gps mode and init mag state correctly

EKF/ekf.cpp

@@ -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;
 

EKF/ekf.h

@@ -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;

EKF/estimator_base.cpp

@@ -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;
 	}