负载识别，空载和带负载速度环参数不一样

Signed-off-by: huhui <huhui@sharkgulf.com>

Applications/app/app.c

@@ -14,6 +14,7 @@
 #include "app/nv_storage.h"
 #include "foc/commands.h"
 #include "foc/core/thro_torque.h"
+#include "foc/core/F_Calc.h"
 #include "foc/motor/motor_param.h"
 
 
@@ -138,12 +139,13 @@ static u32 _app_report_task(void *p) {
 		encoder_log();
 		//sample_log();
 		PMSM_FOC_LogDebug();
+		F_debug();
 		//eCtrl_debug_log();
 		//err_code_log();
 	}
 	return 200;
 }
-int plot_type = 9;
+int plot_type = 10;
 static void plot_smo_angle(void) {
 	float smo_angle = foc_observer_sensorless_angle();
 	float delta = smo_angle - PMSM_FOC_Get()->in.s_motAngle;
@@ -154,7 +156,8 @@ static void plot_smo_angle(void) {
 }
 static u32 _app_plot_task(void * args) {
 	if (plot_type == 1) {
-		can_plot2((s16)foc_observer_sensorless_speed(), (s16)PMSM_FOC_GetSpeed());
+		//can_plot2((s16)foc_observer_sensorless_speed(), (s16)PMSM_FOC_GetSpeed());
+		can_plot3((s16)PMSM_FOC_GetSpeed() + 1000, (s16)PMSM_FOC_GetSpeed(), (s16)PMSM_FOC_GetSpeed() - 1000);
 	}else if (plot_type == 2) {
 		can_plot2(eCtrl_get_RefTorque(), eCtrl_get_FinalTorque());
 	}else if (plot_type == 3) {
@@ -175,6 +178,8 @@ static u32 _app_plot_task(void * args) {
 		#endif
 	}else if (plot_type == 9) {
 		can_plot3((s16)PMSM_FOC_Get()->out.s_FilteriDC, (s16)PMSM_FOC_Get()->out.s_CalciDC, (s16)PMSM_FOC_Get()->out.s_CalciDC2);
+	}else if (plot_type == 10) {
+		can_plot3((s16)F_get_MotAccl() * 100, (s16)PMSM_FOC_GetSpeed(), (s16)F_get_Te());
 	}
 	
 	return 20;

Applications/app/nv_storage.c

@@ -49,8 +49,12 @@ static void nv_default_motor_params(void) {
 	m_params.flux_linkage = 0;
 #endif
 	m_params.offset = MOTOR_ENC_OFFSET;//180;//(69.0f);
-	m_params.est_pll_band = 100;
+	m_params.est_pll_band = 50;
+	m_params.epm_pll_band = 150;
 	m_params.pos_lock_pll_band = 500;
+	m_params.velocity_weight = 190;
+	m_params.velocity_C = 145;
+	m_params.gear_ratio = 6250;//6.25:1
 }
 
 static void nv_default_limter(void) {
@@ -508,6 +512,6 @@ void nv_storage_init(void) {
 		foc_params.s_PhaseCurrLim = CONFIG_DEFAULT_PHASE_CURR_LIM;
 	}
 #endif
-	sys_debug("%f -- %f, flux: %f\n", foc_params.n_currentBand, m_params.ld, m_params.flux_linkage);
+	sys_debug("%f -- %f, flux: %f, %d\n", foc_params.n_currentBand, m_params.ld, m_params.flux_linkage, sizeof(m_params));
 }
 

Applications/app/nv_storage.h

@@ -52,6 +52,11 @@ typedef struct {
 	float offset;
 	float est_pll_band; //normal工作模式下的pll带宽
 	float pos_lock_pll_band; //电机锁定模式下的pll带宽
+	float epm_pll_band;
+	s16   velocity_weight;//车重， kg
+	s16   velocity_C; //车轮周长， cm
+	s16   gear_ratio; //传动比
+	u8    res[2048 - 72 - 6];
 	u16   crc16;
 }motor_params_t;
 

Applications/foc/core/F_Calc.c

@@ -0,0 +1,62 @@
+#include "os/os_types.h"
+#include "foc/core/PMSM_FOC_Core.h"
+#include "libs/logger.h"
+#include "math/fast_math.h"
+#include "app/nv_storage.h"
+
+static float F_Te = 0.0f; //电磁转矩生成的力
+static float F_Air = 0.0f; //空气阻力
+static float F_accl = 0.0f; //加速阻力
+static float mot_vel = 0.0f; //公里每小时
+static float mot_accl = 0.0f; //加速度 m/ss
+static float C = 1.4f; //轮子周长
+static float gear_ratio = 6.5f; //传动比
+static float M  = 190.0f; //190公斤
+static bool _init = false;
+void F_all_Calc(void) {
+	if (!_init) {
+		_init = true;
+		C = (float)nv_get_motor_params()->velocity_C / 100.0f;
+		M = (float)nv_get_motor_params()->velocity_weight;
+		gear_ratio = (float)nv_get_motor_params()->gear_ratio / 1000.0f;
+	}
+	float kmph = PMSM_FOC_GetSpeed() / gear_ratio * C * 60.0f / 1000.0f;
+	if (mot_vel == 0.0f) {
+		mot_vel = kmph;
+		mot_accl = 0.0f;
+		F_Air = 0.0f;
+		F_accl = 0.0f;
+		return;
+	}
+	float delta_mph = (kmph - mot_vel) * 1000.0f / 3600.0f;
+	float diff = delta_mph / (CONFIG_SPD_CTRL_MS / 1000.0f);
+	LowPass_Filter(mot_accl, diff, 0.03f);
+
+	F_accl = M * mot_accl;
+	F_Air = SQ(kmph) * 0.02f;
+	PMSM_FOC_Ctrl *foc = PMSM_FOC_Get();
+	F_Te = 1.5f * foc->params.n_poles * (foc->params.flux * foc->out.s_RealIdq.q + 
+					(foc->params.ld - foc->params.lq) * foc->out.s_RealIdq.q * foc->out.s_RealIdq.d) * gear_ratio / (C / (2 * PI));
+	mot_vel = kmph;
+}
+
+float F_get_Te(void) {
+	return F_Te;
+}
+
+float F_get_accl(void) {
+	return F_accl;
+}
+
+float F_get_air(void) {
+	return F_Air;
+}
+
+float F_get_MotAccl(void) {
+	return mot_accl;
+}
+
+void F_debug(void) {
+	sys_debug("F: %f, %f, %f, %f, %f\n", F_Te, F_Air, F_accl, mot_vel, mot_accl);
+	sys_debug("Vel: %f, %f, %f\n", C, M, gear_ratio);
+}

Applications/foc/core/F_Calc.h

@@ -0,0 +1,12 @@
+#ifndef _F_CALC_H__
+#define _F_CALC_H__
+
+void F_all_Calc(void);
+float F_get_Te(void);
+float F_get_accl(void);
+float F_get_air(void);
+float F_get_MotAccl(void);
+void F_debug(void);
+
+#endif /* _F_CALC_H__ */
+

Applications/foc/core/PMSM_FOC_Core.c

@@ -10,6 +10,7 @@
 #include "foc/core/svpwm.h"
 #include "foc/core/thro_torque.h"
 #include "foc/core/foc_observer.h"
+#include "foc/core/F_Calc.h"
 #include "foc/samples.h"
 #include "foc/limit.h"
 #include "app/nv_storage.h"
@@ -154,7 +155,7 @@ static void PMSM_FOC_Conf_PID(void) {
 
 #ifdef CONFIG_SPEED_LADRC
 	ladrc_init(&gFoc_Ctrl.vel_lim_adrc, slow_ctrl_ts, nv_get_foc_params()->f_adrc_vel_lim_Wo, nv_get_foc_params()->f_adrc_vel_lim_Wcv, nv_get_foc_params()->f_adrc_vel_lim_B0);
-	ladrc_init(&gFoc_Ctrl.vel_adrc, slow_ctrl_ts, nv_get_foc_params()->f_adrc_vel_lim_Wo, nv_get_foc_params()->f_adrc_vel_lim_Wcv, nv_get_foc_params()->f_adrc_vel_lim_B0);
+	ladrc_init(&gFoc_Ctrl.vel_adrc, slow_ctrl_ts, nv_get_foc_params()->f_adrc_vel_Wo, nv_get_foc_params()->f_adrc_vel_Wcv, nv_get_foc_params()->f_adrc_vel_B0);
 #else
 	gFoc_Ctrl.pi_torque.kp = nv_get_foc_params()->pid_conf[PID_TRQ_id].kp;
 	gFoc_Ctrl.pi_torque.ki = nv_get_foc_params()->pid_conf[PID_TRQ_id].ki;
@@ -538,7 +539,7 @@ u8 PMSM_FOC_CtrlMode(void) {
 			ladrc_copy(&gFoc_Ctrl.vel_adrc, &gFoc_Ctrl.vel_lim_adrc);
 #else
 			float target_troque = gFoc_Ctrl.in.s_targetTorque;
-			if (gFoc_Ctrl.pi_id->is_sat || gFoc_Ctrl.pi_iq->is_sat) {
+			if (gFoc_Ctrl.pi_id.is_sat || gFoc_Ctrl.pi_iq.is_sat) {
 				target_troque = PMSM_FOC_Get_Real_dqVector();
 			}
 			PI_Controller_Reset(&gFoc_Ctrl.pi_speed, target_troque);
@@ -590,6 +591,42 @@ static void crosszero_step_towards(float *value, float target) {
 
 
 /* MPTA, 弱磁, 功率限制，主要是分配DQ轴电流 */
+
+#define CHANGE_MAX_CNT 3
+static __INLINE void PMSM_FOC_VelCtrl_Decide(void) {
+	static int change_cnt = 0;
+	static bool change_done = false;
+	float f_te = F_get_Te();
+	if (gFoc_Ctrl.in.s_motVelocity == 0.0f || gFoc_Ctrl.out.n_RunMode == CTRL_MODE_OPEN) {
+		change_cnt = 0;
+		change_done = false;
+		return;
+	}
+	if (f_te <= 0.0f) {
+		change_cnt = 0;
+		return;
+	}
+	float f_accl = F_get_accl();
+
+	if ((f_accl > 200.0f) && (f_accl/f_te > 1.5f )) {
+		change_cnt++;
+	}else if ((F_get_MotAccl() >= 10.0f) && (f_accl/f_te > 1.2f )) {
+		change_cnt = CHANGE_MAX_CNT;
+	}
+	else {
+		change_cnt = 0;
+	}
+	if (!change_done && (change_cnt >= CHANGE_MAX_CNT)) {
+		change_done = true;
+#ifdef CONFIG_SPEED_LADRC
+		ladrc_change_b0(&gFoc_Ctrl.vel_lim_adrc, 2500.0f);
+		ladrc_change_K(&gFoc_Ctrl.vel_lim_adrc, 3);
+		ladrc_change_b0(&gFoc_Ctrl.vel_adrc, 2500.0f);
+		ladrc_change_K(&gFoc_Ctrl.vel_adrc, 3);
+#endif
+	}
+}
+
 static __INLINE float PMSM_FOC_Limit_iDC(float maxTrq) {
 #if 1
 	gFoc_Ctrl.pi_power.max = maxTrq;
@@ -607,8 +644,8 @@ static __INLINE float PMSM_FOC_Limit_Speed(float maxTrq) {
 	return ladrc_run(&gFoc_Ctrl.vel_lim_adrc, lim, gFoc_Ctrl.in.s_motVelocity);
 #else
 #if 1
-	gFoc_Ctrl.pi_torque->max = maxTrq;
-	gFoc_Ctrl.pi_torque->min = 0;
+	gFoc_Ctrl.pi_torque.max = maxTrq;
+	gFoc_Ctrl.pi_torque.min = 0;
 
 	float err = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.rpmLimRamp) - gFoc_Ctrl.in.s_motVelocity;
 	return PI_Controller_RunLimit(&gFoc_Ctrl.pi_torque, err);
@@ -690,16 +727,16 @@ void PMSM_FOC_idqCalc(void) {
 		float maxTrq = ladrc_run(&gFoc_Ctrl.vel_adrc, refSpeed, gFoc_Ctrl.in.s_motVelocity);
 #else
 		if (maxSpeed >= 0) {
-			gFoc_Ctrl.pi_speed->max = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp);//gFoc_Ctrl.userLim.s_PhaseCurrLim;
-			gFoc_Ctrl.pi_speed->min = -CONFIG_MAX_NEG_TORQUE;
+			gFoc_Ctrl.pi_speed.max = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp);//gFoc_Ctrl.userLim.s_PhaseCurrLim;
+			gFoc_Ctrl.pi_speed.min = -CONFIG_MAX_NEG_TORQUE;
 		}else if (maxSpeed < 0) {
-			gFoc_Ctrl.pi_speed->min = -eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp);//gFoc_Ctrl.userLim.s_PhaseCurrLim;
-			gFoc_Ctrl.pi_speed->max = CONFIG_MAX_NEG_TORQUE;
+			gFoc_Ctrl.pi_speed.min = -eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp);//gFoc_Ctrl.userLim.s_PhaseCurrLim;
+			gFoc_Ctrl.pi_speed.max = CONFIG_MAX_NEG_TORQUE;
 		}
 
 		if ((maxSpeed == 0) && (gFoc_Ctrl.in.s_motVelocity < CONFIG_MIN_RPM_EXIT_EBRAKE)) {
-			gFoc_Ctrl.pi_speed->max = 0;
-			gFoc_Ctrl.pi_speed->min = 0; //防止倒转
+			gFoc_Ctrl.pi_speed.max = 0;
+			gFoc_Ctrl.pi_speed.min = 0; //防止倒转
 		}
 		gFoc_Ctrl.in.s_targetRPM = refSpeed;
 		float errRef = refSpeed - gFoc_Ctrl.in.s_motVelocity;
@@ -738,6 +775,7 @@ void PMSM_FOC_Slow_Task(void) {
 	eRamp_running(&gFoc_Ctrl.rtLim.DCCurrLimRamp);
 	eRamp_running(&gFoc_Ctrl.rtLim.rpmLimRamp);
 	eRamp_running(&gFoc_Ctrl.in.cruiseRpmRamp);
+	PMSM_FOC_VelCtrl_Decide();
 	PMSM_FOC_idqCalc();
 }
 
@@ -966,9 +1004,8 @@ void PMSM_FOC_Reset_Torque(void) {
 
 bool PMSM_FOC_Set_CruiseSpeed(float rpm) {
 	if (PMSM_FOC_Is_CruiseEnabled()) {
-		if (rpm < CONFIG_MIN_CRUISE_RPM) {
-			PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
-			return false;
+		if (rpm < CONFIG_MIN_CRUISE_RPM + 20) {
+			rpm = CONFIG_MIN_CRUISE_RPM + 20;
 		}
 		gFoc_Ctrl.in.s_cruiseRPM = min(ABS(rpm), gFoc_Ctrl.userLim.s_motRPMLim)*SIGN(rpm);
 		eRamp_set_step_target(&gFoc_Ctrl.in.cruiseRpmRamp, gFoc_Ctrl.in.s_cruiseRPM, CONFIG_eCTRL_STEP_TS);

Applications/foc/core/adrc.c

@@ -1,7 +1,7 @@
 #include "adrc.h"
 
 void ladrc_init(ladrc_t*  adrc, float ts, float wo, float wc, float b0) {
-	adrc->z1 = adrc->z2 = adrc->last_z1 = adrc->last_z2 = adrc->out = 0;
+	adrc->z1 = adrc->z2 = adrc->out = 0;
 	adrc->ts = ts;
 	adrc->b0 = b0;
 	adrc->wcv = wc;
@@ -13,13 +13,11 @@ float ladrc_run(ladrc_t *adrc, float in, float feedback) {
 	float y = feedback;
 	float u = adrc->out;
 	//do eso
-	float error = y - adrc->last_z1;
+	float error = y - adrc->z1;
 	
-	adrc->z2 = adrc->last_z2 + adrc->ts * (error * adrc->p2);
-	adrc->z1 = adrc->last_z1 + adrc->ts * (error * adrc->p1 + adrc->z2 + u * adrc->b0);
+	adrc->z2 = adrc->z2 + adrc->ts * (error * adrc->p2);
+	adrc->z1 = adrc->z1 + adrc->ts * (error * adrc->p1 + adrc->z2 + u * adrc->b0);
 
-	adrc->last_z2 = adrc->z2;
-	adrc->last_z1 = adrc->z1;
 	//do error compestion
 	float ref = in;
 	float out = ((ref - adrc->z1) * adrc->wcv - adrc->z2) / adrc->b0;
@@ -29,16 +27,14 @@ float ladrc_run(ladrc_t *adrc, float in, float feedback) {
 }
 
 void ladrc_reset(ladrc_t* adrc, float feedback, float out_init) {
-	adrc->last_z1 = feedback;
-	adrc->last_z2 = -out_init * adrc->b0;
-	adrc->out = adrc->last_z2;
+	adrc->z1 = feedback;
+	adrc->z2 = -out_init * adrc->b0;
+	adrc->out = adrc->z2;
 }
 
 void ladrc_copy(ladrc_t *dst, ladrc_t *src) {
 	dst->z1 = src->z1;
 	dst->z2 = src->z2;
-	dst->last_z1 = src->last_z1;
-	dst->last_z2 = src->last_z2;
 	dst->out = src->out;
 }
 

Applications/foc/core/adrc.h

@@ -9,7 +9,6 @@ typedef struct {
 	float b0;
 	float z1,z2;
 	float p1,p2;
-	float last_z1, last_z2;
 	float ts;
 	//线性状态误差反馈控制器
 	float wcv;
@@ -27,5 +26,23 @@ static void __INLINE ladrc_set_range(ladrc_t *adrc, float min, float max) {
 	adrc->out_min = min;
 	adrc->out_max = max;
 }
+
+
+static void __INLINE ladrc_change_b0(ladrc_t *adrc, float b0) {
+	adrc->b0 = b0;
+}
+
+static void __INLINE ladrc_change_K(ladrc_t *adrc, float k) {
+	adrc->wcv = k;
+}
+
+static void ladrc_change_params(ladrc_t*  adrc, float wo, float wc, float b0) {
+	adrc->b0 = b0;
+	adrc->wcv = wc;
+	adrc->p1 = 2 * wo;
+	adrc->p2 = wo * wo;
+}
+
+
 #endif /* _ADRC_H__ */
 

Applications/foc/foc_config.h

@@ -43,7 +43,7 @@
 #define CONFIG_EBRK_RAMP_TIME 500
 #define CONFIG_AUTOHOLD_DETECT_TIME 3000
 
-#define CONFIG_CRUISE_RAMP_TIME 5000
+#define CONFIG_CRUISE_RAMP_TIME 2000
 
 #define CONFIG_LIMIT_RAMP_TIME (2 * 1000)
 

Applications/foc/motor/encoder.c

@@ -45,7 +45,7 @@ static void _init_pll(void) {
 	g_encoder.est_pll.DT = FOC_CTRL_US;
 	g_encoder.est_pll.max_wp = g_encoder.cpr;
 	g_encoder.pll_bandwidth = 0;
-	g_encoder.pll_bandwidth_shadow = nv_get_motor_params()->est_pll_band;
+	g_encoder.pll_bandwidth_shadow = 50;//nv_get_motor_params()->est_pll_band;
 	encoder_pll_update_gain();
 	PLL_Reset(&g_encoder.est_pll, (float)_abi_count());
 }
@@ -94,6 +94,14 @@ void encoder_lock_position(bool enable) {
 	}
 }
 
+void encoder_epm_pll_band(bool epm) {
+	if (epm) {
+		encoder_set_bandwidth(nv_get_motor_params()->epm_pll_band);
+	}else {
+		encoder_set_bandwidth(nv_get_motor_params()->est_pll_band);
+	}
+}
+
 static __INLINE float _pll_over_comp(void) {
 	u8 dir = ENC_DIR_DOWN;
 #ifdef ENCODER_CC_INVERT

Applications/foc/motor/encoder.h

@@ -48,6 +48,7 @@ void encoder_detect_upload(void);
 void encoder_clear_cnt_offset(void);
 u32 encoder_get_cnt_offset(void);
 bool encoder_get_cali_error(void);
+void encoder_epm_pll_band(bool epm);
 
 #endif /* _Encoder_H__ */
 

Applications/foc/motor/motor.c

@@ -13,6 +13,7 @@
 #include "foc/motor/motor_param.h"
 #include "foc/core/foc_observer.h"
 #include "foc/core/thro_torque.h"
+#include "foc/core/F_Calc.h"
 #include "app/nv_storage.h"
 //#include "foc/core/torque.h"
 #include "foc/limit.h"
@@ -451,6 +452,7 @@ bool mc_start_epm(bool epm) {
 	}
 	u32 mask = cpu_enter_critical();
 	motor.b_epm = epm;
+	motor_encoder_band_epm(epm);
 	if (epm) {
 		eCtrl_set_TgtSpeed(0);
 		motor.mode = CTRL_MODE_SPD;
@@ -1129,7 +1131,7 @@ static bool mc_process_force_running(void) {
 static void mc_process_curise(void) {
 	static bool can_pause_resume = false;
 	if (motor.b_cruise) {
-		if (PMSM_FOC_GetSpeed() < CONFIG_MIN_CRUISE_RPM) {
+		if (PMSM_FOC_GetSpeed() < (CONFIG_MIN_CRUISE_RPM - 20)) {
 			mc_enable_cruise(false);
 			return;
 		}
@@ -1171,6 +1173,8 @@ void Sched_MC_mTask(void) {
 
 	adc_vref_filter();
 
+	F_all_Calc();
+
 #ifdef CONFIG_CRUISE_ENABLE_ACCL
 	mc_process_curise();
 #endif

Applications/foc/motor/motor.h

@@ -244,6 +244,16 @@ static __INLINE void motor_encoder_lock_pos(bool lock) {
 #endif
 }
 
+static __INLINE void motor_encoder_band_epm(bool epm) {
+#ifdef USE_ENCODER_HALL
+		
+#elif defined (USE_ENCODER_ABI)
+		encoder_epm_pll_band(epm);
+#else
+	#error "Postion sensor ERROR"
+#endif
+}
+
 
 #endif /* _MOTOR_H__ */
 

Project/MC105_V3.uvoptx

@@ -447,6 +447,18 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>2</GroupNumber>
+      <FileNumber>18</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\Applications\foc\core\F_Calc.c</PathWithFileName>
+      <FilenameWithoutPath>F_Calc.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
   <Group>
@@ -457,7 +469,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>18</FileNumber>
+      <FileNumber>19</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -469,7 +481,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>19</FileNumber>
+      <FileNumber>20</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -481,7 +493,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>20</FileNumber>
+      <FileNumber>21</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -493,7 +505,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>21</FileNumber>
+      <FileNumber>22</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -505,7 +517,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>22</FileNumber>
+      <FileNumber>23</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -525,7 +537,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>4</GroupNumber>
-      <FileNumber>23</FileNumber>
+      <FileNumber>24</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -537,7 +549,7 @@
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
-      <FileNumber>24</FileNumber>
+      <FileNumber>25</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -549,7 +561,7 @@
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
-      <FileNumber>25</FileNumber>
+      <FileNumber>26</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -561,7 +573,7 @@
     </File>
     <File>
       <GroupNumber>4</GroupNumber>
-      <FileNumber>26</FileNumber>
+      <FileNumber>27</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -581,7 +593,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>5</GroupNumber>
-      <FileNumber>27</FileNumber>
+      <FileNumber>28</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -593,7 +605,7 @@
     </File>
     <File>
       <GroupNumber>5</GroupNumber>
-      <FileNumber>28</FileNumber>
+      <FileNumber>29</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -605,7 +617,7 @@
     </File>
     <File>
       <GroupNumber>5</GroupNumber>
-      <FileNumber>29</FileNumber>
+      <FileNumber>30</FileNumber>
       <FileType>4</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -617,7 +629,7 @@
     </File>
     <File>
       <GroupNumber>5</GroupNumber>
-      <FileNumber>30</FileNumber>
+      <FileNumber>31</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -637,7 +649,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>31</FileNumber>
+      <FileNumber>32</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -649,7 +661,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>32</FileNumber>
+      <FileNumber>33</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -661,7 +673,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>33</FileNumber>
+      <FileNumber>34</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -673,7 +685,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>34</FileNumber>
+      <FileNumber>35</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -685,7 +697,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>35</FileNumber>
+      <FileNumber>36</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -697,7 +709,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>36</FileNumber>
+      <FileNumber>37</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -709,7 +721,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>37</FileNumber>
+      <FileNumber>38</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -721,7 +733,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>38</FileNumber>
+      <FileNumber>39</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -733,7 +745,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>39</FileNumber>
+      <FileNumber>40</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -745,7 +757,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>40</FileNumber>
+      <FileNumber>41</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -757,7 +769,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>41</FileNumber>
+      <FileNumber>42</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -769,7 +781,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>42</FileNumber>
+      <FileNumber>43</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -781,7 +793,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>43</FileNumber>
+      <FileNumber>44</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -793,7 +805,7 @@
     </File>
     <File>
       <GroupNumber>6</GroupNumber>
-      <FileNumber>44</FileNumber>
+      <FileNumber>45</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -813,7 +825,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>45</FileNumber>
+      <FileNumber>46</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -825,7 +837,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>46</FileNumber>
+      <FileNumber>47</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -837,7 +849,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>47</FileNumber>
+      <FileNumber>48</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -849,7 +861,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>48</FileNumber>
+      <FileNumber>49</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -861,7 +873,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>49</FileNumber>
+      <FileNumber>50</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -873,7 +885,7 @@
     </File>
     <File>
       <GroupNumber>7</GroupNumber>
-      <FileNumber>50</FileNumber>
+      <FileNumber>51</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -893,7 +905,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>51</FileNumber>
+      <FileNumber>52</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -905,7 +917,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>52</FileNumber>
+      <FileNumber>53</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -917,7 +929,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>53</FileNumber>
+      <FileNumber>54</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -937,7 +949,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>54</FileNumber>
+      <FileNumber>55</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -949,7 +961,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>55</FileNumber>
+      <FileNumber>56</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -961,7 +973,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>56</FileNumber>
+      <FileNumber>57</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -973,7 +985,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>57</FileNumber>
+      <FileNumber>58</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -985,7 +997,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>58</FileNumber>
+      <FileNumber>59</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -997,7 +1009,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>59</FileNumber>
+      <FileNumber>60</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1009,7 +1021,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>60</FileNumber>
+      <FileNumber>61</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1021,7 +1033,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>61</FileNumber>
+      <FileNumber>62</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1033,7 +1045,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>62</FileNumber>
+      <FileNumber>63</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1045,7 +1057,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>63</FileNumber>
+      <FileNumber>64</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1057,7 +1069,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>64</FileNumber>
+      <FileNumber>65</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1069,7 +1081,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>65</FileNumber>
+      <FileNumber>66</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1081,7 +1093,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>66</FileNumber>
+      <FileNumber>67</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1093,7 +1105,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>67</FileNumber>
+      <FileNumber>68</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1105,7 +1117,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>68</FileNumber>
+      <FileNumber>69</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1117,7 +1129,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>69</FileNumber>
+      <FileNumber>70</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1137,7 +1149,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>70</FileNumber>
+      <FileNumber>71</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1149,7 +1161,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>71</FileNumber>
+      <FileNumber>72</FileNumber>
       <FileType>2</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1161,7 +1173,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>72</FileNumber>
+      <FileNumber>73</FileNumber>
       <FileType>2</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>

Project/MC105_V3.uvprojx

@@ -473,6 +473,11 @@
               <FileType>1</FileType>
               <FilePath>..\Applications\foc\core\ladrc_observer.c</FilePath>
             </File>
+            <File>
+              <FileName>F_Calc.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Applications\foc\core\F_Calc.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>