ostc4: Small_CPU/Src/scheduler.c comparison

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

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

comparison

equal deleted inserted replaced

-:f012fcd7f465
+:5b675077ccfb
 #include "stm32f4xx_hal_rtc_ex.h"
 #include "decom.h"
 #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 */
 #define INVALID_PREASURE_VALUE 			(0.0f)
 #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};
 /* Exported variables --------------------------------------------------------*/
 void copyDeviceData(void);
 void copyPICdata(void);
 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);
 /* Exported functions --------------------------------------------------------*/
 */
 void scheduleDiveMode(void)
 {
 	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;
 	Scheduler.counterSPIdata100msec = 0;
 #endif
 				counterAscentRate++;
 				if(counterAscentRate == 4)
 				{
-					tickPressureDiff = time_elapsed_ms(lastPressureTick,lasttick); /* Calculate ascent rate every 400ms use timer to take care for small time shifts */
+					global.lifeData.pressure_ambient_bar = get_pressure_mbar() / 1000.0f;
-					if(tickPressureDiff != 0)
+					evaluateAscentSpeed();
-					{
-						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;
 					counterAscentRate = 0;
 				}
 				copyPressureData();
 				Scheduler.counterPressure100msec++;
 		}
 				decom_oxygen_calculate_otu(&global.lifeData.otu,global.lifeData.ppO2);
 				battery_gas_gauge_get_data();
 				/** 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)
 				{
 					counter_exit++;
 					if(counter_exit >= 2)
 					}
 				}
 				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))
 					{
 						global.seconds_since_last_dive = 1; // start counter
 				}
 				// 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
 					{
 						global.lifeData.dive_time_seconds = 0; // this apnea dive ends here
 					}
 		}
 	}
 	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****/

Mercurial > public > ostc4

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