Motor_PMSM/Application/FOC/foc.c

#include <string.h>
#include "libs/task.h"
#include "bsp/bsp.h"
#include "foc/foc.h"
#include "foc/park_clark.h"
#include "foc/svpwm.h"
#include "foc/foc_task.h"
#include "foc/phase_current.h"
#include "foc/hall_sensor.h"
#include "foc/vbus_sensor.h"
#include "foc/ntc_sensor.h"

static u32 foc_measure_task(void);
static void foc_defulat_value(void);

static motor_foc_t mFOC = {
	.motor_p = {
		.poles = 2,
		.ld = 0.578477f,
		.lq = 5.78477f,
		.rs = 1.088f,
		.inertia = 3.319367f,
		.b_emf = 4.332566f,
	},
};

void foc_init(void) {
	foc_defulat_value();	
	HAL_ADC1_Enable();
	/* init pwm hardware timer */
	PWM_TimerEnable();
	/* enable tim4 to run the foc normal task */
	TIM4_Enable();

	hall_sensor_init();
	vbus_sensor_init();
	ntc_sensor_init();
	
	task_start(foc_measure_task, 0);

	///hall_sensor_calibrate(8.0f, mFOC.hall_table);
}

static void foc_defulat_value(void){
	memset(&mFOC, 0, sizeof(mFOC));
	mFOC.state = IDLE;
	mFOC.mosGate = false;
	mFOC.vbus = 12.0f;
	phase_current_init(&mFOC.current_samp);
}


void foc_clear(void) {
	PWM_Stop();
	mFOC.mosGate = false;
	foc_defulat_value();
	hall_sensor_init();
}

FOCState FOC_STM_State(void){
	return mFOC.state;
}

u32 foc_get_speed(void) {
	return abs(hall_sensor_get_speed()/mFOC.motor_p.poles);
}

FError FOC_STM_NextState(FOCState state) {
	bool changed = false;
	if (state == mFOC.state) {
		return NoError;
	}
	if (state == START) {
		if (mFOC.state == IDLE) {
			changed = true;
		}
	}else if (state == IDLE) {
		if (mFOC.state == ANY_STOP) {
			changed = true;
		}
	}else if (state == ANY_STOP) {
		if (mFOC.state != IDLE) {
			changed = true;
		}
	}else if (state == CURRENT_CALIBRATE) {
		if (mFOC.state == START) {
			changed = true;
		}
	}else if (state == READY_TO_RUN) {
		if (mFOC.state == CURRENT_CALIBRATE) {
			changed = true;
		}
	}else if (state == RAMPING_START) {
		if (mFOC.state == READY_TO_RUN) {
			changed = true;
		}
	}else if (state == RUNNING) {
		if (mFOC.state == RAMPING_START) {
			changed = true;
		}
	}
	if (changed) {
		mFOC.state = state;
		return NoError;
	}
	return STMNotAllow;
}

/* ÉèÖÃÆô¶¯rampµçÁ÷ºÍʱ¼ä */
void Foc_Set_StartRamp(float final, u32 duration_ms){
	ramp_ctrl_init(&mFOC.start_ramp, 0.0f, final, duration_ms);
}

FError foc_start_motor(void){
	return FOC_STM_NextState(START);
}

FError foc_stop_motor(void) {
	return FOC_STM_NextState(ANY_STOP);
}

void foc_current_calibrate(void){
	mFOC.current_samp.adc_offset_a = 0;
	mFOC.current_samp.adc_offset_b = 0;
	mFOC.current_samp.adc_offset_c = 0;

	PWM_Disable_Channels();
	//foc_pwm_start(false);

	task_udelay(10);
	
	phase_current_init(&mFOC.current_samp);
	mFOC.current_samp.is_calibrating = true;
	mFOC.current_samp.sector = SECTOR_5;
	foc_pwm_start(true);
	HAL_ADC1_InJ_StartConvert();
	while(mFOC.current_samp.offset_sample_count != 0){};
	
	foc_pwm_start(false);
	task_udelay(100);
	phase_current_init(&mFOC.current_samp);	
	mFOC.current_samp.sector = SECTOR_1;
	foc_pwm_start(true);
	while(mFOC.current_samp.offset_sample_count != 0){};
	mFOC.current_samp.is_calibrating = false;
	foc_pwm_start(false);
	PWM_Enable_Channels();
}

void foc_overide_theta(bool enable){
	mFOC.override.is_theta = enable;
}

void foc_overide_vdq(bool enable){
	mFOC.override.is_vdq = enable;
}

void foc_overide_set_theta(float theta){
	mFOC.override.theta = theta;
}

void foc_overide_set_vdq(float d, float q){
	mFOC.override.vdq.d = d;
	mFOC.override.vdq.q = q;
}


static u32 foc_measure_task(void){
	vbus_sample_voltage();
	ntc_sensor_sample();
	LowPass_Filter(mFOC.vbus, vbus_get_voltage(), 0.1f);
	wdog_reload();
	return 1;
}

void foc_brake_handler(void) {

}

void foc_pwm_up_handler(void){
	phase_current_adc_triger(&mFOC.current_samp);
}

#if defined (CCMRAM)
#if defined (__ICCARM__)
#pragma location = ".ccmram"
#elif defined (__CC_ARM)
__attribute__( ( section ( ".ccmram" ) ) )
#endif
#endif

void current_sample_handler(void) {
	if (mFOC.current_samp.is_calibrating) {
		phase_current_offset(&mFOC.current_samp);
	}else {
		FOC_Fast_Task(&mFOC);
	}
}

void foc_slow_task_handler(void) {
	FOC_Normal_Task(&mFOC);
}

void foc_pwm_start(bool start) {
	if (start == mFOC.mosGate) {
		return;
	}
	if (start) {
		PWM_Start();
	}else {
		PWM_Stop();
	}
	mFOC.mosGate = start;	
}