fix: ramp start from 0 to -vq

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

Applications/bsp/bsp.h

@@ -24,7 +24,7 @@
 #define ADC_CLOCK_MHz (30)
 #define NS_PER_TCLK (8) /* (1/120000000 * 1000000000) */
 #define NS_2_TCLK(ns) ((ns/NS_PER_TCLK) + 1) //ns תΪpwmʹ�õ��Ǹ�TIM��clk count
-#define FOC_PWM_FS (20 * 1000)
+#define FOC_PWM_FS (30 * 1000)
 #define FOC_PWM_period (TIM_CLOCK/FOC_PWM_FS)
 #define FOC_PWM_Half_Period (FOC_PWM_period/2)
 #define MAX_VBUS (12.f) //12v
@@ -33,7 +33,7 @@
 #define ADC_SAMPLING_CYCLES 7.5f
 
 
-#define HW_DEAD_TIME_NS  800
+#define HW_DEAD_TIME_NS  1200
 #define HW_RISE_TIME_NS  50
 #define HW_NOISE_TIME_NS 50
 #define TDead NS_2_TCLK(HW_DEAD_TIME_NS/2)/* ����ʱ�� */ 
@@ -56,6 +56,7 @@
 
 #define TIMER_UP_IRQ_PRIORITY 0
 #define ADC_IRQ_PRIORITY 1
+#define HALL_IRQ_PRIORITY 2
 
 void bsp_init(void);
 void wdog_reload(void);

Applications/bsp/mc_hall_gpio.c

@@ -11,8 +11,6 @@ void mc_hall_init(void){
 }
 
 int get_hall_stat(int samples) {
-	samples = 1 + 2 * samples;
-
 	int h1 = 0, h2 = 0, h3 = 0;
 	int tres = samples / 2;
 
@@ -44,7 +42,7 @@ static void _gpio_irq_enable(void){
 	exti_interrupt_flag_clear(EXTI_8);
 	exti_interrupt_enable(EXTI_8);
 
-	nvic_irq_enable(EXTI5_9_IRQn, 3U, 0U);
+	nvic_irq_enable(EXTI5_9_IRQn, HALL_IRQ_PRIORITY, 0U);
 }
 
 

Applications/bsp/mc_hall_gpio.h

@@ -15,10 +15,19 @@
 
 #define HALL_PLACE DEGREES_120
 
-
-#define READ_HALL1() (gpio_input_bit_get(HALL_1_GROUP, HALL_1_PIN) == SET ?1:0)
-#define READ_HALL2() (gpio_input_bit_get(HALL_2_GROUP, HALL_2_PIN) == SET ?1:0)
-#define READ_HALL3() (gpio_input_bit_get(HALL_3_GROUP, HALL_3_PIN) == SET ?1:0)
+static int __inline__ hall_bit_get(uint32_t gpio_periph,uint32_t pin)
+{
+    if((uint32_t)RESET != (GPIO_ISTAT(gpio_periph)&(pin))){
+        return 1; 
+    }else{
+        return 0;
+    }
+}
+
+
+#define READ_HALL1() (hall_bit_get(HALL_1_GROUP, HALL_1_PIN))
+#define READ_HALL2() (hall_bit_get(HALL_2_GROUP, HALL_2_PIN))
+#define READ_HALL3() (hall_bit_get(HALL_3_GROUP, HALL_3_PIN))
 
 void mc_hall_init(void);
 int get_hall_stat(int samples);

Applications/bsp/timer_count32.c

@@ -77,7 +77,6 @@ static __inline__ u32 _timer_count32_get(void) {
 		high = TIMER_CNT(SLAVE);
 		low = TIMER_CNT(MASTER);
 	}while((high != TIMER_CNT(SLAVE)) /*|| (low != TIMER_CNT(MASTER))*/);
-
 	return (high<<16) | low;
 }
 

Applications/foc/foc_api.c

@@ -69,7 +69,6 @@ void foc_clear(void) {
 
 u32 foc_get_speed(void) {
 	float speed = hall_sensor_avg_speed()/(g_foc.motor_param.poles);
-	//printf("avg speed %f, %d\n", speed, g_foc.motor_p.poles);
 	return abs(speed);
 }
 

Applications/foc/foc_cmd.c

@@ -80,9 +80,9 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			int offset = 0;
 			u8 inc = decode_u8((u8 *)command->data);
 			if (inc) {
-				hall_offset_increase(1);
+				offset = hall_offset_increase(1);
 			}else {
-				hall_offset_increase(-1);
+				offset = hall_offset_increase(-1);
 			}
 			sys_debug("cali hall offset %d\n", offset);
 			can_send_ack(command->can_src, CMD_2_CAN_KEY(Foc_Cali_Hall_Offset), 1);
@@ -90,7 +90,7 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		}
 		case Foc_Set_Open_Dq_Vol:
 		{
-			u32 v_q = decode_u32(((u8 *)command->data) + 4);
+			s32 v_q = decode_s32(((u8 *)command->data) + 4);
 			sys_debug("set v_q %d\n", v_q);
 			foc_set_voltage_ramp(v_q);
 			break;

Applications/foc/foc_cmd.h

@@ -4,7 +4,7 @@
 #include "config.h"
 
 typedef enum {
-	Foc_Start_Motor = 1, //foc_start_stop_t, ready, unread, 0x014d01
+	Foc_Start_Motor = 1, //foc_start_stop_t, ready, unread, 014d01
 	Foc_Cali_Hall_Phase, //calibrate hall phase, get the angle when motor running 0-360
 	Foc_Cali_Hall_Offset,//calibrate hall offset from A phase 0x034d00
 	Foc_Current_Limit,   //u32, set the limited current//mA
@@ -20,10 +20,9 @@ typedef enum {
 	Foc_Report_Vbus_Vol,		//u32
 	Foc_Report_Phase_Vol,		//u32,u32,u32
 	Foc_Report_Dq_Vol,			//u32, u32
-	Foc_Set_Open_Dq_Vol,  		//u32, u32, 0x114d000000000a000000
+	Foc_Set_Open_Dq_Vol,  		//u32, u32, 114d000000000a000000, 114d0000000000000000
 	Foc_Cmd_Max
 }foc_cmd_t;
-
 #define CMD_2_CAN_KEY(cmd) ((u16)(((u16)cmd) | (CAN_MY_ADDRESS<<8)))
 typedef enum {
 	Foc_Start = 1,

Applications/foc/foc_core.c

@@ -163,7 +163,7 @@ void FOC_Fast_Task(motor_foc_t *foc){
 	/* 电流环，输出电压给SVPWM */
 	Foc_Calc_Voltage(foc, &sample_dq, &v_dq);
 	/* 确保电压在6个扇区的内切圆中 */
-	CirCle_Limitation_Process(&v_dq, foc->vbus, 0.95f);
+	CirCle_Limitation_Process(&v_dq, foc->vbus, 1.0f);
 	/* d-q坐标系到alpha-beta坐标系，输出给svpwm */
 	Rev_Park(&v_dq, foc->motor_stat.theta, &pwm_ab);
 	/* SVPWM，获取三相逆变器的开关时间，用的是pwm1模式，如果是pwm2模式，这个函数需要修改 */
@@ -182,18 +182,12 @@ void FOC_Fast_Task(motor_foc_t *foc){
 
 /* 计算电流环的参考输入 */
 void Foc_Calc_Current_Ref(motor_foc_t *foc) {
-	static int count = 0;
 	float speed_ref = ramp_get_target(&foc->speed_ramp);
 	float speed_feedback = foc_get_speed();
 	float vq_out = pi_control(&foc->speed_controller, speed_ref - speed_feedback);	
 	if (foc->mode == FOC_MODE_SPEED_LOOP || foc->mode == FOC_MODE_CLOSE_LOOP){
 		foc->dq_command.Iq = vq_out;
-		foc->dq_command.Id = 0.0f; //if MTPA used, d is not 0
-		if (((count) % 10) == 0) {
-			printf("vq_out = %f, %f, %f\n", vq_out, speed_ref, speed_feedback);
-		}
-		count++;
-		
+		foc->dq_command.Id = 0.0f; //if MTPA used, d is not 0		
 	}else {
 		foc->dq_command.Iq = ramp_get_target(&foc->current_ramp);
 		foc->dq_command.Id = 0.0f; //if MTPA used, d is not 0
@@ -223,7 +217,7 @@ void foc_pwm_up_handler(void){
 	phase_current_adc_triger(&g_foc.current_samp);
 }
 
-measure_time_t g_meas_foc = {.exec_max_time = 25,};
+measure_time_t g_meas_foc = {.exec_max_time = 15,};
 void mc_phase_current_irq(void) {
 	if (g_foc.current_samp.is_calibrating_offset) {
 		phase_current_offset(&g_foc.current_samp);

Applications/foc/hall_sensor.c

@@ -36,6 +36,7 @@ static u16 _hall_table[] = {0xFFFF, 121/*1*/, 240/*2*/, 190/*3*/, 13/*4*/, 58/*5
 static hall_t _hall;
 static hall_sample_t h_samples;
 static timer_t _hall_detect_timer = TIMER_INIT(_hall_detect_timer, _hall_timeout_handler);
+measure_time_t g_meas_hall = {.exec_max_time = 1,};
 
 #define read_hall(h,t) {h = get_hall_stat(HALL_READ_TIMES); t = _hall_table[h];}
 #define us_2_s(tick) ((float)tick / 1000000.0f)
@@ -44,10 +45,6 @@ static float __inline _delta_seconds(u32 prev) {
 	return (float)timer_count32_delta_us(prev, NULL)/1000000.0f;
 }
 
-static u32 __inline get_seconds(void) {
-	return ticks_2_ms(co_task_sys64_ticks()/1000);
-}
-
 static void _hall_put_sample(float angle, u32 ticks) {
 	hall_sample_t *s = &h_samples;
 	s->index += 1;
@@ -82,8 +79,8 @@ void hall_sensor_init(void) {
 
 void _hall_timeout_handler(timer_t *timer){
 
-	_hall.degree_per_s = 0;
-	_hall.e_rpm = _hall.e_filted_rpm = 0;
+	_hall.angle_speed = _hall.angle_speed_avg = 0;
+	_hall.rpm = _hall.rpm_avg = 0;
 	h_samples.index = 0;
 }
 
@@ -99,7 +96,7 @@ float hall_sensor_get_theta(void){
 		read_hall(_hall.state, _hall.theta);
 		return _hall.theta;
 	}
-	_hall.est_theta = _delta_seconds(_hall.ticks) * _hall.degree_per_s + _hall.theta;
+	_hall.est_theta = _delta_seconds(_hall.ticks) * _hall.angle_speed + _hall.theta;
 	float est_delta = _hall.est_theta - _hall.theta;
 	if (est_delta > PHASE_60_DEGREE) {
 		_hall.est_theta = _hall.theta + PHASE_60_DEGREE;
@@ -108,7 +105,7 @@ float hall_sensor_get_theta(void){
 		}
 	}else if (est_delta < -PHASE_60_DEGREE){
 		_hall.est_theta = _hall.theta - PHASE_60_DEGREE;
-		if (_hall.est_theta <= -PHASE_360_DEGREE) {
+		if (_hall.est_theta < 0/*= -PHASE_360_DEGREE*/) {
 			_hall.est_theta += PHASE_360_DEGREE;
 		}
 	}
@@ -121,11 +118,11 @@ void hall_sensor_set_theta(bool override, float theta){
 }
 
 float hall_sensor_get_speed(void) {
-	return _hall.e_rpm;
+	return _hall.rpm;
 }
 
 float hall_sensor_avg_speed(void) {
-	return _hall.e_filted_rpm;
+	return _hall.rpm_avg;
 }
 
 int hall_offset_increase(int inc) {
@@ -204,10 +201,12 @@ int hall_sensor_calibrate(float voltage, u16 *hall_table){
 
 
 void hall_sensor_handler(void) {
+	
+	time_measure_start(&g_meas_hall);
 	u8 state_now = get_hall_stat(HALL_READ_TIMES);
 	float theta_now = _hall_table[state_now];
 	u8 state_prev = _hall.state;
-
+	
 	if (!_hall.working) {		
 		if(theta_now != 0xFFFF) {
 			_hall.working = true;
@@ -217,7 +216,7 @@ void hall_sensor_handler(void) {
 		}
 		return;
 	}
-
+	
 	switch (state_now) {
 		case STATE_1:
 			if (state_prev == STATE_5) {
@@ -277,26 +276,39 @@ void hall_sensor_handler(void) {
 			return;
 	}
 
-
+	if (theta_now >= 360.0f) {
+		theta_now -= 360.0f;
+	}
+	
 	float delta_time = _delta_seconds(_hall.ticks);
 	if (delta_time == 0.0f) { //may be errors ???
 		return;
 	}
+	
 	float delta_theta = (_hall.direction == POSITIVE)?60.0f : -60.0f;
 	_hall_put_sample(delta_theta, timer_count32_delta_us(_hall.ticks, NULL));
+	_hall.angle_speed = delta_theta / delta_time;
+
+	os_disable_irq();
+
 	if (!h_samples.full) {
-		_hall.degree_per_s = delta_theta / delta_time;
-	}else {
-		_hall.degree_per_s = _hall_avg_speed();
-		_hall.e_filted_rpm = _hall.degree_per_s / 360.0f * 60.0f; //电角速度
+		_hall.angle_speed_avg = _hall.angle_speed;
+	}else {		
+		_hall.angle_speed_avg = _hall_avg_speed();
 	}
 	_hall.ticks = timer_count32_get();
-	_hall.second = get_seconds();
 	_hall.theta = theta_now;
+
+	os_enable_irq();
+	
 	_hall.state = state_now;
-	_hall.e_rpm = _hall.degree_per_s / 360.0f * 60.0f;
-	timer_post(&_hall_detect_timer, 2000);
-	//printf("speed :%.4f - %.4f - %.4f - %d\n", _hall.degree_per_s, delta_theta, delta_time, (int)_hall.e_rpm);
+	
+	_hall.rpm = _hall.angle_speed / 360.0f * 60.0f;
+	_hall.rpm_avg = _hall.angle_speed_avg / 360 * 60.0f;
+	
+	timer_post(&_hall_detect_timer, 1500);
+
+	time_measure_end(&g_meas_hall);
 }
 
 

Applications/foc/hall_sensor.h

@@ -27,14 +27,14 @@ typedef struct {
 	bool  alignmnet;
 	float theta;
 	float est_theta;
-	float e_rpm;        //当前的电角度, 单位：RPM
-	float e_filted_rpm;    //滤波后的电角度
+	float rpm;        		//当前的电速度, 单位：RPM
+	float rpm_avg;    		//滤波后的电速度
+	float angle_speed; 		//当前的电角速度, 单位：rad/s
+	float angle_speed_avg;	//滤波后的电角速度
 	u8    state;
 	u32   ticks;
-	u32   second;
 	bool  working;
 	s8    direction;
-	float degree_per_s; //当前的电角度, 单位：rad/s
 	float phase_offset;
 	bool  is_override_theta;
 	float override_theta;

Applications/foc/ramp_ctrl.c

@@ -41,11 +41,11 @@ void ramp_timer_handler(timer_t *timer) {
 	ramp_t *ramp = (ramp_t *)timer;
 	float target = ramp->target + ramp->steps;
 	if (ramp->steps < 0) {
-		if (abs(target) < abs(ramp->final_point)) {
+		if (target < ramp->final_point) {
 			target = ramp->final_point;
 		}
 	}else {
-		if (abs(target) > abs(ramp->final_point)) {
+		if (target > ramp->final_point) {
 			target = ramp->final_point;
 		}
 	}

Applications/os/co_task.h

@@ -8,6 +8,10 @@ Task如果没有调用task_yield/delay 永远执行下去
 
 #define TICKS_PER_HZ 1000
 #define ticks_2_ms(ticks) (ticks*(1000/TICKS_PER_HZ))
+
+#define os_disable_irq() __disable_irq()
+#define os_enable_irq()  __enable_irq()
+
 typedef void (*task_func)(void *);
 
 void cpu_counts_enable(void);

Applications/os/timer.c

@@ -15,7 +15,7 @@ void timer_post(timer_t *timer, u32 delay)
 {
 	timer_t *node;
 	u64 time = g_task_ticks64 + (delay > 0 ? delay : 1);
-	__disable_irq();
+	os_disable_irq();
 	if (timer->prev != NULL) {
 		timer->prev->next = timer->next;
 	}
@@ -35,7 +35,7 @@ void timer_post(timer_t *timer, u32 delay)
 	node->prev = timer;
 	timer->next = node;
 	timer->time = time;
-	__enable_irq();
+	os_enable_irq();
 	if (!_timer_start) {
 		co_task_create(timer_task_handler, NULL, 384);
 		_timer_start = true;
@@ -44,13 +44,13 @@ void timer_post(timer_t *timer, u32 delay)
 
 void timer_cancel(timer_t *timer)
 {
-	__disable_irq();
+	os_disable_irq();
 	if (timer->prev != NULL && timer->next != NULL) {
 		timer->prev->next = timer->next;
 		timer->next->prev = timer->prev;
 	}
 	timer->next = timer->prev = timer;
-	__enable_irq();
+	os_enable_irq();
 }
 
 static void timer_task_handler(void *args)