MC100/Applications/foc/core/foc_core.c

#include "os/os_task.h"
#include "bsp/pwm.h"
#include "bsp/adc.h"
#include "bsp/mc_hall_gpio.h"
#include "foc/core/foc_core.h"
#include "foc/motor/current.h"
#include "bsp/timer_count32.h"
#include "libs/time_measure.h"
#include "libs/logger.h"

pmsm_foc_t pmsm_foc = {0};

extern void PMSM_FOC_Init(void);
extern ExtU *PMSM_FOC_GetInputs(void);
extern ExtY *PMSM_FOC_GetOutputs(void);
extern void PMSM_FOC_Step(void);

void PMSM_FOC_CoreInit(void) {
	mc_hall_init();
	PMSM_FOC_Init();
	pmsm_foc.FOC_In  = PMSM_FOC_GetInputs();
	pmsm_foc.FOC_Out = PMSM_FOC_GetOutputs();
}

static __INLINE void PMSM_FOC_PWMCurrent_Update(void) {
	current_samp_t *cs = get_phase_sample_point(pmsm_foc.FOC_Out->sector);
	
	pwm_update_duty(pmsm_foc.FOC_Out->PWM[0], pmsm_foc.FOC_Out->PWM[1], pmsm_foc.FOC_Out->PWM[2]);
	
	pwm_update_sample(cs->time.Samp_p1, cs->time.Samp_p2, cs->sector);
}

static __INLINE void PMSM_FOC_Controller(void) {
	u8  hall[3];
	pwm_clear_updata();
	phase_current_sample(&pmsm_foc.FOC_In->adc_a, &pmsm_foc.FOC_In->adc_b);
	pmsm_foc.FOC_In->hw_count = hall_get_hwcount(hall);
	pmsm_foc.FOC_In->hall_a = hall[0];
	pmsm_foc.FOC_In->hall_b = hall[1];
	pmsm_foc.FOC_In->hall_c = hall[2];
	pmsm_foc.FOC_In->input_target = (s16)ramp_get_target(&pmsm_foc.speed_ramp);

	if (pmsm_foc.b_brake_in) {
		pmsm_foc.FOC_In->b_motEna = false;
	}else {
		pmsm_foc.FOC_In->b_motEna = pmsm_foc.b_FocEna;
	}
	
	PMSM_FOC_Step();

	PMSM_FOC_PWMCurrent_Update();
}

void PMSM_FOC_Start(u8 nCtrlMode) {
	if (pmsm_foc.b_FocEna) {
		return;
	}
	pmsm_foc.b_FocEna = true;
	pmsm_foc.FOC_In->b_motEna = true;
	pmsm_foc.FOC_In->n_ctrlModReq = nCtrlMode;
	pmsm_foc.FOC_In->b_cruiseEna = 0;
	pmsm_foc.FOC_In->b_hall_calibrate = 0;
	pmsm_foc.FOC_In->input_target = 0;
	pmsm_foc.FOC_In->vd_open_target = 0;
	pmsm_foc.FOC_In->vq_open_target = 0;
}

void PMSM_FOC_Stop(void) {
	if (!pmsm_foc.b_FocEna) {
		return;
	}
	pmsm_foc.b_FocEna = false;
	memset(pmsm_foc.FOC_In, 0, sizeof(ExtU));
}

void PMSM_FOC_iBusLimit(int16_T ibusLimit) {
	pmsm_foc.FOC_In->i_dc_limit = ibusLimit;
}

void PMSM_FOC_SpeedLimit(int16_T speedLimit) {
	pmsm_foc.FOC_In->speed_limit = speedLimit;
}

void PMSM_FOC_VbusVoltage(int16_T vbusVol) {
	pmsm_foc.FOC_In->vbus_voltage = vbusVol;
}

void PMSM_FOC_SetCtrlMode(uint8_T mode) {
	pmsm_foc.FOC_In->n_ctrlModReq = mode;
}

void PMSM_FOC_SetOpenVdq(int16_T vd, int16_T vq) {
	pmsm_foc.FOC_In->vd_open_target = vd;
	pmsm_foc.FOC_In->vq_open_target = vq;
}

bool PMSM_FOC_EnableCruise(boolean_T enable) {
	if (enable) {
		if (pmsm_foc.FOC_Out->rpm < 100) { //
			PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
			return false;
		}
		ramp_set_target(&pmsm_foc.speed_ramp, ramp_get_target(&pmsm_foc.speed_ramp), pmsm_foc.FOC_Out->rpm, 500);
	}
	pmsm_foc.FOC_In->b_cruiseEna = enable;
	return true;
}

bool PMSM_FOC_Is_CruiseEnabled(void) {
	return (pmsm_foc.FOC_In->b_cruiseEna && (pmsm_foc.FOC_Out->running_mode == SPD_MODE));
}

bool PMSM_FOC_Set_Speed(s16 rpm, u32 ramp) {
	if (!pmsm_foc.FOC_In->b_cruiseEna) {
		ramp_set_target(&pmsm_foc.speed_ramp, ramp_get_target(&pmsm_foc.speed_ramp), rpm, ramp);
	}
	return true;
}

bool PMSM_FOC_Set_CruiseSpeed(s16 rpm) {
	if (PMSM_FOC_Is_CruiseEnabled()) {
		ramp_set_target(&pmsm_foc.speed_ramp, ramp_get_target(&pmsm_foc.speed_ramp), rpm, 0);
		return true;
	}
	PMSM_FOC_SetErrCode(FOC_NotCruiseMode);
	return false;
}

void PMSM_FOC_HallCalibrate(boolean_T b_caliHall, int16_T open_vd) {
	if (pmsm_foc.FOC_In->b_hall_calibrate == b_caliHall) {
		return;
	}
	if (pmsm_foc.FOC_In->b_motEna == b_caliHall) { //motor must be stoped when start cali
		return;
	}
	pmsm_foc.FOC_In->b_motEna = b_caliHall;
	pmsm_foc.FOC_In->b_hall_calibrate = b_caliHall;
	pmsm_foc.FOC_In->vq_open_target = 0;
	pmsm_foc.FOC_In->vd_open_target = open_vd;
	pmsm_foc.FOC_In->n_ctrlModReq = 0;
}

s16 PMSM_FOC_GetSpeed(void) {
	return pmsm_foc.FOC_Out->rpm;
}


void PMSM_FOC_SetErrCode(u8 code) {
	pmsm_foc.error_code = code;
}


u8 PMSM_FOC_GetErrCode(void) {
	return pmsm_foc.error_code;
}

void foc_brake_handler(bool brake) {
	pmsm_foc.b_brake_in = brake;
	if (pmsm_foc.b_brake_in & pmsm_foc.FOC_In->b_cruiseEna) {
		pmsm_foc.FOC_In->b_cruiseEna = false;
	}
}

void foc_pwm_up_handler(void){
	phase_current_adc_triger();
}

measure_time_t g_meas_foc = {.exec_max_time = 15,};
#define TIME_MEATURE_START() time_measure_start(&g_meas_foc)
#define TIME_MEATURE_END() time_measure_end(&g_meas_foc)

/*ADC 电流采集中断，调用FOC的核心处理函数*/
void mc_phase_current_irq(void) {
	if (phase_current_offset()) {//check if is adc offset checked
		return;
	}
	TIME_MEATURE_START();
	PMSM_FOC_Controller();
	TIME_MEATURE_END();
}