phase current samples

Signed-off-by: huhui <huhui@sharkgulf.com>

Application/App/app.c

@@ -8,7 +8,7 @@ void app_init(void){
 void start_stop_handler(void) {
 	FOCState s = FOC_STM_State();
 	if (s == IDLE) {
-		Foc_Set_StartRamp(8.0f, 1000);
+		Foc_Set_StartRamp(8.0f, 100);
 		foc_start_motor();
 	}else {
 		foc_stop_motor();

Application/Bsp/foc_irqs.c

@@ -2,6 +2,7 @@
 #include "stm32f3xx_ll_tim.h"
 #include "stm32f3xx_ll_adc.h"
 #include "stm32f3xx_ll_exti.h"
+#include "libs/types.h"
 
 __weak void foc_brake_handler(void) {}
 __weak void foc_pwm_up_handler(void) {}

Application/FOC/foc.c

@@ -16,13 +16,16 @@ static void foc_defulat_value(void);
 static motor_foc_t mFOC;
 
 void foc_init(void) {
-	foc_defulat_value();
-	hall_sensor_init();
+	foc_defulat_value();	
 	HAL_ADC1_Enable();
 	/* init pwm hardware timer */
 	PWM_TimerEnable();
 	/* enable tim4 to run the foc normal task */
 	TIM4_Enable();
+
+	hall_sensor_init();
+	vbus_sensor_init();
+	ntc_sensor_init();
 	
 	task_start(foc_measure_task, 0);
 }
@@ -71,12 +74,12 @@ FError FOC_STM_NextState(FOCState state) {
 		if (mFOC.state == CURRENT_CALIBRATE) {
 			changed = true;
 		}
-	}else if (state == RAMPINT_START) {
+	}else if (state == RAMPING_START) {
 		if (mFOC.state == READY_TO_RUN) {
 			changed = true;
 		}
 	}else if (state == RUNNING) {
-		if (mFOC.state == RAMPINT_START) {
+		if (mFOC.state == RAMPING_START) {
 			changed = true;
 		}
 	}
@@ -100,15 +103,42 @@ FError foc_stop_motor(void) {
 	return FOC_STM_NextState(ANY_STOP);
 }
 
+void foc_current_calibrate(void){
+	mFOC.current_samp.adc_offset_a = 0;
+	mFOC.current_samp.adc_offset_b = 0;
+	mFOC.current_samp.adc_offset_c = 0;
+	PWM_Disable_Channels();
+	foc_pwm_start(false);
+
+	task_udelay(10);
+	
+	phase_current_init(&mFOC.current_samp);
+	mFOC.current_samp.is_calibrating = true;
+	mFOC.current_samp.sector = SECTOR_5;
+	foc_pwm_start(true);
+	while(mFOC.current_samp.offset_sample_count == 0){};
+	
+	foc_pwm_start(false);
+	phase_current_init(&mFOC.current_samp);	
+	mFOC.current_samp.sector = SECTOR_1;
+	foc_pwm_start(true);
+	while(mFOC.current_samp.offset_sample_count == 0){};
+	foc_pwm_start(false);
+	PWM_Enable_Channels();
+}
+
 void foc_overide_theta(bool enable){
 	mFOC.override.is_theta = enable;
 }
+
 void foc_overide_vdq(bool enable){
 	mFOC.override.is_vdq = enable;
 }
+
 void foc_overide_set_theta(float theta){
 	mFOC.override.theta = theta;
 }
+
 void foc_overide_set_vdq(float d, float q){
 	mFOC.override.vdq.d = d;
 	mFOC.override.vdq.q = q;
@@ -131,7 +161,11 @@ void foc_pwm_up_handler(void){
 
 
 void current_sample_handler(void) {
-	FOC_Fast_Task(&mFOC);
+	if (mFOC.current_samp.is_calibrating) {
+		phase_current_offset(&mFOC.current_samp);
+	}else {
+		FOC_Fast_Task(&mFOC);
+	}
 }
 
 void foc_slow_task_handler(void) {

Application/FOC/foc.h

@@ -17,6 +17,7 @@ void foc_overide_set_vdq(float d, float q);
 FError FOC_STM_NextState(FOCState s);
 void Foc_Set_StartRamp(float final, u32 duration_ms);
 FOCState FOC_STM_State(void);
+void foc_current_calibrate(void);
 
 #endif /* _FOC_H__ */
 

Application/FOC/foc_task.c

@@ -73,18 +73,20 @@ static void __inline Foc_Speed_PI_Control(motor_foc_t *foc) {
 void FOC_Normal_Task(motor_foc_t *foc) {
 	switch (foc->state) {
 		case START:
+			PWM_TurnOnLowSides();
 			FOC_STM_NextState(CURRENT_CALIBRATE);
 			break;
 		case CURRENT_CALIBRATE:
+			foc_current_calibrate();
 			FOC_STM_NextState(READY_TO_RUN);
 			break;
 		case READY_TO_RUN:
 			foc_pwm_start(true);
-			FOC_STM_NextState(RAMPINT_START);
+			FOC_STM_NextState(RAMPING_START);
 			ramp_exc(&foc->start_ramp);
 			foc_overide_vdq(true);
 			break;
-		case RAMPINT_START:
+		case RAMPING_START:
 			foc_overide_set_vdq(0.0f, ramp_get_target(&foc->start_ramp));
 			if (ramp_complete(&foc->start_ramp)) {
 				FOC_STM_NextState(RUNNING);
@@ -94,6 +96,7 @@ void FOC_Normal_Task(motor_foc_t *foc) {
 			Foc_Speed_PI_Control(foc);
 			break;
 		case ANY_STOP:
+			ramp_clear(&foc->start_ramp);
 			foc_clear();
 			FOC_STM_NextState(IDLE);
 			break;

Application/FOC/foc_type.h

@@ -42,7 +42,7 @@ typedef enum {
 	CURRENT_CALIBRATE = 2,
 	CHARGER_BOOT_CAP = 3,
 	READY_TO_RUN = 4,
-	RAMPINT_START = 5,
+	RAMPING_START = 5,
 	RUNNING = 6,
 	ANY_STOP = 7
 }FOCState;
@@ -64,6 +64,8 @@ typedef struct current_sample {
 	float ic;
 	u8    sector;
 	u32   adc_inject_flags;
+	int   offset_sample_count;
+	bool  is_calibrating;
 }current_samp_t;
 
 typedef struct _override {

Application/FOC/ntc_sensor.c

@@ -16,7 +16,6 @@ void ntc_sensor_sample(void){
     w_temp = w_temp / 65536 + ( s32 )( T0_C );
 
 	_ntc.temp_avg = w_temp * _ntc.low_pass_filter + _ntc.temp_avg * (1.0f - _ntc.low_pass_filter);
-
 }
 
 

Application/FOC/phase_current.c

@@ -1,6 +1,7 @@
 #include "hal/adc.h"
 #include "foc_type.h"
 
+#define NB_OFFSET_SAMPLES 32
 static float __inline adc_to_current(u32 adc){
 	int i_adc = (int)adc;
 	if (i_adc > INT16_MAX){
@@ -8,11 +9,33 @@ static float __inline adc_to_current(u32 adc){
 	}else if (i_adc < -INT16_MAX) {
 		i_adc = - INT16_MAX;
 	}
-	return (i_adc/4095.0f * 3.3f / 0.001f);
+	return (i_adc/65535.0f * 3.3f / 1.53f / 0.33f);
 }
 
 void phase_current_init(current_samp_t *cs) {
 	cs->adc_inject_flags = LL_ADC_INJ_TRIG_EXT_RISING;
+	cs->offset_sample_count = NB_OFFSET_SAMPLES;
+}
+
+
+void phase_current_offset(current_samp_t *cs) {
+	u32 phase_current1, phase_current2;
+	HAL_ADC1_Inject_Read(cs->sector, &phase_current1, &phase_current2);
+	cs->offset_sample_count--;
+	if (cs->sector == SECTOR_5 && cs->offset_sample_count >= 0) {
+		cs->adc_offset_a += phase_current1;
+		cs->adc_offset_b += phase_current2;
+		if (cs->offset_sample_count == 0) {
+			cs->adc_offset_a = cs->adc_offset_a / NB_OFFSET_SAMPLES;
+			cs->adc_offset_b = cs->adc_offset_b / NB_OFFSET_SAMPLES;
+		}
+	}
+	if (cs->sector == SECTOR_1 && cs->offset_sample_count >= 0) {
+		cs->adc_offset_c += phase_current2;
+		if (cs->offset_sample_count == 0) {
+			cs->adc_offset_c = cs->adc_offset_c / NB_OFFSET_SAMPLES;
+		}
+	}
 }
 
 void phase_current_sample(current_samp_t *cs){
@@ -37,6 +60,10 @@ void phase_current_sample(current_samp_t *cs){
 		cs->ic = adc_to_current(phase_current2 - cs->adc_offset_c);
 		cs->ib = -(cs->ia + cs->ic);
 	}
+	static int tet_p = 0;
+	if (tet_p++ % 20 == 0) {
+	printf("$%d %d %d;", (int)(cs->ia * 1000), (int)(cs->ib*1000), -(int)(cs->ic*1000));
+		}
 }
 
 

Application/FOC/phase_current.h

@@ -3,6 +3,7 @@
 #include "foc_type.h"
 void phase_current_init(current_samp_t *cs);
 void phase_current_sample(current_samp_t *cs);
+void phase_current_offset(current_samp_t *cs);
 u32 get_phase_sample_point(current_samp_t *cs, phase_time_t *time, u8 sector);
 void phase_current_adc_triger(current_samp_t *cs);
 #endif /* _PHASE_CURRENT_H__ */

Application/FOC/ramp_ctrl.c

@@ -1,18 +1,17 @@
 #include "ramp_ctrl.h"
 
-#define RAMP_INTVAL 5 //ms
-static void ramp_timer_handler(timer_t *timer);
+#define RAMP_INTVAL 1 //ms
+void ramp_timer_handler(timer_t *timer);
 
 void ramp_ctrl_init(ramp_t *ramp, float start, float final, u32 duration_ms){
+	timer_cancel(&ramp->timer);
 	ramp->start_point = start;
 	ramp->target = start;
 	ramp->final_point = final;
 	ramp->duration_ms = duration_ms;
 	ramp->steps = (final - start) / (duration_ms / RAMP_INTVAL);
-	if (ramp->timer.handler != NULL) {
-		timer_cancel(&ramp->timer);
-	}
-	ramp->timer.handler = NULL;
+
+	ramp->timer.handler = ramp_timer_handler;
 }
 
 void ramp_clear(ramp_t *ramp) {
@@ -20,10 +19,7 @@ void ramp_clear(ramp_t *ramp) {
 }
 
 void ramp_exc(ramp_t *ramp){
-	if (ramp->timer.handler == NULL) {
-		ramp->timer.handler = ramp_timer_handler;
-		timer_post(&ramp->timer, RAMP_INTVAL);
-	}
+	timer_post(&ramp->timer, RAMP_INTVAL);
 }
 
 float ramp_get_target(ramp_t *ramp){
@@ -34,7 +30,7 @@ bool ramp_complete(ramp_t *ramp) {
 	return ramp->target == ramp->final_point;
 }
 
-static void ramp_timer_handler(timer_t *timer) {
+void ramp_timer_handler(timer_t *timer) {
 	ramp_t *ramp = (ramp_t *)timer;
 	float target = ramp->target + ramp->steps;
 	if (target > ramp->final_point) {
@@ -43,6 +39,8 @@ static void ramp_timer_handler(timer_t *timer) {
 	ramp->target = target;
 	if (target != ramp->final_point) {
 		timer_post(&ramp->timer, RAMP_INTVAL);
+	}else {
+		timer_cancel(&ramp->timer);
 	}
 }
 

Application/FOC/vbus_sensor.c

@@ -24,7 +24,7 @@ void vbus_sample_voltage(void){
 	}
 	vadc -= (max + min);
 	vadc = vadc / (_vbus.avg_count - 2);
-	_vbus.voltage_avg = vadc/65536 * ADC_REFERENCE_VOLTAGE / VBUS_PARTITIONING_FACTOR;
+	_vbus.voltage_avg = ((float)vadc)/(65536.0f) * ADC_REFERENCE_VOLTAGE / VBUS_PARTITIONING_FACTOR;
 }
 
 

Application/FOC/vbus_sensor.h

@@ -8,7 +8,7 @@
                                                        digital value */
 
 typedef struct {
-	int voltage_avg;
+	float voltage_avg;
 	int avg_count;
 }vbus_t;
 

Application/Hal/pwm.c

@@ -135,11 +135,14 @@ void PWM_TimerEnable(void){
 
 void PWM_UpdateDuty(u32 duty1, u32 duty2, u32 duty3, u32 sample_point ) {
 	TIM_TypeDef * TIMx = htim1.Instance;
-	LL_TIM_DisableUpdateEvent(TIMx);
+
+	//LL_TIM_DisableUpdateEvent(TIMx);
+
 	LL_TIM_OC_SetCompareCH1(TIMx, duty1);
 	LL_TIM_OC_SetCompareCH2(TIMx, duty2);
 	LL_TIM_OC_SetCompareCH3(TIMx, duty3);
 	LL_TIM_OC_SetCompareCH4(TIMx, sample_point);
+#if 0
 	/* wait for a new PWM period */
 	LL_TIM_ClearFlag_UPDATE( TIMx );
 	LL_TIM_EnableUpdateEvent(TIMx);
@@ -149,8 +152,18 @@ void PWM_UpdateDuty(u32 duty1, u32 duty2, u32 duty3, u32 sample_point ) {
 	/* Clear Update Flag */
 	LL_TIM_ClearFlag_UPDATE( TIMx );
 	#endif
+#endif	
 }
 
+void PWM_Disable_Channels(void) {
+	TIM_TypeDef * TIMx = htim1.Instance;
+	LL_TIM_CC_DisableChannel(TIMx, TIMxCCER_MASK_CH123);
+}
+
+void PWM_Enable_Channels(void) {
+	TIM_TypeDef * TIMx = htim1.Instance;
+	LL_TIM_CC_EnableChannel(TIMx, TIMxCCER_MASK_CH123);
+}
 
 void PWM_Start(void) {
 	TIM_TypeDef * TIMx = htim1.Instance;

Application/Hal/pwm.h

@@ -8,6 +8,8 @@ void PWM_Start(void);
 void PWM_Stop(void);
 void PWM_TimerEnable(void);
 void PWM_TurnOnLowSides(void);
+void PWM_Disable_Channels(void);
+void PWM_Enable_Channels(void);
 
 #endif /* _PWM_H__ */
 

Application/Hal/uart2.c

@@ -24,7 +24,7 @@ static void UART2_Pin_Init(void){
 
 void UART2_Init(void){
 	huart2.Instance = USART2;
-	huart2.Init.BaudRate = 115200;
+	huart2.Init.BaudRate = 500000;
 	huart2.Init.WordLength = UART_WORDLENGTH_8B;
 	huart2.Init.StopBits = UART_STOPBITS_1;
 	huart2.Init.Parity = UART_PARITY_NONE;

Application/Libs/task.c

@@ -77,7 +77,6 @@ void timer_post(timer_t *timer, u32 delay)
 	u64 time;
 
 	__disable_irq();
-
 	time = task_mseconds + (delay > 0 ? delay : 1);
 
 	if (timer->prev != NULL) {
@@ -100,19 +99,18 @@ void timer_post(timer_t *timer, u32 delay)
 	timer->next = node;
 	timer->time = time;
 	timer_sync();
-
 	__enable_irq();
 }
 
 void timer_cancel(timer_t *timer)
 {
 	__disable_irq();
-
-	timer->prev->next = timer->next;
-	timer->next->prev = timer->prev;
+	if (timer->prev != NULL && timer->next != NULL) {
+		timer->prev->next = timer->next;
+		timer->next->prev = timer->prev;
+	}
 	timer->next = timer->prev = timer;
 	timer_sync();
-
 	__enable_irq();
 }
 
@@ -154,7 +152,7 @@ task_t* task_start(task_func func, u32 delay){
 	for (int i = 0; i < MAX_TASK; i++) {
 		__disable_irq();
 		if (_task_handler[i].handler == NULL) {
-			task = _task_handler;
+			task = _task_handler + i;
 			task->handler = func;
 			task->time = delay;
 			task->next = task_head.next;

Project/Motor_PMSM.uvoptx

@@ -130,7 +130,7 @@
         <SetRegEntry>
           <Number>0</Number>
           <Key>DLGTARM</Key>
-          <Name>(1010=-1,-1,-1,-1,0)(1007=-1,-1,-1,-1,0)(1008=-1,-1,-1,-1,0)(1009=-1,-1,-1,-1,0)(1012=-1,-1,-1,-1,0)</Name>
+          <Name>(1010=-1,-1,-1,-1,0)(1007=-2455,137,-2268,412,0)(1008=-1,-1,-1,-1,0)(1009=-1,-1,-1,-1,0)(1012=-1,-1,-1,-1,0)</Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
@@ -145,7 +145,7 @@
         <SetRegEntry>
           <Number>0</Number>
           <Key>ST-LINKIII-KEIL_SWO</Key>
-          <Name>-U0671FF363730554157013636 -O206 -SF1800 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC1000 -FN1 -FF0STM32F3xx_256.FLM -FS08000000 -FL010000 -FP0($$Device:STM32F302R8Tx$CMSIS\Flash\STM32F3xx_256.FLM)</Name>
+          <Name>-U0671FF363730554157013636 -O718 -SF1800 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8000 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F3xx_256.FLM -FS08000000 -FL010000 -FP0($$Device:STM32F302R8Tx$CMSIS\Flash\STM32F3xx_256.FLM)</Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
@@ -170,6 +170,21 @@
           <WinNumber>1</WinNumber>
           <ItemText>hall_iterations,0x0A</ItemText>
         </Ww>
+        <Ww>
+          <count>3</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>_vbus,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>4</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>_ntc,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>5</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>timer_head</ItemText>
+        </Ww>
       </WatchWindow1>
       <Tracepoint>
         <THDelay>0</THDelay>
@@ -228,7 +243,7 @@
         </Entry>
       </SystemViewers>
       <DebugDescription>
-        <Enable>1</Enable>
+        <Enable>0</Enable>
         <EnableFlashSeq>0</EnableFlashSeq>
         <EnableLog>0</EnableLog>
         <Protocol>2</Protocol>

Project/Motor_PMSM.uvprojx

@@ -134,11 +134,11 @@
             <RunIndependent>0</RunIndependent>
             <UpdateFlashBeforeDebugging>1</UpdateFlashBeforeDebugging>
             <Capability>1</Capability>
-            <DriverSelection>-1</DriverSelection>
+            <DriverSelection>4096</DriverSelection>
           </Flash1>
           <bUseTDR>1</bUseTDR>
           <Flash2>BIN\UL2CM3.DLL</Flash2>
-          <Flash3></Flash3>
+          <Flash3>"" ()</Flash3>
           <Flash4></Flash4>
           <pFcarmOut></pFcarmOut>
           <pFcarmGrp></pFcarmGrp>