set mode,判断如果controller没有start，调用start

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

Applications/app/app.c

@@ -54,9 +54,9 @@ void app_start(void){
 }
 
 static void app_print_log(void) {
-	//sys_debug("Slow: %d - %d\n", g_meas_MCTask.intval_time, g_meas_MCTask.exec_time);
+	//sys_debug("Slow: %d - %d, err:%d, %d\n", g_meas_MCTask.intval_time, g_meas_MCTask.exec_time, g_meas_MCTask.exec_max_error_time, g_meas_MCTask.exec_time_error);
 	//sys_debug("Fast: %d - %d, err: %d-%d-%d\n", g_meas_foc.intval_time, g_meas_foc.exec_time, g_meas_foc.intval_hi_err, g_meas_foc.intval_low_err, g_meas_foc.exec_max_error_time);
-	//sys_debug("FOC time err %d %d\n", g_meas_foc.intval_time_h_error, g_meas_foc.intval_time_l_error);
+	//sys_debug("FOC time err %d %d %d %d\n", g_meas_foc.intval_time_h_error, g_meas_foc.intval_time_l_error, g_meas_foc.exec_max_error_time, g_meas_foc.exec_time_error);
 	//sys_debug("acc vol %d, bid %d\n", get_acc_vol(), gpio_board_id());
 	//sys_debug("throttle %f\n", get_throttle_float());
 	//sys_debug("ADC Vref %f, %f\n", get_adc_vref(), adc_5vref_compesion());

Applications/bsp/gd32/can.c

@@ -132,7 +132,8 @@ static void shark_can0_config(void)
 
 static int shark_send_can0_data(can_trasnmit_message_struct *P_message){
 	can_tx_poll();
-	if (circle_put_data(&g_tx_circle, (u8 *)P_message, sizeof(can_trasnmit_message_struct))){
+	int len = sizeof(can_trasnmit_message_struct);
+	if (circle_put_data(&g_tx_circle, (u8 *)P_message, len) == len){
 		return CAN_SEND_OK;
 	}
 	return CAN_SEND_ERROR;
@@ -218,8 +219,8 @@ void shark_can0_deinit(void){
 }
 
 void shark_can0_init(void){
-	circle_buffer_init(&g_tx_circle, _g_tx_buffer, sizeof(_g_tx_buffer));
-	circle_buffer_init(&g_rx_circle, _g_rx_buffer, sizeof(_g_rx_buffer));
+	circle_buffer_init(&g_tx_circle, _g_tx_buffer, sizeof(_g_tx_buffer) - 1);
+	circle_buffer_init(&g_rx_circle, _g_rx_buffer, sizeof(_g_rx_buffer) - 1);
 
 	shark_task_create(_can_poll_task, NULL);
 	shark_can0_txrx_pin_config();

Applications/foc/commands.c

@@ -442,10 +442,11 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		case Foc_Start_DQ_Calibrate:
 		{
 			u8 start = decode_u8((u8 *)command->data);
-			if (start == Foc_Start) {
+			if (start == 1) {
 				sys_debug("start mpta cali\n");
-				mc_set_foc_mode(CTRL_MODE_CURRENT);
-				mot_contrl_mtpa_calibrate(&motor.controller, true);
+				if (mc_set_foc_mode(CTRL_MODE_CURRENT)) {
+					mot_contrl_mtpa_calibrate(&motor.controller, true);
+				}
 			}else {
 				mot_contrl_mtpa_calibrate(&motor.controller, false);
 				mc_set_foc_mode(CTRL_MODE_TRQ);

Applications/foc/core/controller.c

@@ -276,7 +276,7 @@ static void mot_contrl_dq_assign(mot_contrl_t *ctrl) {
 		float target_current = line_ramp_get_interp(&ctrl->target_current);
 		if (ctrl->b_mtpa_calibrate && (ctrl->adv_angle != INVALID_ANGLE)) {
 			float s, c;
-			normal_sincosf(degree_2_pi(ctrl->adv_angle + 90.0f), &s, &c);
+			arm_sin_cos(ctrl->adv_angle + 90.0f, &s, &c);
 			ctrl->target_idq.d = target_current * c;
 			
 			if (ctrl->target_idq.d > ctrl->hwlim.fw_id) {

Applications/foc/foc_config.h

@@ -61,7 +61,7 @@
 #define CONFIG_Volvec_Delay_Comp /* 电压矢量角度补偿 */
 #define CONFIG_Volvec_Delay_Comp_Start_Vel 500 // rpm
 
-#define CONFIG_ENABLE_IAB_REC 1   // for phase current debug
+#define CONFIG_ENABLE_IAB_REC 0   // for phase current debug
 
 #ifdef CONFIG_SPEED_LADRC
 	#define CONFIG_LADRC_Wo  200.0F

Applications/foc/lineramp.h

@@ -19,7 +19,7 @@ static __INLINE void line_ramp_set_target(lineramp_t *line, float target) {
 		}else {
 			line->step = -delta / line->time_dec;
 		}
-		if ((line->min_step >= 0) && (line->step < line->min_step)) {
+		if ((line->min_step > 0) && (line->step < line->min_step)) {
 			line->step = line->min_step;
 		}
 	}

Applications/foc/motor/motor.c

@@ -222,7 +222,6 @@ static void mc_gear_vmode_changed(void) {
 	mot_contrl_set_vel_limit(&motor.controller, (float)min(gears->max_speed, motor.u_set.rpm_lim));
 	mot_contrl_set_dccurr_limit(&motor.controller, (float)min(gears->max_idc, motor.u_set.idc_lim));
 	mot_contrl_set_torque_limit(&motor.controller, (float)gears->max_torque);
-	sys_debug("change %d, %d, %d\n", gears->max_idc, gears->max_speed, gears->max_torque);
 }
 
 void mc_init(void) {
@@ -595,7 +594,7 @@ bool mc_set_foc_mode(u8 mode) {
 	bool ret = false;
 	if (mot_contrl_request_mode(&motor.controller, mode)) {
 		motor.mode = mode;
-		if (mode == CTRL_MODE_OPEN || mode == CTRL_MODE_CURRENT) {
+		if ((mode == CTRL_MODE_OPEN || mode == CTRL_MODE_CURRENT) && !mot_contrl_is_start(&motor.controller)) {
 			mot_contrl_start(&motor.controller, motor.mode);
 			pwm_enable_channel();
 		}
@@ -1521,7 +1520,7 @@ static void mc_process_throttle_torque(float vol) {
 	}
 }
 /*FOC 的部分处理，比如速度环，状态机，转把采集等*/
-measure_time_t g_meas_MCTask;
+measure_time_t g_meas_MCTask = {.exec_max_time = 100, .intval_max_time = 1000,  .intval_low_err = 0, .intval_hi_err = 0, .first = true,};
 #define mc_TaskStart time_measure_start(&g_meas_MCTask)
 #define mc_TaskEnd time_measure_end(&g_meas_MCTask)
 void Sched_MC_mTask(void) {
@@ -1539,7 +1538,6 @@ void Sched_MC_mTask(void) {
 	mc_process_curise();
 #endif
 	u8 runMode = mot_contrl_mode(&motor.controller);
-
 	/*保护功能*/
 	u8 limted = mot_contrl_protect(&motor.controller);
 	/* 母线电流，实际采集的相电流矢量大小的计算 */

Applications/libs/time_measure.c

@@ -40,7 +40,9 @@ void time_measure_end(measure_time_t *m) {
 	m->exec_time = time_delta_us(m->exec_count, NULL);
 	if (m->exec_time > m->exec_max_time) {
 		m->exec_time_error ++;
-		m->exec_max_error_time = m->exec_time;
+		if (m->exec_time > m->exec_max_error_time) {
+			m->exec_max_error_time = m->exec_time;
+		}
 	}
 }