AttPosEKF: Publish altitude position estimates without GPS

1 files changed, 83 insertions, 85 deletions

diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 81b27ddbe..e02adb305 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -1514,107 +1514,105 @@ void AttitudePositionEstimatorEKF::task_main()
 						_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_ref_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);
+					//Publish position estimations
+					_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_ref_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 = _gps_initialized;
+
+					_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);
-						}
+					} else {
+						/* advertise and publish */
+						_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos);
+					}
 
-						_global_pos.timestamp = _local_pos.timestamp;
+					_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_utc_usec = _gps.time_utc_usec;
-							_global_pos.eph = _gps.eph;
-							_global_pos.epv = _gps.epv;
-						}
+					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_utc_usec = _gps.time_utc_usec;
+						_global_pos.eph = _gps.eph;
+						_global_pos.epv = _gps.epv;
+					}
 
-						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;
-						}
+					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 = _baro_ref + (-_local_pos.z) - _baro_gps_offset;
+					/* local pos alt is negative, change sign and add alt offsets */
+					_global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset;
 
-						if (_local_pos.v_z_valid) {
-							_global_pos.vel_d = _local_pos.vz;
-						}
+					if (_local_pos.v_z_valid) {
+						_global_pos.vel_d = _local_pos.vz;
+					}
 
-						/* terrain altitude */
-						_global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1];
-						_global_pos.terrain_alt_valid = (_distance_last_valid > 0) &&
-						(hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000);
+					/* terrain altitude */
+					_global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1];
+					_global_pos.terrain_alt_valid = (_distance_last_valid > 0) &&
+					(hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000);
 
-						_global_pos.yaw = _local_pos.yaw;
+					_global_pos.yaw = _local_pos.yaw;
 
-						_global_pos.eph = _gps.eph;
-						_global_pos.epv = _gps.epv;
+					_global_pos.eph = _gps.eph;
+					_global_pos.epv = _gps.epv;
+
+					_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);
+					}
 
-						_global_pos.timestamp = _local_pos.timestamp;
+					if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
+						_wind.timestamp = _global_pos.timestamp;
+						_wind.windspeed_north = _ekf->states[14];
+						_wind.windspeed_east = _ekf->states[15];
+						// XXX we need to do something smart about the covariance here
+						// but we default to the estimate covariance for now
+						_wind.covariance_north = _ekf->P[14][14];
+						_wind.covariance_east = _ekf->P[15][15];
 
-						/* 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);
+						/* 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 */
-							_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos);
+							_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
 						}
-
-						if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
-							_wind.timestamp = _global_pos.timestamp;
-							_wind.windspeed_north = _ekf->states[14];
-							_wind.windspeed_east = _ekf->states[15];
-							// XXX we need to do something smart about the covariance here
-							// but we default to the estimate covariance for now
-							_wind.covariance_north = _ekf->P[14][14];
-							_wind.covariance_east = _ekf->P[15][15];
-
-							/* 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);
-							}
-						}
-
 					}
-
 				}
 
 			}