From a3967872f37d575e1d7fb69e682ab61641b20335 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 18 Mar 2016 22:08:32 +0100
Subject: [PATCH] df_bmp280_wrapper: hack to publish altitude

Previously we only published pressure which wasn't inav for the position
estimator.
---
 .../df_bmp280_wrapper/df_bmp280_wrapper.cpp   | 31 ++++++++++++++++---
 1 file changed, 27 insertions(+), 4 deletions(-)

diff --git a/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp b/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp
index 8d9930f39b..b228318142 100644
--- a/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp
@@ -158,7 +158,6 @@ int DfBmp280Wrapper::stop()
 
 int DfBmp280Wrapper::_publish(struct baro_sensor_data &data)
 {
-#if 1
 	perf_begin(_baro_sample_perf);
 
 	baro_report baro_report = {};
@@ -167,8 +166,33 @@ int DfBmp280Wrapper::_publish(struct baro_sensor_data &data)
 	baro_report.pressure = data.pressure_pa;
 	baro_report.temperature = data.temperature_c;
 
-	// TODO: add this
-	baro_report.altitude = -1.0f;
+	// TODO: verify this, it's just copied from the MS5611 driver.
+
+	// Constant for now
+	const float MSL_PRESSURE = 101325.0f;
+
+	/* tropospheric properties (0-11km) for standard atmosphere */
+	const double T1 = 15.0 + 273.15;	/* temperature at base height in Kelvin */
+	const double a  = -6.5 / 1000;	/* temperature gradient in degrees per metre */
+	const double g  = 9.80665;	/* gravity constant in m/s/s */
+	const double R  = 287.05;	/* ideal gas constant in J/kg/K */
+
+	/* current pressure at MSL in kPa */
+	double p1 = MSL_PRESSURE / 1000.0;
+
+	/* measured pressure in kPa */
+	double p = data.pressure_pa / 1000.0;
+
+	/*
+	 * Solve:
+	 *
+	 *     /        -(aR / g)     \
+	 *    | (p / p1)          . T1 | - T1
+	 *     \                      /
+	 * h = -------------------------------  + h1
+	 *                   a
+	 */
+	baro_report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
 
 	// TODO: when is this ever blocked?
 	if (!(m_pub_blocked)) {
@@ -182,7 +206,6 @@ int DfBmp280Wrapper::_publish(struct baro_sensor_data &data)
 	DevMgr::updateNotify(*this);
 
 	perf_end(_baro_sample_perf);
-#endif
 
 	return 0;
 };