Merged voltage compensation

1 files changed, 17 insertions, 4 deletions

diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 2324475ce..0b7986383 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -126,7 +126,7 @@ protected:
 	/**
 	 * Correct for 5V rail voltage variations
 	 */
-	void voltage_correction(float &diff_pres_pa);
+	void voltage_correction(float &diff_pres_pa, float &temperature);
 
 	int _t_system_power;
 	struct system_power_s system_power;
@@ -204,7 +204,7 @@ MEASAirspeed::collect()
 	dp_raw = 0x3FFF & dp_raw;
 	dT_raw = (val[2] << 8) + val[3];
 	dT_raw = (0xFFE0 & dT_raw) >> 5;
-	float temperature = ((200 * dT_raw) / 2047) - 50;
+	float temperature = ((200.0f * dT_raw) / 2047) - 50;
 
 	/* calculate differential pressure. As its centered around 8000
 	 * and can go positive or negative, enforce absolute value
@@ -218,7 +218,7 @@ MEASAirspeed::collect()
 	}
 
         // correct for 5V rail voltage if possible
-        voltage_correction(diff_press_pa);
+        voltage_correction(diff_press_pa, temperature);
 
 	struct differential_pressure_s report;
 
@@ -308,7 +308,7 @@ MEASAirspeed::cycle()
    offset versus voltage for 3 sensors
  */
 void
-MEASAirspeed::voltage_correction(float &diff_press_pa)
+MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
 {
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
 	if (_t_system_power == -1) {
@@ -340,6 +340,19 @@ MEASAirspeed::voltage_correction(float &diff_press_pa)
 		voltage_diff = -0.5f;
 	}
 	diff_press_pa -= voltage_diff * slope;
+
+	/*
+	  the temperature masurement varies as well
+	 */
+	const float temp_slope = 0.887f;
+	voltage_diff = system_power.voltage5V_v - 5.0f;
+	if (voltage_diff > 0.5f) {
+		voltage_diff = 0.5f;
+	}
+	if (voltage_diff < -1.0f) {
+		voltage_diff = -1.0f;
+	}
+	temperature -= voltage_diff * temp_slope;	
 #endif // CONFIG_ARCH_BOARD_PX4FMU_V2
 }