diff --git a/algorithm.c b/algorithm.c
new file mode 100644
index 0000000..ee87e81
--- /dev/null
+++ b/algorithm.c
@@ -0,0 +1,79 @@
+#include"algorithm.h"
+#include"main.h"
+#include <stdio.h>
+
+//int16_t value_last=0;
+//int8_t 	value_state_n=0;
+//int8_t 	value_state_l=0;
+//int8_t 	value_state_storage[3]={0};
+//int8_t 	act=0;
+//int 	value_count=0;
+//uint8_t value_state_valid=0;
+
+int 	value_last=0;
+int 	value_state_n=0;
+int 	value_state_l=0;
+int 	value_state_storage[3]={0};
+int 	act=0;
+int 	value_count=0;
+int 	value_state_valid=0;
+
+int 	qvar_digital(int16_t value)
+{
+	int value_deviation=value-value_last;
+
+	if (value_deviation>=54600)
+		value_state_n=1;
+	else if(value_deviation>=23400)
+		value_state_n=value_state_l+1;
+	else if(value_deviation<=-54600)
+		value_state_n=-1;
+	else if(value_deviation<=-23400)
+		value_state_n=value_state_l-1;
+	else
+		value_state_n=value_state_l;
+
+	value_last=	value_state_n * 32760;
+
+	if (value_state_n==value_state_l)
+	{
+		value_count+=1;
+		if	((value_count>=40)&&(value_state_n!=0)&&(value_state_n!=value_state_storage[0]))
+		{
+			value_count=0;
+			value_state_valid=1;
+			value_state_storage[2]=value_state_storage[1];
+			value_state_storage[1]=value_state_storage[0];
+			value_state_storage[0]=value_state_n;
+		}
+		else if((value_count>=120)&&(value_state_n==0))
+		{
+			value_count=0;
+			value_state_valid=1;
+			value_state_storage[2]=value_state_storage[1];
+			value_state_storage[1]=value_state_storage[0];
+			value_state_storage[0]=value_state_n;
+		}
+
+	}
+	else
+		value_count=0;
+
+	if	(value_state_valid)
+	{
+		value_state_valid=0;
+		if	((value_state_storage[2]==0)&&(value_state_storage[0]==0))
+			act=value_state_storage[1]+value_state_storage[1];
+		else if((value_state_storage[1]==-1)&&(value_state_storage[2]==1))
+			act=-1;
+		else if((value_state_storage[1]==1)&&(value_state_storage[2]==-1))
+			act=1;
+		else
+			act=0;
+	}
+	else
+		act=0;
+	value_state_l=value_state_n;
+	return act;
+//	return value_last;
+}
diff --git a/algorithm.h b/algorithm.h
new file mode 100644
index 0000000..1bdb342
--- /dev/null
+++ b/algorithm.h
@@ -0,0 +1,40 @@
+/**
+  ******************************************************************************
+  * File Name          : app_mems.h
+  * Description        : This file provides code for the configuration
+  *                      of the STMicroelectronics.X-CUBE-MEMS1.10.0.0 instances.
+  ******************************************************************************
+  * @attention
+  *
+  * Copyright (c) 2023 STMicroelectronics.
+  * All rights reserved.
+  *
+  * This software is licensed under terms that can be found in the LICENSE file
+  * in the root directory of this software component.
+  * If no LICENSE file comes with this software, it is provided AS-IS.
+  *
+  ******************************************************************************
+  */
+
+/* Define to prevent recursive inclusion -------------------------------------*/
+#ifndef __ALGORITHM_H
+#define __ALGORITHM_H
+#include <stdio.h>
+
+
+#ifndef _INT8_T_DECLARED
+typedef __int8_t int8_t ;
+#define _INT8_T_DECLARED
+#endif
+#ifndef _INT16_T_DECLARED
+typedef __int16_t int16_t ;
+#define _INT16_T_DECLARED
+#endif
+
+/* Includes ------------------------------------------------------------------*/
+
+/* Exported defines ----------------------------------------------------------*/
+int qvar_digital(int16_t value);
+/* Exported functions --------------------------------------------------------*/
+
+#endif /* __APP_MEMS_H */