Merge pull request #3 from PX4/paul-wip

Enable use of POSCTL mode with EKF2

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>();
@@ -84,9 +85,9 @@ bool Ekf::update()
 	// measurement updates
 
 	if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
-		//fuseHeading();
-		fuseMag(_mag_fuse_index);
-		_mag_fuse_index = (_mag_fuse_index + 1) % 3;
+		fuseHeading();
+		//fuseMag(_mag_fuse_index);
+		//_mag_fuse_index = (_mag_fuse_index + 1) % 3;
 	}
 
 	if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
@@ -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;
 	}
 
@@ -170,6 +170,8 @@ void EstimatorBase::setGpsData(uint64_t time_usec, struct gps_message *gps)
 		gps_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
 		_time_last_gps = gps_sample_new.time_us;
 
+		_last_valid_gps_time_us = hrt_absolute_time();
+
 		gps_sample_new.time_us = math::max(gps_sample_new.time_us, _imu_sample_delayed.time_us);
 
 
@@ -261,8 +263,8 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec)
 	_params.baro_delay_ms = 0;
 	_params.gps_delay_ms = 200;
 	_params.airspeed_delay_ms = 0;
-	_params.requiredEph = 200;
-	_params.requiredEpv = 200;
+	_params.requiredEph = 500;
+	_params.requiredEpv = 800;
 	_params.gyro_noise = 1e-3f;
 	_params.accel_noise = 1e-1f;
 	_params.gyro_bias_p_noise = 1e-5f;
@@ -311,7 +313,7 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec)
 	_gps_speed_valid = false;
 
 	_mag_healthy = false;
-	_in_air = true;			// XXX get this flag from the application
+	_in_air = false;			// XXX get this flag from the application
 
 	_time_last_imu = 0;
 	_time_last_gps = 0;
@@ -335,20 +337,38 @@ void EstimatorBase::initialiseGPS(struct gps_message *gps)
 		map_projection_init(&_posRef, lat, lon);
 		_gps_alt_ref = gps->alt / 1e3f;
 		_gps_initialised = true;
+		_last_gps_origin_time_us = hrt_absolute_time();
 	}
 }
 
 bool EstimatorBase::gps_is_good(struct gps_message *gps)
 {
 	// go through apm implementation of calcGpsGoodToAlign for fancier checks
-	if ((gps->fix_type >= 3) && (gps->eph < _params.requiredEph) && (gps->epv < _params.requiredEpv)) {
-		return true;
+	// Use a stricter check for initialisation than during flight to avoid complete loss of GPS
+	if (_gps_initialised) {
+		if ((gps->fix_type >= 3) && (gps->eph < _params.requiredEph * 2) && (gps->epv < _params.requiredEpv * 2)) {
+			return true;
 
+		} else {
+			return false;
+		}
 	} else {
-		return false;
+		if ((gps->fix_type >= 3) && (gps->eph < _params.requiredEph) && (gps->epv < _params.requiredEpv)) {
+			return true;
+
+		} else {
+			return false;
+		}
 	}
 }
 
+bool EstimatorBase::position_is_valid()
+{
+	// return true if the position estimate is valid
+	// TOTO implement proper check based on published GPS accuracy, innovaton consistency checks and timeout status
+	return _gps_initialised &&  (hrt_absolute_time() - _last_valid_gps_time_us) < 5e6;
+}
+
 void EstimatorBase::printStoredIMU()
 {
 	printf("---------Printing IMU data buffer------------\n");

EKF/estimator_base.h

@@ -190,6 +190,8 @@ protected:
 	float _dt_imu_avg;
 	uint64_t _imu_time_last;
 
+	uint64_t _last_valid_gps_time_us;
+
 	imuSample _imu_sample_delayed;
 	imuSample _imu_down_sampled;
 	Quaternion
@@ -206,9 +208,6 @@ protected:
 	outputSample _output_new;
 	imuSample _imu_sample_new;
 
-	struct map_projection_reference_s _posRef;
-	float _gps_alt_ref;
-
 
 	uint64_t _imu_ticks;
 
@@ -266,6 +265,8 @@ public:
 	void printGps(struct gpsSample *data);
 	void printStoredGps();
 
+	bool position_is_valid();
+
 	void copy_quaternion(float *quat) {
 		for (unsigned i = 0; i < 4; i++) {
 			quat[i] = _output_new.quat_nominal(i);
@@ -284,4 +285,8 @@ public:
 	void copy_timestamp(uint64_t *time_us) {
 		*time_us = _imu_time_last;
 	}
+
+	uint64_t _last_gps_origin_time_us;
+	struct map_projection_reference_s _posRef;
+	float _gps_alt_ref;
 };