Przeglądaj źródła

新的电流采样方式

Signed-off-by: huhui <huhui@sharkgulf.com>
huhui 3 lat temu
rodzic
commit
58e162944e

+ 2 - 2
Applications/app/app.c

@@ -54,8 +54,8 @@ void fetch_jtag_cmd(void){
 #endif
 
 static void mc_exec_log(void) {
-	sys_debug("intval = %d, exec = %d, err = %d, %d, %d, count = %d\n", g_meas_foc.intval_time, g_meas_foc.exec_time, g_meas_foc.intval_time_error, g_meas_foc.intval_low_err, g_meas_foc.intval_hi_err, g_meas_foc.exec_count);
-	sys_debug("timeup intval = %d, err = %d\n\n", g_meas_timeup.intval_time, g_meas_timeup.intval_time_error);
+//	sys_debug("intval = %d, exec = %d, err = %d, %d, %d, count = %d\n", g_meas_foc.intval_time, g_meas_foc.exec_time, g_meas_foc.intval_time_error, g_meas_foc.intval_low_err, g_meas_foc.intval_hi_err, g_meas_foc.exec_count);
+//	sys_debug("timeup intval = %d, err = %d\n\n", g_meas_timeup.intval_time, g_meas_timeup.intval_time_error);
 	PMSM_FOC_LogDebug();
 }
 

+ 2 - 1
Applications/bsp/bsp.h

@@ -48,7 +48,7 @@
 #define TRise NS_2_TCLK(HW_RISE_TIME_NS)/* MOS ����ʱ��*/
 #define TNoise NS_2_TCLK(HW_NOISE_TIME_NS)/* MOS��������Ŀ�������ʱ�� */
 #define TADC  ((uint16_t)((ADC_TRIG_CONV_LATENCY_CYCLES + ADC_SAMPLING_CYCLES) * TIM_CLOCK_MHz) / ADC_CLOCK_MHz + 1u)/* ADC ����ʱ�� */
-#define TSampleMIN (TADC + TDead*2 + TRise) //采样需要的总时间
+#define TSampleMIN (TDead + TRise + TADC) //采样需要的总时间
 #define TSampleBefore (TDead + TRise) //采样开始前需要等待的时间
 
 
@@ -71,6 +71,7 @@
 #define RTC_IRQ_PRIORITY       7 
 #define UART_IRQ_PRIORITY      6 
 
+//#define ENABLE_PWM_UP_IRQ 1
 
 #define THREE_SHUNTS_SAMPLE 1
 #define ONE_SHUNT_SAMPLE 2

+ 1 - 1
Applications/bsp/delay.c

@@ -40,7 +40,7 @@ void task_udelay(uint32_t delay)
 
 void delay_us(uint16_t cnt)
 {
-	task_udelay(1);
+	task_udelay(cnt);
 }
 
 /*!

+ 5 - 1
Applications/bsp/pwm.c

@@ -185,7 +185,9 @@ static void _init_pwm_timer(void) {
 
 	timer_interrupt_disable(timer, TIMER_INT_UP);
 	timer_interrupt_flag_clear(timer, TIMER_INT_FLAG_UP);
-	nvic_irq_enable(TIMER0_UP_IRQn, TIMER_UP_IRQ_PRIORITY, 0);	
+#ifdef ENABLE_PWM_UP_IRQ	
+	nvic_irq_enable(TIMER0_UP_IRQn, TIMER_UP_IRQ_PRIORITY, 0);
+#endif
 #if SHUNT_NUM==THREE_SHUNTS_SAMPLE	
     timer_enable(timer);
 #endif
@@ -331,7 +333,9 @@ void pwm_start(void){
 	timer_primary_output_config(adc_timer,ENABLE);
 #endif
 	timer_flag_clear(pwm_timer, TIMER_FLAG_UP);
+#ifdef ENABLE_PWM_UP_IRQ
 	timer_interrupt_enable(pwm_timer, TIMER_INT_UP);
+#endif
 }
 
 void pwm_stop(void){

+ 2 - 0
Applications/bsp/pwm.h

@@ -71,6 +71,8 @@ extern u16 timer_update_buffer[6];
         	TIMER_CHCTL1(pwm_timer) |= (uint32_t)((uint32_t)(m) << 8U); \
 		} \
 	}while(0)
+
+
 void pwm_3phase_init(void);
 void pwm_start(void);
 void pwm_stop(void);

+ 5 - 2
Applications/foc/core/PMSM_FOC_Core.c

@@ -196,6 +196,9 @@ void PMSM_FOC_Schedule(void) {
 	_gFOC_Ctrl.ctrl_count++;
 
 	PMSM_FOC_Update_Hardware();
+	if (_gFOC_Ctrl.ctrl_count % 5 == 0) {
+		plot_3data16(FtoS16x1000(iabc[0]), FtoS16x1000(iabc[1]), _gFOC_Ctrl.out.test_sample * 100);
+	}
 
 	if (_gFOC_Ctrl.out.n_RunMode != CTRL_MODE_OPEN) {
 
@@ -235,10 +238,10 @@ void PMSM_FOC_Schedule(void) {
 		//plot_1data16(FtoS16x1000(PMSM_FOC_Get_iDC()));
 		//plot_3data16(_gFOC_Ctrl.out.n_Duty[0], _gFOC_Ctrl.out.n_Duty[1], _gFOC_Ctrl.out.n_Duty[2]);
 		//plot_2data16(FtoS16x1000(_gFOC_Ctrl.out.s_RealIdq.d), FtoS16x1000(_gFOC_Ctrl.out.s_RealIdq.q));
-		plot_2data16(_gFOC_Ctrl.in.s_motRPM, FtoS16x1000(_gFOC_Ctrl.in.s_targetTorque));
+		//plot_2data16(_gFOC_Ctrl.in.s_motRPM, FtoS16x1000(_gFOC_Ctrl.in.s_targetTorque));
 		//plot_2data16(FtoS16(_gFOC_Ctrl.in.s_hallAngle), FtoS16(_gFOC_Ctrl.in.s_motAngle));
 		//plot_3data16(_gFOC_Ctrl.in.s_motRPM, FtoS16x1000(_gFOC_Ctrl.out.s_OutVdq.d), FtoS16x1000(_gFOC_Ctrl.out.s_OutVdq.q));
-		//plot_3data16(FtoS16x1000(iabc[0]), FtoS16x1000(iabc[1]), FtoS16(_gFOC_Ctrl.in.s_hallAngle)*10);
+		//plot_3data16(FtoS16x1000(iabc[0]), FtoS16x1000(iabc[1]), FtoS16x1000(iabc[2]));
 		//plot_1data16(FtoS16(_gFOC_Ctrl.in.s_hallAngle));
 	}	
 }

+ 1 - 0
Applications/foc/core/PMSM_FOC_Core.h

@@ -61,6 +61,7 @@ typedef struct {
 	DQ_t  s_FilterIdq;
 	float s_FilteriDC;
 	float f_vdqRation;
+	s16   test_sample;
 	u8    n_Error;
 }FOC_OutP;
 

+ 1 - 1
Applications/foc/foc_config.h

@@ -34,7 +34,7 @@
 #define VDQ_RAMP_FINAL_TIME 3000
 #define CURRENT_BANDWITH 1000 /* 电流环带宽 */
 
-#define SVM_Modulation (0.85f)
+#define SVM_Modulation (0.96f)
 
 #endif /* _FOC_CONFIG_H__ */
 

+ 207 - 46
Applications/foc/motor/current.c

@@ -5,7 +5,7 @@
 #include "foc/core/PMSM_FOC_Core.h"
 #include "libs/utils.h"
 #include "libs/logger.h"
-#include "math/fix_math.h"
+#include "math/fast_math.h"
 static current_samp_t g_cs;
 
 #define NB_OFFSET_SAMPLES 32
@@ -43,6 +43,7 @@ void phase_current_start_cali(void){
 	phase_current_init();
 	g_cs.is_calibrating_offset = true;
 	g_cs.c_phases = PHASE_AB;
+	g_cs.c_ignore_phase = IGNORE_NONE;
 	adc_current_sample_config(g_cs.c_phases);
 }
 
@@ -94,6 +95,7 @@ bool phase_current_offset(void) {
 }
 
 
+#if 0
 void phase_current_get(float *iABC){
 	current_samp_t *cs = &g_cs;
 	s32 phase_current1, phase_current2;
@@ -124,51 +126,6 @@ void phase_current_get(float *iABC){
 	iABC[2] = -cs->adc_ic * ADC_TO_CURR_ceof;
 }
 
-#if 0
-void phase_current_point(void *p){
-	FOC_OutP *out = p;
-	current_samp_t *cs = &g_cs;
-	u32 low_side_low_duty = FOC_PWM_Half_Period - out->n_lowDuty;
-	u32 low_side_mid_duty = FOC_PWM_Half_Period - out->n_midDuty;
-	cs->sector = out->n_Sector;
-	out->n_Sample1 = FOC_PWM_Half_Period + 1;
-	out->n_Sample2 = FOC_PWM_Half_Period + 1;
-	s16 potest = 7;
-	/*底边开mos的时间是2倍的 low_side_low_duty(一个周期)*/
-	if (low_side_low_duty * 2 >= TSampleMIN) { //可以采样
-		if (low_side_low_duty >= (TADC + TDead)) {//可以在pwm的中心点采样
-			out->n_Sample1 = FOC_PWM_Half_Period - 1;
-			cs->sector = SECTOR_1;
-			potest = 1;
-		}else {
-			u32 Samp_p = out->n_lowDuty + TSampleBefore;
-			if (Samp_p >= FOC_PWM_Half_Period) { //需要在pwm中心点过后采样,需要配置PWM0模式
-				out->n_Sample2 = ( 2u * FOC_PWM_Half_Period ) - Samp_p - (uint16_t) 1;
-				potest = 2;
-			}else {
-				out->n_Sample1 = Samp_p;
-				potest = 3;
-			}
-		}
-	}else if (low_side_mid_duty * 2 >= TSampleMIN){
-		if (low_side_mid_duty >= (TADC + TDead)) {//可以在pwm的中心点采样
-			out->n_Sample1 = FOC_PWM_Half_Period - 1;
-			potest = 4;
-		}else {
-			u32 Samp_p = out->n_midDuty + TSampleBefore;
-			if (Samp_p >= FOC_PWM_Half_Period) { //需要在pwm中心点过后采样,需要配置PWM0模式
-				out->n_Sample2 = ( 2u * FOC_PWM_Half_Period ) - Samp_p - (uint16_t) 1;
-				potest = 5;
-			}else {
-				out->n_Sample1 = Samp_p;
-				potest = 6;
-			}
-		}
-	}
-	plot_1data16(potest);
-	out->n_Sector = cs->sector;
-}
-#else
 static __inline__ s16 get_mid_duty(u32 max_hi_side, FOC_OutP *out) {
 	u32 min_low_side = FOC_PWM_Half_Period - max_hi_side;
 	if (min_low_side >= MAX(TSampleBefore, TADC)) { //可以采样
@@ -219,6 +176,210 @@ void phase_current_point(void *p){
 	}
 	out->n_CPhases = cs->c_phases;
 }
+#else
+#define LOW_FP_COEF 1.0f
+void phase_current_get(float *iABC){
+	current_samp_t *cs = &g_cs;
+	s32 phase_current1, phase_current2;
+
+	adc_phase_current_read(cs->c_phases, &phase_current1, &phase_current2);
+
+	if (cs->c_phases == PHASE_AB) {
+		/* Current on Phase C is not accessible */
+		/* Ia = PhaseAOffset - ADC converted value) */
+		cs->adc_ia = (phase_current1 - cs->adc_offset_a);
+		cs->adc_ib = (phase_current2 - cs->adc_offset_b);
+
+		if (cs->c_ignore_phase == IGNORE_NONE) {
+			LowPass_Filter(cs->adc_ia_filter, cs->adc_ia, LOW_FP_COEF);
+			LowPass_Filter(cs->adc_ib_filter, cs->adc_ib, LOW_FP_COEF);
+			LowPass_Filter(cs->adc_ic_filter, -(cs->adc_ia + cs->adc_ib), LOW_FP_COEF);
+		}else if (cs->c_ignore_phase == IGNORE_A) {
+			LowPass_Filter(cs->adc_ib_filter, cs->adc_ib, LOW_FP_COEF);
+			cs->adc_ia = cs->adc_ia_filter;
+		}else if (cs->c_ignore_phase == IGNORE_B) {
+			LowPass_Filter(cs->adc_ia_filter, cs->adc_ia, LOW_FP_COEF);
+			cs->adc_ib = cs->adc_ib_filter;
+		}else if (cs->c_ignore_phase == IGNORE_ALL) {
+			cs->adc_ia = cs->adc_ia_filter;
+			cs->adc_ib = cs->adc_ib_filter;
+		}
+		cs->adc_ic = -(cs->adc_ia + cs->adc_ib);
+	}else if (cs->c_phases == PHASE_BC) {
+		/* Current on Phase A is not accessible 	*/
+		/* Ib = PhaseBOffset - ADC converted value) */
+		cs->adc_ib = (phase_current1 - cs->adc_offset_b);
+		cs->adc_ic = (phase_current2 - cs->adc_offset_c);
+		
+		if (cs->c_ignore_phase == IGNORE_NONE) {
+			LowPass_Filter(cs->adc_ib_filter, cs->adc_ib, LOW_FP_COEF);
+			LowPass_Filter(cs->adc_ic_filter, cs->adc_ic, LOW_FP_COEF);
+			LowPass_Filter(cs->adc_ia_filter, -(cs->adc_ib + cs->adc_ic), LOW_FP_COEF);
+		}else if (cs->c_ignore_phase == IGNORE_B) {
+			LowPass_Filter(cs->adc_ic_filter, cs->adc_ic, LOW_FP_COEF);
+			cs->adc_ib = cs->adc_ib_filter;
+		}else if (cs->c_ignore_phase == IGNORE_C) {
+			LowPass_Filter(cs->adc_ib_filter, cs->adc_ib, LOW_FP_COEF);
+			cs->adc_ic = cs->adc_ic_filter;
+		}else if (cs->c_ignore_phase == IGNORE_ALL) {
+			cs->adc_ib = cs->adc_ib_filter;
+			cs->adc_ic = cs->adc_ic_filter;
+		}
+
+		cs->adc_ia = -(cs->adc_ib + cs->adc_ic);
+	}else if (cs->c_phases == PHASE_AC) {
+		/* Current on Phase B is not accessible 	*/
+		/* Ia = PhaseAOffset - ADC converted value) */
+		cs->adc_ia = (phase_current1 - cs->adc_offset_a);
+		cs->adc_ic = (phase_current2 - cs->adc_offset_c);
+		if (cs->c_ignore_phase == IGNORE_NONE) {
+			LowPass_Filter(cs->adc_ia_filter, cs->adc_ia, LOW_FP_COEF);
+			LowPass_Filter(cs->adc_ic_filter, cs->adc_ic, LOW_FP_COEF);
+			LowPass_Filter(cs->adc_ib_filter, -(cs->adc_ia + cs->adc_ic), LOW_FP_COEF);
+		}else if (cs->c_ignore_phase == IGNORE_A) {
+			LowPass_Filter(cs->adc_ic_filter, cs->adc_ic, LOW_FP_COEF);
+			cs->adc_ia = cs->adc_ia_filter;
+		}else if (cs->c_ignore_phase == IGNORE_C) {
+			LowPass_Filter(cs->adc_ia_filter, cs->adc_ia, LOW_FP_COEF);
+			cs->adc_ic = cs->adc_ic_filter;
+		}else if (cs->c_ignore_phase == IGNORE_ALL) {
+			cs->adc_ia = cs->adc_ia_filter;
+			cs->adc_ic = cs->adc_ic_filter;
+		}
+
+		cs->adc_ib = -(cs->adc_ia + cs->adc_ic);
+	}
+	iABC[0] = -cs->adc_ia * ADC_TO_CURR_ceof;
+	iABC[1] = -cs->adc_ib * ADC_TO_CURR_ceof;
+	iABC[2] = -cs->adc_ic * ADC_TO_CURR_ceof;
+}
+
+void phase_current_point(void *p){
+	FOC_OutP *out = p;
+	current_samp_t *cs = &g_cs;
+	u16 *duty = out->n_Duty;
+	s16  test_sample = 0;
+	u16 max_hi_side = MAX(duty[0], MAX(duty[1], duty[2]));
+	u16 min_low_side = FOC_PWM_Half_Period - max_hi_side;
+
+	out->n_Sample1 = FOC_PWM_Half_Period + 1;
+	out->n_Sample2 = FOC_PWM_Half_Period + 1;
+
+	cs->c_ignore_phase = IGNORE_NONE;
+	
+	if (min_low_side > TSampleMIN) {
+		u16 Samp_p = max_hi_side + TSampleBefore;
+		if (Samp_p >= FOC_PWM_Half_Period) { //需要在pwm中心点过后采样,需要配置PWM0模式
+			out->n_Sample2 = ( 2u * FOC_PWM_Half_Period ) - Samp_p - (uint16_t) 1;
+			test_sample = 1;
+		}else {
+			out->n_Sample1 = Samp_p;
+			test_sample = 2;
+		}
+		cs->c_phases = PHASE_AB;
+	}else {
+		u16 lowA = FOC_PWM_Half_Period - duty[0];
+		u16 lowB = FOC_PWM_Half_Period - duty[1];
+		u16 lowC = FOC_PWM_Half_Period - duty[2];
+		u16 t2, t3, t_pointer, m = 0;
+		if (max_hi_side == duty[0]) { //A 相底边开通时间最短
+			cs->c_phases = PHASE_BC;
+			if (lowB > lowC) {
+				t2 = (lowC - lowA)/2;
+				t3 = (lowB - lowC)/2;
+				t_pointer = duty[2]; //通过C计算
+			}else {
+				t2 = (lowB - lowA)/2;
+				t3 = (lowC - lowB)/2;
+				t_pointer = duty[1]; //通过B计算
+				m = 1;
+			}
+			
+			if (t2 > TSampleMIN) {
+				out->n_Sample1 = t_pointer + TSampleBefore;
+				test_sample = 3;
+			}else if (t3 > TSampleMIN) {
+				if (m == 0) {
+					out->n_Sample1 = duty[1] + TSampleBefore;
+					cs->c_ignore_phase = IGNORE_C;
+					test_sample = 4;
+				}else {
+					out->n_Sample1 = duty[2] + TSampleBefore;
+					cs->c_ignore_phase = IGNORE_B;
+					test_sample = 5;
+				}
+			}else {
+				out->n_Sample1 = FOC_PWM_Half_Period - 1;
+				cs->c_ignore_phase = IGNORE_ALL;
+				test_sample = 6;
+			}
+		}else if (max_hi_side == duty[1]) { //B 相底边开通时间最短
+			cs->c_phases = PHASE_AC;
+			if (lowA > lowC) {
+				t2 = (lowC - lowB)/2;
+				t3 = (lowA - lowC)/2;
+				t_pointer = duty[2]; //通过C计算
+			}else {
+				t2 = (lowA - lowB)/2;
+				t3 = (lowC - lowA)/2;
+				t_pointer = duty[0]; //通过A计算
+				m = 1;
+			}
+			
+			if (t2 > TSampleMIN) {
+				out->n_Sample1 = t_pointer + TSampleBefore;
+				test_sample = 7;
+			}else if (t3 > TSampleMIN) {
+				if (m == 0) {
+					out->n_Sample1 = duty[0] + TSampleBefore;
+					cs->c_ignore_phase = IGNORE_C;
+					test_sample = 8;
+				}else {
+					out->n_Sample1 = duty[2] + TSampleBefore;
+					cs->c_ignore_phase = IGNORE_A;
+					test_sample = 9;
+				}
+			}else {
+				out->n_Sample1 = FOC_PWM_Half_Period - 1;
+				cs->c_ignore_phase = IGNORE_ALL;
+				test_sample = 10;
+			}
+		}else {  //C 相底边开通时间最短
+			cs->c_phases = PHASE_AB;
+			if (lowA > lowB) {
+				t2 = (lowB - lowC)/2;
+				t3 = (lowA - lowB)/2;
+				t_pointer = duty[1]; //通过B计算
+			}else {
+				t2 = (lowA - lowC)/2;
+				t3 = (lowB - lowA)/2;
+				t_pointer = duty[0]; //通过A计算
+				m = 1;
+			}
+
+			if (t2 > TSampleMIN) {
+				out->n_Sample1 = t_pointer + TSampleBefore;
+				test_sample = 11;
+			}else if (t3 > TSampleMIN) {
+				if (m == 0) {
+					out->n_Sample1 = duty[0] + TSampleBefore;
+					cs->c_ignore_phase = IGNORE_B;
+					test_sample = 12;
+				}else {
+					out->n_Sample1 = duty[1] + TSampleBefore;
+					cs->c_ignore_phase = IGNORE_A;
+					test_sample = 13;
+				}
+			}else {
+				out->n_Sample1 = FOC_PWM_Half_Period - 1;
+				cs->c_ignore_phase = IGNORE_ALL;
+				test_sample = 14;
+			}
+		}
+	}
+	out->n_CPhases = cs->c_phases;
+	out->test_sample = test_sample;
+}
 
 #endif
 

+ 11 - 15
Applications/foc/motor/current.h

@@ -1,22 +1,8 @@
 #ifndef _PHASE_CURRENT_H__
 #define _PHASE_CURRENT_H__
 #include "math/fix_math.h"
-/* boundary zone definition */
-#define REGULAR         ((uint8_t)0u)
-#define BOUNDARY_1      ((uint8_t)1u)  /* Two small, one big */
-#define BOUNDARY_2      ((uint8_t)2u)  /* Two big, one small */
-#define BOUNDARY_3      ((uint8_t)3u)  /* Three equal        */
 
-#define SAMP_NO 0u
-#define SAMP_IA 1u
-#define SAMP_IB 2u
-#define SAMP_IC 3u
-#define SAMP_NIA 4u
-#define SAMP_NIB 5u
-#define SAMP_NIC 6u
-#define SAMP_OLDA 7u
-#define SAMP_OLDB 8u
-#define SAMP_OLDC 9u
+
 typedef struct _phase_time {
 	s32 A;
 	s32 B;
@@ -32,6 +18,12 @@ typedef struct _phase_time {
 	u8  three_shunts_flags;
 }phase_time_t; //����pwn��duty cnt
 
+#define IGNORE_NONE 0
+#define IGNORE_A    1
+#define IGNORE_B    2
+#define IGNORE_C    3
+#define IGNORE_ALL    4
+
 typedef struct current_sample {
 	s32   adc_offset_a;
 	s32   adc_offset_b;
@@ -39,7 +31,11 @@ typedef struct current_sample {
 	s16   adc_ia;
 	s16   adc_ib;
 	s16   adc_ic;
+	s16   adc_ia_filter;
+	s16   adc_ib_filter;
+	s16   adc_ic_filter;
 	u8    c_phases;
+	u8    c_ignore_phase;
 	phase_time_t time;	
 	volatile int   offset_sample_count;
 	volatile bool  is_calibrating_offset;

+ 1 - 1
Applications/foc/motor/motor.c

@@ -204,7 +204,7 @@ static shark_timer_t _brake_timer = TIMER_INIT(_brake_timer, brake_timer_handler
 void MC_Brake_IRQHandler(void){
 	shark_timer_post(&_brake_timer, 0);
 }
-measure_time_t g_meas_timeup = {.intval_max_time = 50, .intval_low_err = 0, .intval_hi_err = 0, .first = true,};
+measure_time_t g_meas_timeup = {.intval_max_time = 62, .intval_low_err = 0, .intval_hi_err = 0, .first = true,};
 void TIMER_UP_IRQHandler(void){
 	//phase_current_adc_triger();
 	time_measure_start(&g_meas_timeup);

+ 2 - 2
Applications/libs/time_measure.c

@@ -26,11 +26,11 @@ void time_measure_start(measure_time_t *m){
 		return;
 	}
 	if (m->intval_time > m->intval_max_time+1) {
-		m->intval_time_error ++;
+		m->intval_time_h_error ++;
 		m->intval_hi_err = m->intval_time;
 	}
 	if (m->intval_time < m->intval_max_time-1) {
-		m->intval_time_error ++;
+		m->intval_time_l_error ++;
 		m->intval_low_err = m->intval_time;
 	}
 }

+ 2 - 1
Applications/libs/time_measure.h

@@ -14,7 +14,8 @@ typedef struct {
 	u32 exec_max_time;
 	u32 exec_max_error_time;
 	u32 exec_time_error;
-	u32 intval_time_error;
+	u32 intval_time_l_error;
+	u32 intval_time_h_error;
 }measure_time_t;
 
 

+ 1 - 1
Applications/math/fix_math.h

@@ -41,7 +41,7 @@ typedef int32_t   s32q19_t;
 #define S16_mul(a, b, q) (((s32)(a)*(b)) >> q)
 
 #define FtoS16(f) ((s16)f)
-#define FtoS16x1000(f) ((s16)(f * 1000))
+#define FtoS16x1000(f) ((s16)(f * 100))
 #define LowPass_Filter_Fix(value, sample, filter_constant, shift)	(value = ((s32)value * (S16Q14(1) - filter_constant) + (s32)sample * filter_constant) >> (shift))
 
 #define MIN_S16Q5 (1.0F/32.0F)