Mercurial > public > ostc4

diff Small_CPU/Src/scheduler.c @ 881:5b675077ccfb Evo_2_23

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Added filtering of ascent speed: In the previous version the ascent speed was simply calculated based of the last measured pressure values. This may lead to unexpected colorchanges of the depth value. To smooth this behavior a filter has been added which evaluates if a movment direction is valid for a certain period of time. If the condition direction is stable then the ascend speed is provided.
author Ideenmodellierer
date Sat, 31 Aug 2024 17:28:37 +0200
parents 3311b720a072
children 2225c467f1e9
line wrap: on
line diff
--- a/Small_CPU/Src/scheduler.c	Thu Aug 29 15:01:30 2024 +0200
+++ b/Small_CPU/Src/scheduler.c	Sat Aug 31 17:28:37 2024 +0200
@@ -43,6 +43,7 @@
 #include "tm_stm32f4_otp.h"
 #include "externalInterface.h"
 #include "uart.h"
+#include "math.h"
 
 /* uncomment to enable restoting of last known date in case of a power loss (RTC looses timing data) */
 /* #define RESTORE_LAST_KNOWN_DATE */
@@ -51,6 +52,14 @@
 #define START_DIVE_MOUNTAIN_MODE_BAR	(0.88f)
 #define START_DIVE_IMMEDIATLY_BAR		(1.16f)
 
+/* Ascent rate calculation */
+typedef enum
+{
+	ASCENT_NONE = 0,
+	ASCENT_RISING,
+	ASCENT_FALLING,
+} AscentStates_t;
+
 /* Private types -------------------------------------------------------------*/
 const SGas Air = {79,0,0,0,0};
 
@@ -93,6 +102,7 @@
 void copyExtADCdata();
 void copyExtCO2data();
 static void schedule_update_timer_helper(int8_t thisSeconds);
+static void evaluateAscentSpeed(void);
 uint32_t time_elapsed_ms(uint32_t ticksstart,uint32_t ticksnow);
 
 void scheduleSetDate(SDeviceLine *line);
@@ -480,11 +490,8 @@
 {
 	uint32_t ticksdiff = 0; 
 	uint32_t lasttick = 0;
-	uint32_t lastPressureTick = 0;
-	uint32_t tickPressureDiff = 0;
 	uint8_t extAdcChannel = 0;
 	uint8_t counterAscentRate = 0;
-	float lastPressure_bar = 0.0f;
 	global.dataSendToMaster.mode = MODE_DIVE;
 	global.deviceDataSendToMaster.mode = MODE_DIVE;
 	uint8_t counter_exit = 0;
@@ -558,17 +565,8 @@
 				counterAscentRate++;
 				if(counterAscentRate == 4)
 				{
-					tickPressureDiff = time_elapsed_ms(lastPressureTick,lasttick); /* Calculate ascent rate every 400ms use timer to take care for small time shifts */
-					if(tickPressureDiff != 0)
-					{
-						global.lifeData.pressure_ambient_bar = get_pressure_mbar() / 1000.0f;
-						if(lastPressure_bar >= 0)
-						{
-							global.lifeData.ascent_rate_meter_per_min = (lastPressure_bar - global.lifeData.pressure_ambient_bar)  * (60000.0 / tickPressureDiff) * 10; /* bar * 10 = meter */
-						}
-					}
-					lastPressure_bar = global.lifeData.pressure_ambient_bar;
-					lastPressureTick = lasttick;
+					global.lifeData.pressure_ambient_bar = get_pressure_mbar() / 1000.0f;
+					evaluateAscentSpeed();
 					counterAscentRate = 0;
 				}
 				copyPressureData();
@@ -606,7 +604,7 @@
 
 
 				/** counter_exit allows safe exit via button for testing
-					* and demo_mode is exited too if aplicable.
+					* and demo_mode is exited too if applicable.
 					*/
 				if(global.dataSendToMaster.mode == MODE_ENDDIVE)
 				{
@@ -620,6 +618,7 @@
 
 				if(is_ambient_pressure_close_to_surface(&global.lifeData))
 				{
+
 					global.lifeData.counterSecondsShallowDepth++;
 					if((global.lifeData.counterSecondsShallowDepth >= global.settings.timeoutDiveReachedZeroDepth) || ((global.lifeData.dive_time_seconds < 60) && (global.demo_mode == 0))
 							|| (ManualExitDiveCounter))
@@ -657,6 +656,7 @@
 				// surface break
 				if(is_ambient_pressure_close_to_surface(&global.lifeData))
 				{
+					global.lifeData.ascent_rate_meter_per_min = 0;
 					global.lifeData.counterSecondsShallowDepth++;
 					if(global.lifeData.counterSecondsShallowDepth > 3) // time for main cpu to copy to apnea_last_dive_time_seconds
 					{
@@ -1814,6 +1814,78 @@
 	return retval;
 }
 
+void evaluateAscentSpeed()
+{
+	static uint32_t lastPressureTick = 0;
+	static float lastPressure_bar = 0.0f;
+	static AscentStates_t ascentState = ASCENT_NONE;
+	static uint8_t ascentStableCnt = 0;
+	uint32_t tickPressureDiff = 0;
+	uint32_t lasttick = HAL_GetTick();
+	float localAscentRate = 0.0;
+
+	tickPressureDiff = time_elapsed_ms(lastPressureTick,lasttick); /* Calculate ascent rate every 400ms use timer to take care for small time shifts */
+	if(tickPressureDiff != 0)
+	{
+		if(lastPressure_bar >= 0)
+		{
+			localAscentRate = (lastPressure_bar - global.lifeData.pressure_ambient_bar)  * (60000.0 / tickPressureDiff) * 10; /* bar * 10 = meter */
+			if(fabs(localAscentRate) < 1.0)
+			{
+				ascentState = ASCENT_NONE;
+				ascentStableCnt = 0;
+			}
+			else if(localAscentRate > 0.0)
+			{
+				if(ascentState != ASCENT_FALLING)
+				{
+					if(ascentStableCnt < 5)
+					{
+						ascentStableCnt++;
+					}
+					else
+					{
+						ascentState = ASCENT_RISING;
+					}
+				}
+				else
+				{
+					ascentState = ASCENT_NONE;
+					ascentStableCnt = 0;
+				}
+			}
+			else	/* must be falling */
+			{
+				if(ascentState != ASCENT_RISING)
+				{
+					if(ascentStableCnt < 5)
+					{
+						ascentStableCnt++;
+					}
+					else
+					{
+							ascentState = ASCENT_FALLING;
+					}
+				}
+				else
+				{
+					ascentState = ASCENT_NONE;
+					ascentStableCnt = 0;
+				}
+			}
+			if(ascentState != ASCENT_NONE)
+			{
+				global.lifeData.ascent_rate_meter_per_min = localAscentRate;
+			}
+			else
+			{
+				global.lifeData.ascent_rate_meter_per_min = 0;
+			}
+		}
+	}
+	lastPressure_bar = global.lifeData.pressure_ambient_bar;
+	lastPressureTick = lasttick;
+}
 
 /************************ (C) COPYRIGHT heinrichs weikamp *****END OF FILE****/