From 4ea49242d30c8f920b53895de067d99af41e36aa Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Wed, 2 Aug 2017 20:19:28 +0200
Subject: [PATCH] SDP3x pitot compensation: fixed the compensation and protect
 agains negative compensation values

---
 src/modules/systemlib/airspeed.c | 11 ++++++++---
 1 file changed, 8 insertions(+), 3 deletions(-)

diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c
index aad1540ea0..26a8aed81e 100644
--- a/src/modules/systemlib/airspeed.c
+++ b/src/modules/systemlib/airspeed.c
@@ -78,12 +78,17 @@ float calc_indicated_airspeed_corrected(enum AIRSPEED_PITOT_MODEL pmodel, enum A
 		break;
 
 	case AIRSPEED_SENSOR_MODEL_SDP3X: {
-			//
-			double flow_SDP33 = (300.805 - 300.878 / (0.00344205 * dp * 0.68698 * 0.68698 + 1)) * 1.29 / rho_air;
+			// flow through sensor
+			double flow_SDP33 = (300.805 - 300.878 / (0.00344205 * pow(dp, 0.68698) + 1)) * 1.29 / rho_air;
+
+			// for too small readings the compensation might result in a negative flow which causes numerical issues
+			if (flow_SDP33 < 0.0) {
+				flow_SDP33 = 0.0;
+			}
 
 			switch (pmodel) {
 			case AIRSPEED_PITOT_MODEL_HB:
-				dp_pitot = 28557670.0 - 28557670.0 / (1 + (flow_SDP33 / 5027611.0) * 1.227924 * 1.227924);
+				dp_pitot = 28557670.0 - 28557670.0 / (1 + pow((flow_SDP33 / 5027611.0), 1.227924));
 				break;
 
 			default: