Reworked the estimator initialization and recovery logic. Should be more resilient to mishaps now

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -577,6 +577,11 @@ FixedwingEstimator::task_main()
 	bool newAdsData = false;
 	bool newDataMag = false;
 
+	float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m)
+	_gps.vel_n_m_s = 0.0f;
+	_gps.vel_e_m_s = 0.0f;
+	_gps.vel_d_m_s = 0.0f;
+
 	while (!_task_should_exit) {
 
 		/* wait for up to 500ms for data */
@@ -926,8 +931,15 @@ FixedwingEstimator::task_main()
 				newDataMag = false;
 			}
 
+			/*
+			 *    CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY
+			 */
+			if (hrt_elapsed_time(&_filter_start_time) < FILTER_INIT_DELAY) {
+				continue;
+			}
 
-			/**
+
+			/*
 			 *    CHECK IF THE INPUT DATA IS SANE
 			 */
 			int check = _ekf->CheckAndBound();
@@ -959,6 +971,13 @@ FixedwingEstimator::task_main()
 					mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
 					break;
 				}
+				case 4:
+				{
+					const char* str = "excessive gyro offsets";
+					warnx("%s", str);
+					mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
+					break;
+				}
 
 				default:
 				{
@@ -974,7 +993,7 @@ FixedwingEstimator::task_main()
 			}
 
 			// If non-zero, we got a filter reset
-			if (check) {
+			if (check > 0 && check != 3) {
 
 				struct ekf_status_report ekf_report;
 
@@ -1013,10 +1032,12 @@ FixedwingEstimator::task_main()
 
 				_baro_init = false;
 				_gps_initialized = false;
+				_initialized = false;
 				last_sensor_timestamp = hrt_absolute_time();
 				last_run = last_sensor_timestamp;
 
 				_ekf->ZeroVariables();
+				_ekf->statesInitialised = false;
 				_ekf->dtIMU = 0.01f;
 
 				// Let the system re-initialize itself
@@ -1027,23 +1048,26 @@ FixedwingEstimator::task_main()
 
 			/**
 			 *    PART TWO: EXECUTE THE FILTER
+			 *
+			 *    We run the filter only once all data has been fetched
 			 **/
 
-			if ((hrt_elapsed_time(&_filter_start_time) > FILTER_INIT_DELAY) && _baro_init && _gyro_valid && _accel_valid && _mag_valid) {
+			if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) {
 
 				float initVelNED[3];
 
+				/* Initialize the filter first */
 				if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) {
 
-					initVelNED[0] = _gps.vel_n_m_s;
-					initVelNED[1] = _gps.vel_e_m_s;
-					initVelNED[2] = _gps.vel_d_m_s;
-
 					// GPS is in scaled integers, convert
 					double lat = _gps.lat / 1.0e7;
 					double lon = _gps.lon / 1.0e7;
 					float gps_alt = _gps.alt / 1e3f;
 
+					initVelNED[0] = _gps.vel_n_m_s;
+					initVelNED[1] = _gps.vel_e_m_s;
+					initVelNED[2] = _gps.vel_d_m_s;
+
 					// Set up height correctly
 					orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
 					_baro_gps_offset = _baro_ref - _baro.altitude;
@@ -1070,10 +1094,13 @@ FixedwingEstimator::task_main()
 
 					map_projection_init(&_pos_ref, lat, lon);
 					mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
+
+					#if 0
 					warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt,
 						(double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
 					warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_gps_offset);
 					warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m, (double)math::degrees(declination));
+					#endif
 
 					_gps_initialized = true;
 
@@ -1082,282 +1109,268 @@ FixedwingEstimator::task_main()
 					initVelNED[0] = 0.0f;
 					initVelNED[1] = 0.0f;
 					initVelNED[2] = 0.0f;
-					_ekf->posNED[0] = 0.0f;
-					_ekf->posNED[1] = 0.0f;
-					_ekf->posNED[2] = 0.0f;
-
-					_ekf->posNE[0] = _ekf->posNED[0];
-					_ekf->posNE[1] = _ekf->posNED[1];
+					_ekf->posNE[0] = posNED[0];
+					_ekf->posNE[1] = posNED[1];
 
 					_local_pos.ref_alt = _baro_ref;
 					_baro_gps_offset = 0.0f;
 
 					_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
-				}
-			}
-
-			// If valid IMU data and states initialised, predict states and covariances
-			if (_ekf->statesInitialised) {
-				// Run the strapdown INS equations every IMU update
-				_ekf->UpdateStrapdownEquationsNED();
-#if 0
-				// debug code - could be tunred into a filter mnitoring/watchdog function
-				float tempQuat[4];
+				} else if (_ekf->statesInitialised) {
 
-				for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j];
+					// We're apparently initialized in this case now
 
-				quat2eul(eulerEst, tempQuat);
 
-				for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j];
+					// Run the strapdown INS equations every IMU update
+					_ekf->UpdateStrapdownEquationsNED();
+	#if 0
+					// debug code - could be tunred into a filter mnitoring/watchdog function
+					float tempQuat[4];
 
-				if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi;
+					for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j];
 
-				if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi;
+					quat2eul(eulerEst, tempQuat);
 
-#endif
-				// store the predicted states for subsequent use by measurement fusion
-				_ekf->StoreStates(IMUmsec);
-				// Check if on ground - status is used by covariance prediction
-				_ekf->OnGroundCheck();
-				// sum delta angles and time used by covariance prediction
-				_ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng;
-				_ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU;
-				dt += _ekf->dtIMU;
-
-				// perform a covariance prediction if the total delta angle has exceeded the limit
-				// or the time limit will be exceeded at the next IMU update
-				if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) {
-					_ekf->CovariancePrediction(dt);
-					_ekf->summedDelAng.zero();
-					_ekf->summedDelVel.zero();
-					dt = 0.0f;
-				}
+					for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j];
 
-				_initialized = true;
-			}
+					if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi;
 
-			// Fuse GPS Measurements
-			if (newDataGps && _gps_initialized) {
-				// Convert GPS measurements to Pos NE, hgt and Vel NED
-				_ekf->velNED[0] = _gps.vel_n_m_s;
-				_ekf->velNED[1] = _gps.vel_e_m_s;
-				_ekf->velNED[2] = _gps.vel_d_m_s;
-				_ekf->calcposNED(_ekf->posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef);
-
-				_ekf->posNE[0] = _ekf->posNED[0];
-				_ekf->posNE[1] = _ekf->posNED[1];
-				// set fusion flags
-				_ekf->fuseVelData = true;
-				_ekf->fusePosData = true;
-				// recall states stored at time of measurement after adjusting for delays
-				_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
-				_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
-				// run the fusion step
-				_ekf->FuseVelposNED();
-
-			} else if (_ekf->statesInitialised) {
-				// Convert GPS measurements to Pos NE, hgt and Vel NED
-				_ekf->velNED[0] = 0.0f;
-				_ekf->velNED[1] = 0.0f;
-				_ekf->velNED[2] = 0.0f;
-				_ekf->posNED[0] = 0.0f;
-				_ekf->posNED[1] = 0.0f;
-				_ekf->posNED[2] = 0.0f;
-
-				_ekf->posNE[0] = _ekf->posNED[0];
-				_ekf->posNE[1] = _ekf->posNED[1];
-				// set fusion flags
-				_ekf->fuseVelData = true;
-				_ekf->fusePosData = true;
-				// recall states stored at time of measurement after adjusting for delays
-				_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
-				_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
-				// run the fusion step
-				_ekf->FuseVelposNED();
-
-			} else {
-				_ekf->fuseVelData = false;
-				_ekf->fusePosData = false;
-			}
-
-			if (newHgtData && _ekf->statesInitialised) {
-				// Could use a blend of GPS and baro alt data if desired
-				_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
-				_ekf->fuseHgtData = true;
-				// recall states stored at time of measurement after adjusting for delays
-				_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
-				// run the fusion step
-				_ekf->FuseVelposNED();
-
-			} else {
-				_ekf->fuseHgtData = false;
-			}
-
-			// Fuse Magnetometer Measurements
-			if (newDataMag && _ekf->statesInitialised) {
-				_ekf->fuseMagData = true;
-				_ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
-
-			} else {
-				_ekf->fuseMagData = false;
-			}
+					if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi;
 
-			if (_ekf->statesInitialised) _ekf->FuseMagnetometer();
-
-			// Fuse Airspeed Measurements
-			if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) {
-				_ekf->fuseVtasData = true;
-				_ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
-				_ekf->FuseAirspeed();
-
-			} else {
-				_ekf->fuseVtasData = false;
-			}
-
-			// Publish results
-			if (_initialized && (check == OK)) {
-
-
-
-				// State vector:
-				// 0-3: quaternions (q0, q1, q2, q3)
-				// 4-6: Velocity - m/sec (North, East, Down)
-				// 7-9: Position - m (North, East, Down)
-				// 10-12: Delta Angle bias - rad (X,Y,Z)
-				// 13-14: Wind Vector  - m/sec (North,East)
-				// 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down)
-				// 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z)
-
-				math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
-				math::Matrix<3, 3> R = q.to_dcm();
-				math::Vector<3> euler = R.to_euler();
-
-				for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
-						_att.R[i][j] = R(i, j);
-
-				_att.timestamp = last_sensor_timestamp;
-				_att.q[0] = _ekf->states[0];
-				_att.q[1] = _ekf->states[1];
-				_att.q[2] = _ekf->states[2];
-				_att.q[3] = _ekf->states[3];
-				_att.q_valid = true;
-				_att.R_valid = true;
-
-				_att.timestamp = last_sensor_timestamp;
-				_att.roll = euler(0);
-				_att.pitch = euler(1);
-				_att.yaw = euler(2);
-
-				_att.rollspeed = _ekf->angRate.x - _ekf->states[10];
-				_att.pitchspeed = _ekf->angRate.y - _ekf->states[11];
-				_att.yawspeed = _ekf->angRate.z - _ekf->states[12];
-				// gyro offsets
-				_att.rate_offsets[0] = _ekf->states[10];
-				_att.rate_offsets[1] = _ekf->states[11];
-				_att.rate_offsets[2] = _ekf->states[12];
-
-				/* lazily publish the attitude only once available */
-				if (_att_pub > 0) {
-					/* publish the attitude setpoint */
-					orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att);
-
-				} else {
-					/* advertise and publish */
-					_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att);
-				}
-			}
-
-			if (_gps_initialized) {
-				_local_pos.timestamp = last_sensor_timestamp;
-				_local_pos.x = _ekf->states[7];
-				_local_pos.y = _ekf->states[8];
-				// XXX need to announce change of Z reference somehow elegantly
-				_local_pos.z = _ekf->states[9] - _baro_gps_offset;
+	#endif
+					// store the predicted states for subsequent use by measurement fusion
+					_ekf->StoreStates(IMUmsec);
+					// Check if on ground - status is used by covariance prediction
+					_ekf->OnGroundCheck();
+					// sum delta angles and time used by covariance prediction
+					_ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng;
+					_ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU;
+					dt += _ekf->dtIMU;
+
+					// perform a covariance prediction if the total delta angle has exceeded the limit
+					// or the time limit will be exceeded at the next IMU update
+					if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) {
+						_ekf->CovariancePrediction(dt);
+						_ekf->summedDelAng.zero();
+						_ekf->summedDelVel.zero();
+						dt = 0.0f;
+					}
 
-				_local_pos.vx = _ekf->states[4];
-				_local_pos.vy = _ekf->states[5];
-				_local_pos.vz = _ekf->states[6];
+					_initialized = true;
+
+					// Fuse GPS Measurements
+					if (newDataGps && _gps_initialized) {
+						// Convert GPS measurements to Pos NE, hgt and Vel NED
+						_ekf->velNED[0] = _gps.vel_n_m_s;
+						_ekf->velNED[1] = _gps.vel_e_m_s;
+						_ekf->velNED[2] = _gps.vel_d_m_s;
+						_ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef);
+
+						_ekf->posNE[0] = posNED[0];
+						_ekf->posNE[1] = posNED[1];
+						// set fusion flags
+						_ekf->fuseVelData = true;
+						_ekf->fusePosData = true;
+						// recall states stored at time of measurement after adjusting for delays
+						_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
+						_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
+						// run the fusion step
+						_ekf->FuseVelposNED();
+
+					} else if (_ekf->statesInitialised) {
+						// Convert GPS measurements to Pos NE, hgt and Vel NED
+						_ekf->velNED[0] = 0.0f;
+						_ekf->velNED[1] = 0.0f;
+						_ekf->velNED[2] = 0.0f;
+
+						_ekf->posNE[0] = 0.0f;
+						_ekf->posNE[1] = 0.0f;
+						// set fusion flags
+						_ekf->fuseVelData = true;
+						_ekf->fusePosData = true;
+						// recall states stored at time of measurement after adjusting for delays
+						_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
+						_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
+						// run the fusion step
+						_ekf->FuseVelposNED();
 
-				_local_pos.xy_valid = _gps_initialized;
-				_local_pos.z_valid = true;
-				_local_pos.v_xy_valid = _gps_initialized;
-				_local_pos.v_z_valid = true;
-				_local_pos.xy_global = true;
+					} else {
+						_ekf->fuseVelData = false;
+						_ekf->fusePosData = false;
+					}
 
-				_local_pos.z_global = false;
-				_local_pos.yaw = _att.yaw;
+					if (newHgtData && _ekf->statesInitialised) {
+						// Could use a blend of GPS and baro alt data if desired
+						_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
+						_ekf->fuseHgtData = true;
+						// recall states stored at time of measurement after adjusting for delays
+						_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
+						// run the fusion step
+						_ekf->FuseVelposNED();
 
-				/* lazily publish the local position only once available */
-				if (_local_pos_pub > 0) {
-					/* publish the attitude setpoint */
-					orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos);
+					} else {
+						_ekf->fuseHgtData = false;
+					}
 
-				} else {
-					/* advertise and publish */
-					_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos);
-				}
+					// Fuse Magnetometer Measurements
+					if (newDataMag && _ekf->statesInitialised) {
+						_ekf->fuseMagData = true;
+						_ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
 
-				_global_pos.timestamp = _local_pos.timestamp;
+					} else {
+						_ekf->fuseMagData = false;
+					}
 
-				if (_local_pos.xy_global) {
-					double est_lat, est_lon;
-					map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon);
-					_global_pos.lat = est_lat;
-					_global_pos.lon = est_lon;
-					_global_pos.time_gps_usec = _gps.time_gps_usec;
-					_global_pos.eph = _gps.eph_m;
-					_global_pos.epv = _gps.epv_m;
-				}
+					if (_ekf->statesInitialised) _ekf->FuseMagnetometer();
 
-				if (_local_pos.v_xy_valid) {
-					_global_pos.vel_n = _local_pos.vx;
-					_global_pos.vel_e = _local_pos.vy;
-				} else {
-					_global_pos.vel_n = 0.0f;
-					_global_pos.vel_e = 0.0f;
-				}
+					// Fuse Airspeed Measurements
+					if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) {
+						_ekf->fuseVtasData = true;
+						_ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
+						_ekf->FuseAirspeed();
 
-				/* local pos alt is negative, change sign and add alt offsets */
-				_global_pos.alt = _local_pos.ref_alt + _baro_gps_offset + (-_local_pos.z);
+					} else {
+						_ekf->fuseVtasData = false;
+					}
 
-				if (_local_pos.v_z_valid) {
-					_global_pos.vel_d = _local_pos.vz;
-				}
 
-				_global_pos.yaw = _local_pos.yaw;
+					// Output results
+					math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
+					math::Matrix<3, 3> R = q.to_dcm();
+					math::Vector<3> euler = R.to_euler();
+
+					for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
+							_att.R[i][j] = R(i, j);
+
+					_att.timestamp = last_sensor_timestamp;
+					_att.q[0] = _ekf->states[0];
+					_att.q[1] = _ekf->states[1];
+					_att.q[2] = _ekf->states[2];
+					_att.q[3] = _ekf->states[3];
+					_att.q_valid = true;
+					_att.R_valid = true;
+
+					_att.timestamp = last_sensor_timestamp;
+					_att.roll = euler(0);
+					_att.pitch = euler(1);
+					_att.yaw = euler(2);
+
+					_att.rollspeed = _ekf->angRate.x - _ekf->states[10];
+					_att.pitchspeed = _ekf->angRate.y - _ekf->states[11];
+					_att.yawspeed = _ekf->angRate.z - _ekf->states[12];
+					// gyro offsets
+					_att.rate_offsets[0] = _ekf->states[10];
+					_att.rate_offsets[1] = _ekf->states[11];
+					_att.rate_offsets[2] = _ekf->states[12];
+
+					/* lazily publish the attitude only once available */
+					if (_att_pub > 0) {
+						/* publish the attitude setpoint */
+						orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att);
 
-				_global_pos.eph = _gps.eph_m;
-				_global_pos.epv = _gps.epv_m;
+					} else {
+						/* advertise and publish */
+						_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att);
+					}
 
-				_global_pos.timestamp = _local_pos.timestamp;
+					if (_gps_initialized) {
+						_local_pos.timestamp = last_sensor_timestamp;
+						_local_pos.x = _ekf->states[7];
+						_local_pos.y = _ekf->states[8];
+						// XXX need to announce change of Z reference somehow elegantly
+						_local_pos.z = _ekf->states[9] - _baro_gps_offset;
+
+						_local_pos.vx = _ekf->states[4];
+						_local_pos.vy = _ekf->states[5];
+						_local_pos.vz = _ekf->states[6];
+
+						_local_pos.xy_valid = _gps_initialized;
+						_local_pos.z_valid = true;
+						_local_pos.v_xy_valid = _gps_initialized;
+						_local_pos.v_z_valid = true;
+						_local_pos.xy_global = true;
+
+						_local_pos.z_global = false;
+						_local_pos.yaw = _att.yaw;
+
+						/* lazily publish the local position only once available */
+						if (_local_pos_pub > 0) {
+							/* publish the attitude setpoint */
+							orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos);
+
+						} else {
+							/* advertise and publish */
+							_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos);
+						}
+
+						_global_pos.timestamp = _local_pos.timestamp;
+
+						if (_local_pos.xy_global) {
+							double est_lat, est_lon;
+							map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon);
+							_global_pos.lat = est_lat;
+							_global_pos.lon = est_lon;
+							_global_pos.time_gps_usec = _gps.time_gps_usec;
+							_global_pos.eph = _gps.eph_m;
+							_global_pos.epv = _gps.epv_m;
+						}
+
+						if (_local_pos.v_xy_valid) {
+							_global_pos.vel_n = _local_pos.vx;
+							_global_pos.vel_e = _local_pos.vy;
+						} else {
+							_global_pos.vel_n = 0.0f;
+							_global_pos.vel_e = 0.0f;
+						}
+
+						/* local pos alt is negative, change sign and add alt offsets */
+						_global_pos.alt = _local_pos.ref_alt + _baro_gps_offset + (-_local_pos.z);
+
+						if (_local_pos.v_z_valid) {
+							_global_pos.vel_d = _local_pos.vz;
+						}
+
+
+						_global_pos.yaw = _local_pos.yaw;
+
+						_global_pos.eph = _gps.eph_m;
+						_global_pos.epv = _gps.epv_m;
+
+						_global_pos.timestamp = _local_pos.timestamp;
+
+						/* lazily publish the global position only once available */
+						if (_global_pos_pub > 0) {
+							/* publish the global position */
+							orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos);
+
+						} else {
+							/* advertise and publish */
+							_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos);
+						}
+
+						if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
+							_wind.timestamp = _global_pos.timestamp;
+							_wind.windspeed_north = _ekf->states[14];
+							_wind.windspeed_east = _ekf->states[15];
+							_wind.covariance_north = 0.0f; // XXX get form filter
+							_wind.covariance_east = 0.0f;
+
+							/* lazily publish the wind estimate only once available */
+							if (_wind_pub > 0) {
+								/* publish the wind estimate */
+								orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
+
+							} else {
+								/* advertise and publish */
+								_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
+							}
+
+						}
 
-				/* lazily publish the global position only once available */
-				if (_global_pos_pub > 0) {
-					/* publish the global position */
-					orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos);
+					}
 
-				} else {
-					/* advertise and publish */
-					_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos);
 				}
 
-				if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
-					_wind.timestamp = _global_pos.timestamp;
-					_wind.windspeed_north = _ekf->states[14];
-					_wind.windspeed_east = _ekf->states[15];
-					_wind.covariance_north = 0.0f; // XXX get form filter
-					_wind.covariance_east = 0.0f;
-
-					/* lazily publish the wind estimate only once available */
-					if (_wind_pub > 0) {
-						/* publish the wind estimate */
-						orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
-
-					} else {
-						/* advertise and publish */
-						_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
-					}
-				}
 			}
 
 		}
@@ -1407,9 +1420,10 @@ FixedwingEstimator::print_status()
 	// 4-6: Velocity - m/sec (North, East, Down)
 	// 7-9: Position - m (North, East, Down)
 	// 10-12: Delta Angle bias - rad (X,Y,Z)
-	// 13-14: Wind Vector  - m/sec (North,East)
-	// 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
-	// 18-20: Body Magnetic Field Vector - gauss (X,Y,Z)
+	// 13:    Accelerometer offset
+	// 14-15: Wind Vector  - m/sec (North,East)
+	// 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
+	// 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
 
 	printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec);
 	printf("ref alt: %8.6f\n", (double)_local_pos.ref_alt);

src/modules/ekf_att_pos_estimator/estimator.cpp

@@ -145,7 +145,7 @@ AttPosEKF::AttPosEKF()
      * instead to allow clean in-air re-initialization.
      */
 {
-
+    memset(&last_ekf_error, 0, sizeof(last_ekf_error));
     ZeroVariables();
     InitialiseParameters();
 }
@@ -2382,7 +2382,7 @@ int AttPosEKF::CheckAndBound()
 
     // Reset the filter if the IMU data is too old
     if (dtIMU > 0.3f) {
-
+        FillErrorReport(&last_ekf_error);
         ResetVelocity();
         ResetPosition();
         ResetHeight();
@@ -2397,6 +2397,7 @@ int AttPosEKF::CheckAndBound()
 
     // Check if we switched between states
     if (currStaticMode != staticMode) {
+        FillErrorReport(&last_ekf_error);
         ResetVelocity();
         ResetPosition();
         ResetHeight();
@@ -2405,6 +2406,15 @@ int AttPosEKF::CheckAndBound()
         return 3;
     }
 
+    // Reset the filter if gyro offsets are excessive
+    if (fabs(states[10]) > 1.0f || fabsf(states[11]) > 1.0f || fabsf(states[12]) > 1.0f) {
+        
+        InitializeDynamic(velNED, magDeclination);
+
+        // that's all we can do here, return
+        return 4;
+    }
+
     return 0;
 }
 
@@ -2531,8 +2541,6 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do
     // the baro offset must be this difference now
     baroHgtOffset = baroHgt - referenceHgt;
 
-    memset(&last_ekf_error, 0, sizeof(last_ekf_error));
-
     InitializeDynamic(initvelNED, declination);
 }
 

src/modules/ekf_att_pos_estimator/estimator.h

@@ -200,7 +200,6 @@ public:
     float hgtMea; //  measured height (m)
     float baroHgtOffset;        ///< the baro (weather) offset from normalized altitude
     float rngMea; // Ground distance
-    float posNED[3]; // North, East Down position (m)
 
     float innovMag[3]; // innovation output
     float varInnovMag[3]; // innovation variance output