添加标定加速度计的机械热噪声

User/cfg.c

@@ -32,6 +32,11 @@ int Config_Init(void)
 			
 				Factory_reset();
 			
+				//设备内参， 不加入出厂设置
+				config->xaxis_threshold = 0.0;
+				config->yaxis_threshold = 0.0;
+				config->zaxis_threshold = 0.0;
+			
 				//
 				config->IapFlag = Startup_Normal;
 				config->hw_version = 0x0000;

User/cfg.h

@@ -77,7 +77,13 @@ typedef struct
     uint8_t  addr;
     uint8_t  br_index;
 	
-		uint32_t  res[58];
+
+		uint32_t  res[55];
+	
+		//三个轴向上的机械热噪声阈值
+		float   xaxis_threshold;
+		float   yaxis_threshold;
+		float   zaxis_threshold;
 	
 		uint16_t hw_version;
 		uint16_t devicetype;

User/main_task.c

@@ -134,6 +134,10 @@ void Uart1_RxDataHandle(void)
 										case Cmd_TriggerSendFiltered:
 											ret_write = Trigger_SendFiltered(rxbuf+4, rx_len-4-2);
 											break;
+										case Cmd_CalibrateNoise:
+											break;
+										case Cmd_GetNoisethreshold:
+											break;
 										
 										case Cmd_IapUpgrade:
 											tx_len += IAP_CmdHandle(rxbuf+tx_len, rx_len-4-2, txbuf+tx_len);

User/process.c

@@ -6,6 +6,7 @@
 #include "crc16.h"
 #include "string.h"
 #include "math.h"
+#include "cfg.h"
 
 
 uint16_t	g_blinkLedTime;			/*RUN LED闪烁频率控制时间*/
@@ -40,13 +41,8 @@ static int16_t acc_x = 0;
 static int16_t acc_y = 0;
 static int16_t acc_z = 0;
 
-//static float acc_fx = 0;
-//static float acc_fy = 0;
-//static float acc_fz = 0;
-
-//static float fx_sum = 0;
-//static float fy_sum = 0;
-//static float fz_sum = 0;
+uint8_t g_autocalibration = 0;
+uint16_t g_samplecount = 0;
 
 uint8_t g_send_raw = 0;
 uint8_t g_send_filtered = 0;
@@ -142,6 +138,9 @@ void Process_Init(void)
 		cbuffer.write_index = 0;
 	
 		filter_avg.count = 0;
+		
+		g_autocalibration=0;
+		g_samplecount = 0;
 	
 	
 }
@@ -240,6 +239,66 @@ int cbuffer_popdata(int16_t* acc_x, int16_t* acc_y, int16_t* acc_z)
 }
 
 
+void autocaculate_mtnoise(axis_info_t* paxis_info)
+{
+		//复用测速 buffer 
+		// 以防精度不够用 double 类型
+		double accx_average = 0.0;
+		double accy_average = 0.0;
+		double accz_average = 0.0;
+	
+		double accx_sd = 0.0;
+		double accy_sd = 0.0;
+		double accz_sd = 0.0;
+	
+		uint16_t i=0;
+
+		detect_info.info[g_samplecount].x = paxis_info->x;
+		detect_info.info[g_samplecount].y = paxis_info->y;
+		detect_info.info[g_samplecount].z = paxis_info->z;
+	
+		g_samplecount++;
+		if(DETECT_CNT == g_samplecount){
+			
+			for(i=0; i < DETECT_CNT; i++){
+				accx_average += detect_info.info[i].x;
+				accy_average += detect_info.info[i].y;
+				accz_average += detect_info.info[i].z;
+			}
+			
+			accx_average = accx_average/DETECT_CNT;
+			accy_average = accy_average/DETECT_CNT;
+			accz_average = accz_average/DETECT_CNT;
+			
+			for(i=0; i < DETECT_CNT; i++){
+				accx_sd += pow(detect_info.info[i].x - accx_average, 2)/DETECT_CNT;
+				accy_sd += pow(detect_info.info[i].y - accy_average, 2)/DETECT_CNT;
+				accz_sd += pow(detect_info.info[i].z - accz_average, 2)/DETECT_CNT;
+				
+			}
+			
+			accx_sd = pow(accx_sd, 0.5);
+			accy_sd = pow(accy_sd, 0.5);
+			accz_sd = pow(accz_sd, 0.5);
+			
+			//3 sigma 
+			//设备内参， 不加入出厂设置
+			config->xaxis_threshold = 3*fabs(accx_sd);
+			config->yaxis_threshold = 3*fabs(accy_sd);
+			config->zaxis_threshold = 3*fabs(accz_sd);
+			SaveConfig();
+			
+			g_samplecount=0;
+			g_autocalibration=0;
+			
+		}
+	
+		
+		
+
+}
+
+
 void alg_PreProcess(void)
 {
 		static uint8_t _bvopen = BV_CLOSE;
@@ -329,13 +388,13 @@ void alg_detect(axis_info_t* paxis_info)
 		paxis_info->z  -= g_alg_context.axis_offset.z;
 	
 		//机械滤波
-		if(fabs(paxis_info->x) <= MECHANICAL_NOISE_THRESHOLD){
+		if(fabs(paxis_info->x) <= config->xaxis_threshold){
 				paxis_info->x = 0.0;
 		}
-		if(fabs(paxis_info->y) <= MECHANICAL_NOISE_THRESHOLD){
+		if(fabs(paxis_info->y) <= config->yaxis_threshold){
 				paxis_info->y = 0.0;
 		}
-		if(fabs(paxis_info->z) <= MECHANICAL_NOISE_THRESHOLD){
+		if(fabs(paxis_info->z) <= config->zaxis_threshold){
 				paxis_info->z = 0.0;
 		}
 		
@@ -554,29 +613,34 @@ void Process_Alg(void)
 				uint32_t start =  OSIF_GetMilliseconds();
 
 				//printf("alg:%d ms st: %d \r", start, g_alg_context.state);
+			if(1 == g_autocalibration ){
+					autocaculate_mtnoise(&axis_info);
+				
+			}else{
 			
-			switch(g_alg_context.state){
-				case ALG_Start:
-					alg_start();
-					break;
-				case ALG_Calibrate:
-					alg_calibrate(&axis_info);
-					break;
-				case ALG_Detecting:
-					alg_detect(&axis_info);
-					break;
-				case ALG_Triggering:
-					alg_Triggering();
-					break;
-				case ALG_Finished:
-					alg_Finished();
-					break;
-				default:
-					break;
+				switch(g_alg_context.state){
+					case ALG_Start:
+						alg_start();
+						break;
+					case ALG_Calibrate:
+						alg_calibrate(&axis_info);
+						break;
+					case ALG_Detecting:
+						alg_detect(&axis_info);
+						break;
+					case ALG_Triggering:
+						alg_Triggering();
+						break;
+					case ALG_Finished:
+						alg_Finished();
+						break;
+					default:
+						break;
+				}
+				
 			}
 			
-		#if 1
-		
+	#if 1
 			if(g_send_raw){
 				
 					d_buffer[0] = 0xAA;

User/process.h

@@ -46,6 +46,9 @@ extern "C" {
 extern uint8_t	  g_bvopen;  // 0: close , 1: open
 extern uint8_t   	g_trigger_vo;
 
+extern uint8_t g_autocalibration;
+extern uint16_t g_samplecount;
+
 extern uint8_t g_send_raw;
 extern uint8_t g_send_filtered;
 extern uint32_t g_send_sequence;
@@ -67,7 +70,7 @@ extern uint32_t g_send_sequence;
 #define ALG_Triggering    (4)
 #define ALG_Finished    	(5)
 
-#define CALIBRATE_COUNT    (5*100)   //10ÃëÖÓ
+#define CALIBRATE_COUNT    (5*100)   //5ÃëÖÓ
 #define RUNDELAY_COUNT			(5*60*100) //5·ÖÖÓ
 
 

User/protocol.c

@@ -114,6 +114,30 @@ uint16_t Read_BvStatus(uint8_t *pBuf, uint16_t buf_len)
 
 }
 
+uint16_t Read_NoiseThreshold(uint8_t *pBuf, uint16_t buf_len)
+{
+	if( buf_len < 12){
+		return 0;
+	}
+	
+	pBuf[0] = ((uint8_t*)(&config->xaxis_threshold))[0];
+	pBuf[1] = ((uint8_t*)(&config->xaxis_threshold))[1];
+	pBuf[2] = ((uint8_t*)(&config->xaxis_threshold))[2];
+	pBuf[3] = ((uint8_t*)(&config->xaxis_threshold))[3];
+	
+	pBuf[5] = ((uint8_t*)(&config->yaxis_threshold))[0];
+	pBuf[6] = ((uint8_t*)(&config->yaxis_threshold))[1];
+	pBuf[7] = ((uint8_t*)(&config->yaxis_threshold))[2];
+	pBuf[8] = ((uint8_t*)(&config->yaxis_threshold))[3];
+	
+	pBuf[9] = ((uint8_t*)(&config->zaxis_threshold))[0];
+	pBuf[10] = ((uint8_t*)(&config->zaxis_threshold))[1];
+	pBuf[11] = ((uint8_t*)(&config->zaxis_threshold))[2];
+	pBuf[12] = ((uint8_t*)(&config->zaxis_threshold))[3];
+	
+	return 12;
+}
+
 
 
 
@@ -254,5 +278,18 @@ uint8_t Trigger_SendFiltered(uint8_t *pdata, uint8_t len)
 	}
 }
 
-
+//触发标定加速度计的机械热噪声
+uint8_t Trigger_CalibrateNoise(uint8_t *pdata, uint8_t len)
+{
+	if(len == 2){
+		
+		g_autocalibration = 1;
+		g_samplecount = 0;
+		
+		return RET_OK;
+		
+	}else{
+		return RET_DATAINVALID;
+	}
+}
 

User/protocol.h

@@ -57,7 +57,8 @@ extern "C" {
 #define Cmd_TriggerVO    		 		(0x0023)
 #define Cmd_TriggerSendRaw    	(0x0024)
 #define Cmd_TriggerSendFiltered (0x0025)
-
+#define Cmd_CalibrateNoise			(0x0026)
+#define Cmd_GetNoisethreshold		(0x0027)
 
 #define Cmd_IapUpgrade    		 (0xAABB)
 
@@ -80,7 +81,7 @@ uint16_t Read_Devicetype(uint8_t *pBuf, uint16_t buf_len);
 uint16_t Read_Addr(uint8_t *pBuf, uint16_t buf_len);
 uint16_t Read_Baudrate(uint8_t *pBuf, uint16_t buf_len); 
 uint16_t Read_BvStatus(uint8_t *pBuf, uint16_t buf_len);
-
+uint16_t Read_NoiseThreshold(uint8_t *pBuf, uint16_t buf_len);
 
 
 #define RET_ERROR_MASK     (0xF0)
@@ -111,6 +112,9 @@ uint8_t Trigger_SendRaw(uint8_t *pdata, uint8_t len);
 
 //发送滤波后数据
 uint8_t Trigger_SendFiltered(uint8_t *pdata, uint8_t len); 
+
+//触发标定加速度计的机械热噪声
+uint8_t Trigger_CalibrateNoise(uint8_t *pdata, uint8_t len); 
     
 #ifdef __cplusplus
 }