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 "foc/core/e_ctrl.h"
#include "foc/foc_config.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();
	eCtrl_init(0, 0); //default
	PMSM_FOC_Init();
	pmsm_foc.FOC_In  = PMSM_FOC_GetInputs();
	pmsm_foc.FOC_Out = PMSM_FOC_GetOutputs();
	PMSM_FOC_iBusLimit(S16Q5(Default_DC_iLimit));
	PMSM_FOC_SpeedLimit(S32Q4(Default_Spd_Limit));
}

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


void PMSM_FOC_Schedule(void) {
	u8  hall[3];
	pwm_clear_updata();
	phase_current_sample(&pmsm_foc.FOC_In->adc_Pha, &pmsm_foc.FOC_In->adc_Phb);
	pmsm_foc.FOC_In->us_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];

	if (pmsm_foc.FOC_In->n_ctrlModReq == SPD_MODE) {
		pmsm_foc.FOC_In->spd_Target = S32Q4(eCtrl_get_RefSpd());
		pmsm_foc.FOC_In->idq_Target = S16Q5(eCtrl_get_FinalTorque());
	}else if(pmsm_foc.FOC_In->n_ctrlModReq == TRQ_MODE){
		pmsm_foc.FOC_In->spd_Target = S32Q4(eCtrl_get_FinalSpd());
		pmsm_foc.FOC_In->idq_Target = S16Q5(eCtrl_get_RefTorque());
	}
	
	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->FOC_Flags = 0;
	pmsm_foc.FOC_In->spd_Target = 0;
	pmsm_foc.FOC_In->idq_Target = 0;
	pmsm_foc.FOC_In->vdq_Open[0] = 0;
	pmsm_foc.FOC_In->vdq_Open[1] = 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(_s16q5_t ibusLimit) {
	pmsm_foc.FOC_In->iDC_Limit = (ibusLimit);
}

void PMSM_FOC_SpeedLimit(_s32q4_t speedLimit) {
	pmsm_foc.FOC_In->spd_Limit = (speedLimit);
}

float PMSM_FOC_GetSpeedLimit(void) {
	return S32Q4toF(pmsm_foc.FOC_In->spd_Limit);
}

void PMSM_FOC_VbusVoltage(_s16q5_t vbusVol) {
	pmsm_foc.FOC_In->vDC = (vbusVol);
}

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

void PMSM_FOC_SetOpenVdq(_s16q5_t vd, _s16q5_t vq) {
	pmsm_foc.FOC_In->vdq_Open[0] = (vd);
	pmsm_foc.FOC_In->vdq_Open[1] = (vq);
}

bool PMSM_FOC_EnableCruise(boolean_T enable) {
	if (enable) {
		float motSpd = PMSM_FOC_GetSpeed();
		if (motSpd < MIN_CRUISE_RPM) { //
			PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
			return false;
		}
		eCtrl_set_TargetSpeed(motSpd);
	}
	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->n_runingMode == SPD_MODE));
}

bool PMSM_FOC_Set_Speed(float rpm) {
	if (pmsm_foc.FOC_In->b_cruiseEna) {
		return false;
	}
	eCtrl_set_TargetSpeed(rpm);
	return true;
}

bool PMSM_FOC_Set_Current(float current) {
	eCtrl_set_TrqCurrent(current);
	return true;
}

bool PMSM_FOC_Set_CruiseSpeed(float rpm) {
	if (PMSM_FOC_Is_CruiseEnabled()) {
		eCtrl_set_TargetSpeed(rpm);
		return true;
	}
	PMSM_FOC_SetErrCode(FOC_NotCruiseMode);
	return false;
}

void PMSM_FOC_HallCalibrate(boolean_T b_caliHall, _s16q5_t open_vd) {
	uint16_T foc_cali = 0;
	if (b_caliHall) {
		foc_cali = FOC_CALIMOD_HALL;
	}
	if ((pmsm_foc.FOC_In->FOC_Flags & FOC_CALIMOD_HALL) == foc_cali) {
		return;
	}
	if (pmsm_foc.FOC_In->b_motEna == b_caliHall) { //motor must be stoped when start cali
		return;
	}
	pmsm_foc.FOC_In->FOC_Flags &= ~(FOC_CALIMOD_HALL);
	pmsm_foc.FOC_In->b_motEna = b_caliHall;
	pmsm_foc.FOC_In->FOC_Flags |= foc_cali;
	pmsm_foc.FOC_In->vdq_Open[0] = (open_vd);
	pmsm_foc.FOC_In->vdq_Open[1] = 0;
	pmsm_foc.FOC_In->n_ctrlModReq = OPEN_MODE;
}

float PMSM_FOC_GetSpeed(void) {
	return S32Q4toF(pmsm_foc.FOC_Out->f_MotRPM);
}


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


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

//获取母线电流
float PMSM_FOC_Get_iDC(void) {
	s32 vd = pmsm_foc.FOC_Out->f_Vdq[0];
	s32 vq = pmsm_foc.FOC_Out->f_Vdq[1];
	s32 id = pmsm_foc.FOC_Out->f_Idq[0];
	s32 iq = pmsm_foc.FOC_Out->f_Idq[1];
    /*
		根据公式（等幅值变换，功率不等）:
		iDC x vDC = 2/3(iq x vq + id x vd);
	*/
	s32 m_pow = (vd * id + vq * iq); //s32q10
	s16 iDC = m_pow / pmsm_foc.FOC_In->vDC; //s16q5
	return S16Q5toF(iDC) * 0.667f;
}

void PMSM_FOC_Brake(bool brake) {
	pmsm_foc.b_brake_in = brake;
	if (!pmsm_foc.FOC_In->b_motEna) {
		return;
	}
	if (pmsm_foc.b_brake_in & pmsm_foc.FOC_In->b_cruiseEna) {
		pmsm_foc.FOC_In->b_cruiseEna = false;
	}
	eCtrl_brake_signal(brake);
}