MC100/Applications/foc/motor/motor.c

#include "foc/motor/motor.h"
#include "foc/motor/current.h"
#include "foc/foc_config.h"
#include "foc/mc_error.h"
#include "foc/samples.h"
#include "math/fast_math.h"
#include "bsp/timer_count32.h"
#include "libs/time_measure.h"
#include "bsp/delay.h"
#include "bsp/bsp.h"
#include "bsp/adc.h"
#include "bsp/pwm.h"
#include "foc/commands.h"
#include "libs/logger.h"
#include "bsp/sched_timer.h"
#include "foc/core/e_ctrl.h"
#include "foc/samples.h"
#include "foc/motor/motor_param.h"
#include "foc/core/torque.h"
#include "app/nv_storage.h"

static bool mc_is_hwbrake(void);
static void _pwm_brake_timer_handler(shark_timer_t *);
static shark_timer_t _brake_timer = TIMER_INIT(_brake_timer, _pwm_brake_timer_handler);


static motor_t motor = {
	.s_direction = POSITIVE,
};

static void mc_gpio_init(void) {
#ifdef GPIO_BRAKE_IN_GROUP
	rcu_periph_clock_enable(GPIO_BRAKE_IN_RCU);
	gpio_init(GPIO_BRAKE_IN_GROUP, GPIO_BRAKE_IN_MODE, GPIO_OSPEED_50MHZ, GPIO_BRAKE_IN_PIN);
	
	gpio_exti_source_select(GPIO_BRAKE_EXIT_SRC_GROUP, GPIO_BRAKE_EXIT_SRC_PIN);
	exti_init(GPIO_BRAKE_EXTI, EXTI_INTERRUPT, EXTI_TRIG_BOTH);
	nvic_irq_enable(GPIO_BRAKE_IRQ, EBREAK_IRQ_PRIORITY, 0U);
	exti_interrupt_flag_clear(GPIO_BRAKE_EXTI);
	exti_interrupt_enable(GPIO_BRAKE_EXTI);	
#endif
}

static void MC_Check_MosVbusThrottle(void) {
	int count = 1000;
	gpio_phase_u_detect(true);
	while(count-- >= 0) {
		task_udelay(20);
		sample_uvw_phase();
		sample_throttle();
		sample_vbus();
	}
	gpio_phase_u_detect(false);
	float abc[3];
	get_phase_vols(abc);
	if (abc[0] > 10 || abc[1] > 10 || abc[2] > 10) {
		PMSM_FOC_SetCriticalError(FOC_CRIT_H_MOS_Err);
	}else if (abc[0] < 0.001f){
		PMSM_FOC_SetCriticalError(FOC_CRIT_L_MOS_Err);
	}else if ((abc[0] > 0.5f) && (abc[1] < 0.001f || abc[2] < 0.001f)) {
		PMSM_FOC_SetCriticalError(FOC_CRIT_Phase_Conn_Err);
	}
	if (get_vbus_float() < nv_get_foc_params()->s_minDCVol) {
		PMSM_FOC_SetCriticalError(FOC_CRIT_UN_Vol_Err);
	}
	if (get_vbus_float() > nv_get_foc_params()->s_maxDCVol) {
		PMSM_FOC_SetCriticalError(FOC_CRIT_OV_Vol_Err);
	}
}

static u32 _self_check_task(void *p) {
	if (ENC_Check_error()) {
		err_add_record(FOC_CRIT_Encoder_Err, 0);
		PMSM_FOC_SetCriticalError(FOC_CRIT_Encoder_Err);
	}
	if (get_vbus_float() < nv_get_foc_params()->s_minDCVol) {
		PMSM_FOC_SetCriticalError(FOC_CRIT_UN_Vol_Err);
	}

	return 0;
}

void mc_init(void) {
	adc_init();
	pwm_3phase_init();
	samples_init();
	motor_encoder_init();
	foc_command_init();
	PMSM_FOC_CoreInit();
	mc_gpio_init();
	MC_Check_MosVbusThrottle();
	sched_timer_enable(CONFIG_SPD_CTRL_US);
	shark_task_create(_self_check_task, NULL);
	pwm_up_enable(true);
}

motor_t * mc_params(void) {
	return &motor;
}

bool mc_start(u8 mode) {
	if (motor.b_start) {
		return true;
	}

	MC_Check_MosVbusThrottle();

	if (PMSM_FOC_GetCriticalError() != 0) {
		PMSM_FOC_SetErrCode(FOC_Have_CritiCal_Err);
		return false;
	}
	if (mode > CTRL_MODE_CURRENT) {
		PMSM_FOC_SetErrCode(FOC_Param_Err);
		return false;
	}
	if (PMSM_FOC_GetSpeed() > 10.0f) {
		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
		return false;
	}
	if (!mc_throttle_released()) {
		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
		return false;
	}
	pwm_up_enable(false);
	motor.mode = mode;
	eCtrl_init(1000, 3000);
	motor_encoder_start(motor.s_direction);
	PMSM_FOC_Start(mode);
	pwm_turn_on_low_side();
	delay_ms(100);
	phase_current_offset_calibrate();
	pwm_start();
	adc_start_convert();
	phase_current_calibrate_wait();
	motor.throttle = 0;
	motor.b_start = true;
	motor.b_runStall = false;
	motor.runStall_time = 0;
	if (phase_curr_offset_check()) {
		PMSM_FOC_SetCriticalError(FOC_CRIT_CURR_OFF_Err);
		mc_stop();
		return false;
	}
	if (mc_is_hwbrake()) {
		PMSM_FOC_Brake(true);
	}
	gpio_beep(1000);
	return true;
}

bool mc_stop(void) {
	if (!motor.b_start) {
		return true;
	}
	if (PMSM_FOC_GetSpeed() > 10.0f) {
		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
		sys_debug("speed error\n");
		return false;
	}
	if (!mc_throttle_released()) {
		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
		sys_debug("throttle error\n");
		return false;
	}
	motor.mode = CTRL_MODE_OPEN;
	adc_stop_convert();
	pwm_stop();
	PMSM_FOC_Stop();
	pwm_up_enable(true);
	motor.b_start = false;
	motor.b_epm = false;
	motor.epm_dir = EPM_Dir_None;
	gpio_led2_enable(false);
	return true;
}

bool mc_set_foc_mode(u8 mode) {
	if (mode == motor.mode) {
		return true;
	}
	if (!motor.b_start) {
		return false;
	}
	u32 mask = cpu_enter_critical();
	bool ret = false;
	if (PMSM_FOC_SetCtrlMode(mode)) {
		motor.mode = mode;
		if (mode == CTRL_MODE_OPEN) {
			PMSM_FOC_Start(motor.mode);
			pwm_enable_channel();
		}
		ret = true;
	}
	cpu_exit_critical(mask);
	return ret;
}

bool mc_start_epm(bool epm) {
	if (motor.b_epm == epm) {
		return true;
	}
	if (!motor.b_start) {
		PMSM_FOC_SetErrCode(FOC_NotAllowed);
		return false;
	}
	if (PMSM_FOC_GetSpeed() != 0.0f) {
		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
		return false;
	}
	if (!mc_throttle_released()) {
		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
		return false;
	}
	u32 mask = cpu_enter_critical();
	motor.b_epm = epm;
	if (epm) {
		motor.lim_rpm = PMSM_FOC_GetSpeedLimit();
		eCtrl_set_TgtSpeed(0);
		motor.mode = CTRL_MODE_SPD;
		PMSM_FOC_SpeedLimit(nv_get_foc_params()->s_maxEpmRPM);
		PMSM_FOC_SetCtrlMode(CTRL_MODE_SPD);
	}else {
		motor.epm_dir = EPM_Dir_None;
		motor.mode = CTRL_MODE_TRQ;
		PMSM_FOC_SetCtrlMode(CTRL_MODE_TRQ);
		PMSM_FOC_SpeedLimit(motor.lim_rpm);
	}
	cpu_exit_critical(mask);
	return false;
}

bool mc_is_epm(void) {
	return motor.b_epm;
}

bool mc_start_epm_move(EPM_Dir_t dir) {
	if (!motor.b_epm || !motor.b_start) {
		PMSM_FOC_SetErrCode(FOC_NotAllowed);
		return false;
	}
	if (dir == motor.epm_dir) {
		return true;
	}
	motor.epm_dir = dir;
	if (dir != EPM_Dir_None) {
		u32 mask = cpu_enter_critical();
		if (!PMSM_FOC_Is_Start()) {
			PMSM_FOC_Start(motor.mode);
			pwm_enable_channel();
		}
		cpu_exit_critical(mask);
		float rpm = nv_get_foc_params()->s_maxEpmRPM;
		if (dir == EPM_Dir_Back) {
			rpm = -rpm;
		}
		PMSM_FOC_Set_Speed(rpm);
	}else {
		PMSM_FOC_Set_Speed(0);
	}
	return true;
}

void mc_set_spd_torque(s32 target) {
	motor.b_ignor_throttle = true;
	motor.s_targetFix = target;
}

void mc_use_throttle(void) {
	motor.b_ignor_throttle = false;
}


void mc_encoder_off_calibrate(s16 vd) {
	if (PMSM_FOC_Is_Start()) {
		return;
	}
	motor.b_calibrate = true;
	pwm_turn_on_low_side();
	task_udelay(500);
	PMSM_FOC_Start(CTRL_MODE_OPEN);
	phase_current_offset_calibrate();
	pwm_start();
	adc_start_convert();
	phase_current_calibrate_wait();
	PMSM_FOC_Set_Angle(0);
	PMSM_FOC_SetOpenVdq(vd, 0);
	delay_ms(2000);
	motor_encoder_set_direction(POSITIVE);
	for (int i = 0; i < 100000; i++) {
		for (float angle = 0; angle < 360; angle++) {
			PMSM_FOC_Set_Angle(angle);
			delay_ms(2);
			if (i > 10) {
				motor_encoder_offset(angle);
			}
		}
		wdog_reload();
		if (motor_encoder_offset_is_finish()) {
			break;
		}
	}
	motor_encoder_set_direction(NEGATIVE);
	delay_ms(100);
	for (int i = 0; i < 100000; i++) {
		for (float angle = 360; angle > 0; angle--) {
			PMSM_FOC_Set_Angle(angle);
			delay_ms(2);
			if (i > 10) {
				motor_encoder_offset(angle);
			}
		}
		wdog_reload();
		if (motor_encoder_offset_is_finish()) {
			break;
		}
	}
	delay_ms(500);
	PMSM_FOC_SetOpenVdq(0, 0);
	delay_ms(500);
	wdog_reload();
	adc_stop_convert();
	pwm_stop();
	PMSM_FOC_Stop();
	motor.b_calibrate = false;
}

bool mc_encoder_zero_calibrate(s16 vd) {
	if (PMSM_FOC_Is_Start()) {
		return false;
	}
	motor.b_calibrate = true;
	pwm_turn_on_low_side();
	task_udelay(500);
	PMSM_FOC_Start(CTRL_MODE_OPEN);
	phase_current_offset_calibrate();
	pwm_start();
	adc_start_convert();
	phase_current_calibrate_wait();
	PMSM_FOC_Set_Angle(0);
	PMSM_FOC_SetOpenVdq(vd, 0);
	delay_ms(2000);
	float phase = motor_encoder_zero_phase_detect();
	delay_ms(500);
	PMSM_FOC_SetOpenVdq(0, 0);
	delay_ms(500);
	adc_stop_convert();
	pwm_stop();
	PMSM_FOC_Stop();
	motor.b_calibrate = false;
	if (phase != INVALID_ANGLE) {
		nv_save_angle_offset(phase);
		return true;
	}
	return false;
}


bool mc_current_sensor_calibrate(float current) {
	if (!mc_start(CTRL_MODE_OPEN)) {
		return false;
	}
	phase_current_sensor_start_calibrate(current);
	phase_current_calibrate_wait();
	return true;
}

bool mc_lock_motor(bool lock) {
	if (lock && (PMSM_FOC_GetSpeed() > 10)) {
		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
		return false;
	}
	PMSM_FOC_LockMotor(lock); //if mot enabled, foc core will do lock
	if (!motor.b_start) {
		if (lock) {
			pwm_start();
			pwm_update_duty(0, 0, 0);
		}else {
			pwm_update_duty(FOC_PWM_Half_Period/2, FOC_PWM_Half_Period/2, FOC_PWM_Half_Period/2);
			pwm_stop();
		}
	}
	return true;
}


bool mc_throttle_released(void) {
	return get_throttle_float() < CONFIG_THROTTLE_LOW_VALUE;
}

static bool mc_is_hwbrake(void) {
#ifdef GPIO_BRAKE_IN_GROUP
	int count = 50;
	int settimes = 0;
	while(count-- > 0) {
		bool b1 = gpio_input_bit_get(GPIO_BRAKE_IN_GROUP, GPIO_BRAKE_IN_PIN) == SET;
		if (b1) {
			settimes ++;
		}
		delay_us(1);
	}
	if (settimes == 0) {
#if GPIO_BREAK_MODE==GPIO_LOW_BRK_MODE
		return true;
#else
		return false;
#endif
	}else if (settimes == 50) {
#if GPIO_BREAK_MODE==GPIO_LOW_BRK_MODE
		return false;
#else
		return true;
#endif
	}else {
		//有干扰，do nothing
		motor.n_brake_errors++;
		return false;
	}
#else
	return false;
#endif
}
void MC_Brake_IRQHandler(void) {

	if (!motor.b_start) {
		return;
	}
	if (mc_is_hwbrake()) {
		PMSM_FOC_Brake(true);
	}else {
		PMSM_FOC_Brake(false);
	}
}

static void _pwm_brake_timer_handler(shark_timer_t *t){
	pwm_brake_enable(true);
}

void MC_Protect_IRQHandler(void){
	pwm_brake_enable(false);
	shark_timer_post(&_brake_timer, 1000);
	if (!motor.b_start) {
		return;
	}
	PMSM_FOC_SetCriticalError(FOC_CRIT_Phase_Err);
}

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){
	motor_encoder_update();
}

measure_time_t g_meas_foc = {.exec_max_time = 20, .intval_max_time = 62,  .intval_low_err = 0, .intval_hi_err = 0, .first = true,};
#define TIME_MEATURE_START() time_measure_start(&g_meas_foc)
#define TIME_MEATURE_END() time_measure_end(&g_meas_foc)

/*ADC 电流采集中断，调用FOC的核心处理函数*/
void ADC_IRQHandler(void) {
	if (phase_current_offset()) {//check if is adc offset checked
		return;
	}
	if (phase_current_sensor_do_calibrate()){
		pwm_update_duty(100, FOC_PWM_Half_Period-100, 100);
		pwm_update_sample(FOC_PWM_Half_Period-1, FOC_PWM_Half_Period+1, PHASE_BC);
		return;
	}
	TIME_MEATURE_START();
	PMSM_FOC_Schedule();
	TIME_MEATURE_END();
}

static bool mc_can_stop_foc(void) {
	if (mc_throttle_released() && PMSM_FOC_GetSpeed() == 0.0f) {
		if (!PMSM_FOC_MotorLocking() && motor.epm_dir == EPM_Dir_None) {
			return true;
		}
	}
	return false;
}

//#define ANGLE_TEST
#ifdef ANGLE_TEST
static void _debug_angle(void) {
	if (motor.b_start) {
		PMSM_FOC_Set_Angle(motor.s_testAngle);
		if (++motor.s_testAngle >= 360) {
			motor.s_testAngle = 0;
		}
	}
}
#endif
/*FOC 的部分处理，比如速度环，状态机，转把采集等*/
measure_time_t g_meas_MCTask;
void Sched_MC_mTask(void) {
	time_measure_start(&g_meas_MCTask);
	u8 runMode = PMSM_FOC_CtrlMode();
#if ANGLE_TEST
	_debug_angle();
#endif
	PMSM_FOC_Calc_iDC();
	if (motor.b_calibrate || (motor.mode == CTRL_MODE_OPEN)) {
		return;
	}
	if ((runMode == CTRL_MODE_TRQ || runMode == CTRL_MODE_SPD) && !PMSM_FOC_MotorLocking()) {
		//堵转判断
		if (motor.b_runStall) {
			if (!mc_throttle_released()) {
				return;
			}
			motor.b_runStall = false; //转把释放，清除堵转标志
		}else if ((ABS(PMSM_FOC_GetSpeed()) < 1.0f) && (PMSM_FOC_Get()->out.s_RealIdq.q >= CONFIG_STALL_MAX_CURRENT)){
			if (motor.runStall_time == 0) {
				motor.runStall_time = get_tick_ms();
			}else {
				if (get_delta_ms(motor.runStall_time) >= CONFIG_STALL_MAX_TIME) {
					motor.b_runStall = true;
					motor.runStall_time = 0;
					torque_speed_target(runMode, 0.0f);
					return;
				}
			}
		}else {
			motor.runStall_time = 0;
		}
	}
	if ((runMode != CTRL_MODE_OPEN) || (motor.mode != CTRL_MODE_OPEN)) {
		if (motor.mode != CTRL_MODE_OPEN) {
			u32 mask;
			if (mc_can_stop_foc()) {
				if (PMSM_FOC_Is_Start()) {
					mask = cpu_enter_critical();
					PMSM_FOC_Stop();
					pwm_disable_channel();
					cpu_exit_critical(mask);
				}
			}else {
				if (!PMSM_FOC_Is_Start()) {
					mask = cpu_enter_critical();
					PMSM_FOC_Start(motor.mode);
					pwm_enable_channel();
					cpu_exit_critical(mask);
				}
			}
		}
		if (runMode != CTRL_MODE_OPEN) {
			eCtrl_Running();
			float f_throttle = get_throttle_float();
			if (f_throttle != motor.throttle) {
				motor.throttle = f_throttle;
				torque_speed_target(runMode, f_throttle);
			}
			PMSM_FOC_Slow_Task();
		}
	}
}