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"
#include "foc/core/torque.h"
#include "foc/limit.h"

static bool mc_is_hwbrake(void);
static void _pwm_brake_prot_timer_handler(shark_timer_t *);
static shark_timer_t _brake_prot_timer = TIMER_INIT(_brake_prot_timer, _pwm_brake_prot_timer_handler);
static void _autohold_beep_timer_handler(shark_timer_t *);
static shark_timer_t _autohold_beep_timer = TIMER_INIT(_autohold_beep_timer, _autohold_beep_timer_handler);


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

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);
	int vbus_vol = get_vbus_int();
	if (abc[0] > vbus_vol/2 || abc[1] > vbus_vol/2 || abc[2] > vbus_vol/2) {
		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);
	}
	sys_debug("phase vol %f, %f, %f\n", abc[0], abc[1], abc[2]);
}

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);
	}
	return 0;
}

static void _mc_internal_init(u8 mode, bool start) {
	motor.mode = mode;
	motor.throttle = 0;
	motor.b_start = start;
	motor.b_runStall = false;
	motor.runStall_time = 0;
	motor.b_epm = false;
	motor.b_epm_cmd_move = false;
	motor.epm_dir = EPM_Dir_None;
	motor.n_autohold_time = 0;
	motor.b_auto_hold = 0;
	motor.b_break = false;
	motor.b_wait_brk_release = false;
}
void mc_init(void) {
	adc_init();
	pwm_3phase_init();
	samples_init();
	motor_encoder_init();
	foc_command_init();
	torque_init();
	PMSM_FOC_CoreInit();
	eCtrl_init(nv_get_foc_params()->n_acc_time, nv_get_foc_params()->n_dec_time);
	mc_brk_gpio_init();
	limter_set_under_voltage(nv_get_foc_params()->s_minDCVol);
	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;
}

void mc_need_update(void) {
	motor.b_updated = true;
}

bool mc_start(u8 mode) {
	if (motor.b_start) {
		return true;
	}
#ifdef CONFIG_DQ_STEP_RESPONSE
	mode = CTRL_MODE_CURRENT;
#endif
	if (motor.b_lock_motor) {
		PMSM_FOC_SetErrCode(FOC_NotAllowed);
		return false;
	}
	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 (motor_encoder_get_speed() > 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);

	_mc_internal_init(mode, true);

	torque_reset();
	eCtrl_init(nv_get_foc_params()->n_acc_time, nv_get_foc_params()->n_dec_time);
	motor_encoder_start(true);
	PMSM_FOC_Start(mode);
	PMSM_FOC_RT_LimInit();
	pwm_turn_on_low_side();
	delay_ms(100);
	phase_current_offset_calibrate();
	pwm_start();
	adc_start_convert();
	phase_current_calibrate_wait();
	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(200);
	return true;
}

bool mc_stop(void) {
	if (!motor.b_start) {
		return true;
	}

	if (motor.b_lock_motor) {
		PMSM_FOC_SetErrCode(FOC_NotAllowed);
		return false;
	}

	if (motor_encoder_get_speed() > 10.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();
	_mc_internal_init(CTRL_MODE_OPEN, false);
	adc_stop_convert();
	pwm_stop();
	PMSM_FOC_Stop();
	motor_encoder_start(false);
	pwm_up_enable(true);
	cpu_exit_critical(mask);

	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 || mode == CTRL_MODE_CURRENT) {
			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) {
		eCtrl_set_TgtSpeed(0);
		motor.mode = CTRL_MODE_SPD;
		PMSM_FOC_RT_PhaseCurrLim(nv_get_foc_params()->s_maxEpmPhaseCurrLim);
		PMSM_FOC_SetCtrlMode(CTRL_MODE_SPD);
	}else {
		motor.epm_dir = EPM_Dir_None;
		motor.mode = CTRL_MODE_TRQ;
		motor.b_epm_cmd_move = false;
		PMSM_FOC_SetCtrlMode(CTRL_MODE_TRQ);
		PMSM_FOC_RT_PhaseCurrLim(PMSM_FOC_GetPhaseCurrLim());
	}
	cpu_exit_critical(mask);
	
	return false;
}

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

bool mc_is_start(void) {
	return (motor.b_start || PMSM_FOC_Is_Start());
}

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

bool mc_command_epm_move(EPM_Dir_t dir) {
	return mc_start_epm_move(dir, true);
}

bool mc_throttle_epm_move(EPM_Dir_t dir) {
	return mc_start_epm_move(dir, false);
}

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_get_running_status(u8 *data) {
	data[0] = motor.mode;
	data[0] |= (PMSM_FOC_AutoHoldding()?1:0) << 2;
	data[0] |= (motor.b_break?1:0) << 3;
	data[0] |= (PMSM_FOC_Get()->in.b_cruiseEna?1:0) << 4;
	data[0] |= (motor.b_start?1:0) << 5;
	data[0] |= (mc_is_epm()?1:0) << 6;
	data[0] |= (motor.b_lock_motor) << 7; //motor locked
}


void mc_encoder_off_calibrate(s16 vd) {
	if (motor.b_start) {
		return;
	}
	motor.b_calibrate = true;
	pwm_up_enable(false);
	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 < 200; i++) {
		for (float angle = 0; angle < 360; angle++) {
			PMSM_FOC_Set_Angle(angle);
			delay_ms(1);
			if (i > 20) {
				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 < 200; i++) {
		for (float angle = 360; angle > 0; angle--) {
			PMSM_FOC_Set_Angle(angle);
			delay_ms(1);
			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();
	pwm_up_enable(true);
	motor_encoder_data_upload();
	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 (motor.b_lock_motor == lock) {
		return true;
	}
	if (motor.b_start) {
		PMSM_FOC_SetErrCode(FOC_NotAllowed);
		return false;
	}
	if (lock && (PMSM_FOC_GetSpeed() > 10)) {
		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
		return false;
	}
	motor.b_lock_motor = lock;
	if (lock) {
		pwm_start();
		pwm_update_duty(FOC_PWM_Half_Period/2, FOC_PWM_Half_Period/2, FOC_PWM_Half_Period/2);
		pwm_enable_channel();
	}else {
		pwm_stop();
	}
	return true;
}

bool mc_auto_hold(bool hold) {
	if (motor.b_auto_hold == hold) {
		return true;
	}
	if (nv_get_foc_params()->n_autoHold == 0) {
		PMSM_FOC_SetErrCode(FOC_NotAllowed);
		return false;
	}
	if (!motor.b_start) {
		PMSM_FOC_SetErrCode(FOC_NotAllowed);
		return false;
	}
	if (hold && !mc_throttle_released()) {
		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
		return false;
	}
	u32 mask = cpu_enter_critical();
	motor.b_auto_hold = hold;
	if (!PMSM_FOC_Is_Start()) {
		PMSM_FOC_Start(motor.mode);
		PMSM_FOC_AutoHold(hold);
		pwm_enable_channel();
	}else {
		PMSM_FOC_AutoHold(hold);
	}
	cpu_exit_critical(mask);
	return true;
}

bool mc_throttle_released(void) {
	return get_throttle_float() <= nv_get_foc_params()->n_minThroVol;
}

static bool mc_is_hwbrake(void) {
	int count = 50;
	int settimes = 0;
	while(count-- > 0) {
		bool b1 = mc_get_gpio_brake();
		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;
	}
}
void MC_Brake_IRQHandler(void) {

	if (mc_is_hwbrake()) {
		motor.b_break = true;
	}else {
		motor.b_break = false;
	}

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

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

void MC_Protect_IRQHandler(void){
	pwm_brake_enable(false);
	shark_timer_post(&_brake_prot_timer, 1000);
	if (!motor.b_start) {
		return;
	}
	_mc_internal_init(CTRL_MODE_OPEN, false);
	adc_stop_convert();
	pwm_stop();
	PMSM_FOC_Stop();
	pwm_up_enable(true);
	PMSM_FOC_SetCriticalError(FOC_CRIT_Phase_Err);
}

void TIMER_UP_IRQHandler(void){
	if (!motor.b_start && !PMSM_FOC_Is_Start()) {
		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();
}

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

static bool mc_run_stall_process(u8 run_mode) {
	if ((run_mode == CTRL_MODE_TRQ || run_mode == CTRL_MODE_SPD) && !PMSM_FOC_AutoHoldding()) {
		//堵转判断
		if (motor.b_runStall) {
			if (!mc_throttle_released()) {
				return true;
			}
			motor.runStall_time = 0;
			motor.b_runStall = false; //转把释放，清除堵转标志
		}else if (PMSM_FOC_Get_Real_Torque() >= CONFIG_STALL_MAX_CURRENT){
			if (ABS(PMSM_FOC_GetSpeed()) < 1.0f && (motor.runStall_time == 0)) {
				motor.runStall_time = get_tick_ms();
				motor.runStall_pos = motor_encoder_get_position();
			}
			if (motor.runStall_time > 0) {
				if (get_delta_ms(motor.runStall_time) >= CONFIG_STALL_MAX_TIME) {
					motor.b_runStall = true;
					motor.runStall_time = 0;
					PMSM_FOC_Set_Torque(0);
					torque_reset();
					return true;
				}
				if (ABS(motor.runStall_pos - motor_encoder_get_position()) >= 0.2f) {
					motor.runStall_time = 0;
				}
			}
		}else {
			motor.runStall_time = 0;
		}
	}
	return false;
}

static void _autohold_beep_timer_handler(shark_timer_t *t) {
	gpio_beep(60);
}

static void mc_autohold_process(void) {
	if (nv_get_foc_params()->n_autoHold == 0) {
		if (PMSM_FOC_AutoHoldding()) {
			mc_auto_hold(false);
		}
		return;
	}
	if (PMSM_FOC_AutoHoldding()) {
		if (!mc_throttle_released()) {
			mc_auto_hold(false);
			motor.b_wait_brk_release = false;
		}else if (!motor.b_break && motor.b_wait_brk_release) {
			motor.b_wait_brk_release = false;
		}else if (motor.b_break && !motor.b_wait_brk_release) {
			mc_auto_hold(false);
		}
	}
	if (!PMSM_FOC_AutoHoldding() && motor.b_break && (encoder_get_speed() == 0.0f)) {
		if (motor.n_autohold_time == 0) {
			motor.n_autohold_time = get_tick_ms();
		}else {
			if (get_delta_ms(motor.n_autohold_time) >= CONFIG_AUTOHOLD_DETECT_TIME) {
				if (mc_auto_hold(true)) {
					motor.b_wait_brk_release = true;
					shark_timer_post(&_autohold_beep_timer, 0);
				}
			}
		}
	}else {
		motor.n_autohold_time = 0;
	}
}

static void mc_process_throttle_epm(void) {
	if (!motor.b_epm_cmd_move) {//通过命令前进后退，不处理转把
		if (mc_throttle_released()) {
			mc_throttle_epm_move(EPM_Dir_None);
		}else {
			mc_throttle_epm_move(EPM_Dir_Forward);
		}
	}
}

/*FOC 的部分处理，比如速度环，状态机，转把采集等*/
measure_time_t g_meas_MCTask;
void Sched_MC_mTask(void) {
	time_measure_start(&g_meas_MCTask);
	u8 runMode = PMSM_FOC_CtrlMode();

	/*保护功能*/
	PMSM_FOC_RunTime_Limit();
	/* 母线电流，实际采集的相电流矢量大小的计算 */
	PMSM_FOC_Calc_Current();

	if (motor.b_calibrate || (motor.mode == CTRL_MODE_OPEN)) {
		return;
	}
	/* 堵转处理 */
	if (mc_run_stall_process(runMode) || (runMode == CTRL_MODE_CURRENT)) {
		eCtrl_Running();
		PMSM_FOC_Slow_Task();
		return;
	}

	if ((runMode != CTRL_MODE_OPEN) || (motor.mode != CTRL_MODE_OPEN)) {
#ifndef CONFIG_DQ_STEP_RESPONSE
		mc_autohold_process();

		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);
				}
			}
			if (!PMSM_FOC_Is_Start() && (!mc_throttle_released())) {
				mask = cpu_enter_critical();
				PMSM_FOC_Start(motor.mode);
				torque_reset();
				pwm_enable_channel();
				cpu_exit_critical(mask);
			}
		}
		if (runMode != CTRL_MODE_OPEN) {
			eCtrl_Running();
			if ((runMode == CTRL_MODE_SPD) && mc_is_epm()) {
				mc_process_throttle_epm();
			}else {
				throttle_process(runMode, get_throttle_float());
			}
			PMSM_FOC_Slow_Task();
		}
#endif
	}
}