diff Small_CPU/Src/pressure.c @ 331:b4c578caaafb I2C_Improvment

Added plausibility check for pressure values In case of I2C communication problems at startup the dc jumped into dive mode with depth up to 300m. As no CRC is applied a bit flip may also occure during normal operation without detection => added a plausibility check if last measured value fits to the last measurements
author ideenmodellierer
date Wed, 17 Jul 2019 22:43:51 +0200
parents 8e9c502c0b06
children c11ce8c885d3
line wrap: on
line diff
--- a/Small_CPU/Src/pressure.c	Wed Jul 17 22:43:16 2019 +0200
+++ b/Small_CPU/Src/pressure.c	Wed Jul 17 22:43:51 2019 +0200
@@ -28,7 +28,7 @@
  the last 30 minutes will be saved once per minute in a endless loop
  at the beginning of a dive the oldest value will be used
 */
-
+#include "math.h"
 #include "scheduler.h"
 #include "pressure.h"
 #include "i2c.h"
@@ -46,6 +46,9 @@
 #define CMD_ADC_4096 0x08 // ADC OSR=4096
 #define CMD_PROM_RD 0xA0 // Prom read command
 
+#define PRESSURE_HISTORY_SIZE			(8u)
+#define PRESSURE_JUMP_VALID_MBAR	    (500.0f)		/* values are measure several times a second => jumps > 5m very unlikely */
+
 static uint16_t get_ci_by_coef_num(uint8_t coef_num);
 //void pressure_calculation_new(void);
 //void pressure_calculation_old(void);
@@ -74,10 +77,12 @@
 */
 
 static float ambient_temperature = 0;
-static float ambient_pressure_mbar = 0;
-static float surface_pressure_mbar = 1000;
+static float ambient_pressure_mbar = 1000.0;
+static float surface_pressure_mbar = 1000.0;
 static float surface_ring_mbar[31] = { 0 };
 
+static float pressure_history_mbar[PRESSURE_HISTORY_SIZE];
+
 uint8_t secondCounterSurfaceRing = 0;
 
 float get_temperature(void)
@@ -104,6 +109,13 @@
 	surface_pressure_mbar = ambient_pressure_mbar;
 }
 
+void init_pressure_history(void)
+{
+	for(int i=0; i<PRESSURE_HISTORY_SIZE; i++)
+	{
+		pressure_history_mbar[i] = 1000.0;
+	}
+}
 
 /* the ring has one place with 0
  * after that comes the oldest value
@@ -119,17 +131,20 @@
 	
 	secondCounterSurfaceRing = 0;
 	
-	int hole;
-	for(hole=30;hole>0;hole--)
-		if(surface_ring_mbar[hole] == 0) { break; }
-
-	surface_ring_mbar[hole] = ambient_pressure_mbar;
+	if(is_init_pressure_done())
+	{
+		int hole;
+		for(hole=30;hole>0;hole--)
+			if(surface_ring_mbar[hole] == 0) { break; }
 
-	hole++;
-	if(hole > 30)
-		hole = 0;
-	surface_pressure_mbar = surface_ring_mbar[hole];
-	surface_ring_mbar[hole] = 0;
+		surface_ring_mbar[hole] = ambient_pressure_mbar;
+
+		hole++;
+		if(hole > 30)
+			hole = 0;
+		surface_pressure_mbar = surface_ring_mbar[hole];
+		surface_ring_mbar[hole] = 0;
+	}
 }
 
 #ifdef DEMOMODE
@@ -209,29 +224,6 @@
 }
 #endif
 
-
-/* called just once on power on */
-/* TBD old DR5 code? */
-void init_pressure_DRx(void)
-{
-	uint8_t resetCommand[1] = {0x1E};
-
-	I2C_Master_Transmit(  DEVICE_PRESSURE, resetCommand, 1);
-	HAL_Delay(3);
-
-	C[1] =	get_ci_by_coef_num(0x02);
-	C[2] =	get_ci_by_coef_num(0x04);
-	C[3] =	get_ci_by_coef_num(0x06);
-	C[4] =	get_ci_by_coef_num(0x08);
-	C[5] =	get_ci_by_coef_num(0x0A);
-	C[6] =	get_ci_by_coef_num(0x0C);
-	
-	C5_x_2p8  = C[5] * 256;
-	C2_x_2p16 = C[2] * 65536;
-	C1_x_2p15 = C[1] * 32768;
-	pressure_update();
-}
-
 uint8_t is_init_pressure_done(void)
 {
 	return pressureSensorInitSuccess;
@@ -243,7 +235,10 @@
 	buffer[0] = 0x1e;
 	uint8_t retValue = 0xFF;
 	
-	
+	pressureSensorInitSuccess = false;
+	init_pressure_history();
+
+/* Send reset request to pressure sensor */
 	retValue = I2C_Master_Transmit(  DEVICE_PRESSURE, buffer, 1);
 	if(retValue != HAL_OK)
 	{
@@ -264,8 +259,10 @@
 	if(global.I2C_SystemStatus == HAL_OK)
 	{
 		pressureSensorInitSuccess = 1;
+		retValue = pressure_update();
+
 	}
-	return pressure_update();
+	return retValue;
 }
 
 
@@ -416,6 +413,40 @@
 	pressure_calculation_AN520_004_mod_MS5803_30BA__09_2015();
 }
 
+static uint8_t pressure_plausible(float pressurevalue)
+{
+	static uint8_t pressurewriteindex = 0;
+	uint8_t retval = 0;
+	uint8_t index;
+	float pressure_average = 0;
+
+	for(index = 0; index < PRESSURE_HISTORY_SIZE; index++)
+	{
+		pressure_average += pressure_history_mbar[index];
+	}
+	pressure_average /= PRESSURE_HISTORY_SIZE;
+	if(pressure_average == 1000.0) /* first pressure calculation */
+	{
+		if(fabs(pressurevalue - pressure_average) < 11000.0)  /* just in case a reset occure during dive assume value equal < 100m as valid */
+		{
+			for(index = 0; index < PRESSURE_HISTORY_SIZE; index++)
+			{
+				pressure_history_mbar[index] = pressurevalue;	/* set history to current value */
+				retval = 1;
+			}
+		}
+	}
+	else
+	{
+		if(fabs(pressurevalue - pressure_average) < PRESSURE_JUMP_VALID_MBAR)
+		pressure_history_mbar[pressurewriteindex++] = pressurevalue;
+		pressurewriteindex &= 0x7;	/* wrap around if necessary */
+		retval = 1;
+	}
+
+	return retval;
+}
+
 static void pressure_calculation_AN520_004_mod_MS5803_30BA__09_2015(void)
 {
 	uint32_t local_D1; // ADC value of the pressure conversion
@@ -426,6 +457,8 @@
 	int64_t local_OFF; // offset at actual temperature
 	int64_t local_SENS; // sensitivity at actual temperature
 
+	float calc_pressure;
+
 	int64_t T2;
 	int64_t OFF2;
 	int64_t SENS2;
@@ -479,7 +512,12 @@
 								/  8192 );//     )) / 10; // pow(2,21), pow(2,13)
 
 	ambient_temperature 	= ((float)local_Tx100) / 100;
-	ambient_pressure_mbar	= ((float)local_Px10) / 10;
+
+	calc_pressure = ((float)local_Px10) / 10;
+	if(pressure_plausible(calc_pressure))
+	{
+		ambient_pressure_mbar = calc_pressure;
+	}
 }