EKF: Publish global position also if GPS is not yet valid so that controllers can get a valid altitude

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -221,6 +221,10 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_parameter_handles.eas_noise = param_find("PE_EAS_NOISE");
 	_parameter_handles.pos_stddev_threshold = param_find("PE_POSDEV_INIT");
 
+	/* indicate consumers that the current position data is not valid */
+	_gps.eph = 10000.0f;
+	_gps.epv = 10000.0f;
+
 	/* fetch initial parameter values */
 	parameters_update();
 
@@ -686,21 +690,21 @@ void AttitudePositionEstimatorEKF::task_main()
 						continue;
 					}
 
-					//Run EKF data fusion steps
+					// Run EKF data fusion steps
 					updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData);
 
-					//Publish attitude estimations
+					// Publish attitude estimations
 					publishAttitude();
 
-					//Publish Local Position estimations
+					// Publish Local Position estimations
 					publishLocalPosition();
 
-					//Publish Global Position, but only if it's any good
+					// Publish Global Position, but only if it's any good
-					if (_gps_initialized && (_gpsIsGood || _global_pos.dead_reckoning)) {
+					if (_gpsIsGood || _global_pos.dead_reckoning) {
 						publishGlobalPosition();
 					}
 
-					//Publish wind estimates
+					// Publish wind estimates
 					if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
 						publishWindEstimate();
 					}
@@ -891,6 +895,10 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
 		_global_pos.lat = est_lat;
 		_global_pos.lon = est_lon;
 		_global_pos.time_utc_usec = _gps.time_utc_usec;
+	} else {
+		_global_pos.lat = 0.0;
+		_global_pos.lon = 0.0;
+		_global_pos.time_utc_usec = 0;
 	}
 
 	if (_local_pos.v_xy_valid) {
@@ -907,6 +915,8 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
 
 	if (_local_pos.v_z_valid) {
 		_global_pos.vel_d = _local_pos.vz;
+	} else {
+		_global_pos.vel_d = 0.0f;
 	}
 
 	/* terrain altitude */