From 4a8f799e9e0c862018360db808793e77759a5336 Mon Sep 17 00:00:00 2001
From: Johan Jansen <jnsn.johan@gmail.com>
Date: Fri, 13 Mar 2015 15:27:02 +0100
Subject: [PATCH] AttPosEKF: Make local_pos output Z ref pos relative

---
 .../AttitudePositionEstimatorEKF.h            |   1 +
 .../ekf_att_pos_estimator_main.cpp            | 122 +++++++++---------
 2 files changed, 64 insertions(+), 59 deletions(-)

diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
index 8d036261eb..4d5e56a961 100644
--- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
@@ -193,6 +193,7 @@ private:
     bool            _gpsIsGood;               ///< True if the current GPS fix is good enough for us to use
     uint64_t        _previousGPSTimestamp;    ///< Timestamp of last good GPS fix we have received
     bool            _baro_init;
+    float           _baroAltRef;
     bool            _gps_initialized;
     hrt_abstime     _filter_start_time;
     hrt_abstime     _last_sensor_timestamp;
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 686ec64f9f..6ca10e56ae 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
@@ -132,70 +132,71 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_wind_pub(-1),
 
 	_att({}),
-     _gyro({}),
-     _accel({}),
-     _mag({}),
-     _airspeed({}),
-     _baro({}),
-     _vstatus({}),
-     _global_pos({}),
-     _local_pos({}),
-     _gps({}),
-     _wind({}),
-     _distance {},
-     _landDetector {},
-     _armed {},
+    _gyro({}),
+    _accel({}),
+    _mag({}),
+    _airspeed({}),
+    _baro({}),
+    _vstatus({}),
+    _global_pos({}),
+    _local_pos({}),
+    _gps({}),
+    _wind({}),
+    _distance {},
+    _landDetector {},
+    _armed {},
 
-     _gyro_offsets({}),
-     _accel_offsets({}),
-     _mag_offsets({}),
+    _gyro_offsets({}),
+    _accel_offsets({}),
+    _mag_offsets({}),
 
-     _sensor_combined {},
+    _sensor_combined {},
 
-     _pos_ref {},
-     _baro_ref_offset(0.0f),
-     _baro_gps_offset(0.0f),
+    _pos_ref{},
+    _baro_ref_offset(0.0f),
+    _baro_gps_offset(0.0f),
 
-     /* performance counters */
-     _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
-     _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")),
-     _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
-     _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
-     _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
-     _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")),
-     _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")),
-     _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")),
+    /* performance counters */
+    _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
+    _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")),
+    _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
+    _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
+    _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
+    _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")),
+    _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")),
+    _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")),
 
-     /* states */
-     _gps_alt_filt(0.0f),
-     _baro_alt_filt(0.0f),
-     _covariancePredictionDt(0.0f),
-     _gpsIsGood(false),
-     _previousGPSTimestamp(0),
-     _baro_init(false),
-     _gps_initialized(false),
-     _filter_start_time(0),
-     _last_sensor_timestamp(0),
-     _last_run(0),
-     _distance_last_valid(0),
-     _gyro_valid(false),
-     _accel_valid(false),
-     _mag_valid(false),
-     _gyro_main(0),
-     _accel_main(0),
-     _mag_main(0),
-     _ekf_logging(true),
-     _debug(0),
+    /* states */
+    _gps_alt_filt(0.0f),
+    _baro_alt_filt(0.0f),
+    _covariancePredictionDt(0.0f),
+    _gpsIsGood(false),
+    _previousGPSTimestamp(0),
+    _baro_init(false),
+    _baroAltRef(0.0f),
+    _gps_initialized(false),
+    _filter_start_time(0),
+    _last_sensor_timestamp(0),
+    _last_run(0),
+    _distance_last_valid(0),
+    _gyro_valid(false),
+    _accel_valid(false),
+    _mag_valid(false),
+    _gyro_main(0),
+    _accel_main(0),
+    _mag_main(0),
+    _ekf_logging(true),
+    _debug(0),
 
-     _newHgtData(false),
-     _newAdsData(false),
-     _newDataMag(false),
-     _newRangeData(false),
+    _newHgtData(false),
+    _newAdsData(false),
+    _newDataMag(false),
+    _newRangeData(false),
 
-     _mavlink_fd(-1),
-     _parameters {},
-     _parameter_handles {},
-     _ekf(nullptr)
+    _mavlink_fd(-1),
+    _parameters {},
+    _parameter_handles {},
+    _ekf(nullptr)
 {
 	_last_run = hrt_absolute_time();
 
@@ -810,7 +811,7 @@ void AttitudePositionEstimatorEKF::publishLocalPosition()
 	_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.z = _ekf->states[9] - _baro_ref_offset - _baroAltRef;
 
 	_local_pos.vx = _ekf->states[4];
 	_local_pos.vy = _ekf->states[5];
@@ -1399,7 +1400,10 @@ void AttitudePositionEstimatorEKF::pollData()
 		}
 
 		baro_last = _baro.timestamp;
-		_baro_init = true;
+		if(!_baro_init) {
+			_baro_init = true;
+			_baroAltRef = _baro.altitude;
+		}
 
 		_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f));