加入foc的观测器融合encoder和smo，进行切换，目前实现了一部分

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

Applications/app/app.c

@@ -12,7 +12,7 @@
 #include "libs/time_measure.h"
 #include "app/nv_storage.h"
 #include "foc/commands.h"
-
+#include "bsp/adc.h"
 
 static u32 _app_low_task(void *args);
 static u32 _app_report_task(void *args);
@@ -103,6 +103,7 @@ static u32 _app_report_task(void *p) {
 		sys_debug("FOC time err %d %d\n", g_meas_foc.intval_time_h_error, g_meas_foc.intval_time_l_error);
 		sys_debug("acc vol %d, mos2 %d\n", get_acc_vol(), get_mos_temp2());
 		sys_debug("throttle %f\n", get_throttle_float());
+		sys_debug("ADC Vref %f, %f\n", get_adc_vref(), adc_5vref_compesion());
 		sys_debug("curr lfp %f\n", PMSM_FOC_Get()->params.n_PhaseFilterCeof);
 		//sys_debug("fan rpm %d, %d\n", mc_params()->fan[0].rpm, mc_params()->fan[1].rpm);
 		encoder_log();

Applications/foc/commands.c

@@ -433,8 +433,10 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		{
 			bool sensorless = decode_u8((u8 *)command->data)?true:false;
 			if (sensorless && mc_is_start() && PMSM_FOC_GetSpeed() >= CONFIG_SMO_MIN_SPEED) {
+				sys_debug("use smo %d\n", sensorless);
 				foc_observer_use_smo(sensorless);
 			}else {
+				sys_debug("unuse smo\n");
 				foc_observer_use_smo(false);
 			}
 			break;

Applications/foc/commands.h

@@ -47,6 +47,7 @@ typedef enum {
 	Foc_Report_Status,
 	Foc_Report_MTPA_DQ_Angle,
 	Foc_Report_Plot,
+	Foc_Report_Temp,
 	Foc_Cmd_Max = 0xE0
 }foc_cmd_t;
 #define CMD_2_CAN_KEY(cmd) ((u16)(((u16)cmd) | (CAN_MY_ADDRESS<<8)))

Applications/foc/core/foc_observer.c

@@ -7,23 +7,66 @@ static foc_observer_t foc_obser;
 void foc_observer_init(void) {
 	foc_obser.smo_enabled = false;
 	foc_obser.smo_used = false;
+	foc_obser.enc_angle = INVALID_ANGLE;
+	foc_obser.enc_est_angle = INVALID_ANGLE;
+	foc_obser.enc_speed = 0;
+	foc_obser.smo_angle = INVALID_ANGLE;
+	foc_obser.smo_est_angle = INVALID_ANGLE;
+	foc_obser.smo_speed = 0;
+	foc_obser.fusion_ceof = 1.0f;
 #ifdef CONFIG_SMO_OBSERVER
 	smo_observer_init(CONFIG_SMO_PLL_BANDWITH, CONFIG_SMO_LFP_WC, CONFIG_SMO_GAIN_K, CONFIG_SMO_SIGMOID_MAX);
 	foc_obser.smo_enabled = true;
 #endif
 }
+
+#define RPM_2_degree(rpm)  ((rpm) * 60.0f * nv_get_motor_params()->poles * FOC_CTRL_US)
+
 float foc_observer_update(float uAlp, float uBeta, float iAlp, float iBeta){
-	if (foc_obser.smo_enabled) {
-		foc_obser.smo_angle = smo_observer_update(uAlp, uBeta, iAlp, iBeta);
-		foc_obser.smo_speed = smo_observer_est_rpm();
-	}
+	float prev_enc_angle = foc_obser.enc_angle;
+	float prev_enc_speed = foc_obser.enc_speed;
 	foc_obser.enc_angle = motor_encoder_get_angle();
 	foc_obser.enc_speed = motor_encoder_get_speed();
-	
+
+	if (!foc_obser.smo_enabled) {
+		return foc_obser.enc_angle;
+	}
+	float est_enc_delta = RPM_2_degree(prev_enc_speed);
+	float real_enc_delta = foc_obser.enc_angle - prev_enc_angle;
+	if (real_enc_delta < 0) {
+		real_enc_delta += 360.0f;
+	}
+	float est_ration = real_enc_delta/est_enc_delta;
+	if (est_ration >= 1.5f || est_ration <= 0.5f) {
+		foc_obser.fusion_ceof -= 0.1f;
+		if (foc_obser.fusion_ceof < 0.0f) {
+			foc_obser.fusion_ceof = 0.0f;
+		}
+		if (foc_obser.enc_est_angle == INVALID_ANGLE) {
+			foc_obser.enc_est_angle = prev_enc_angle;
+		}else {
+			foc_obser.enc_est_angle += est_enc_delta;
+			rand_angle(foc_obser.enc_est_angle);
+		}
+	}else {
+		foc_obser.fusion_ceof += 0.1f;
+		if (foc_obser.fusion_ceof > 1.0f) {
+			foc_obser.fusion_ceof = 1.0f;
+		}
+		foc_obser.enc_est_angle = foc_obser.enc_angle;
+	}
+	foc_obser.smo_est_angle = foc_obser.smo_angle + RPM_2_degree(foc_obser.smo_speed);
+	foc_obser.smo_angle = smo_observer_update(uAlp, uBeta, iAlp, iBeta);
+	foc_obser.smo_speed = smo_observer_est_rpm();
+
 	if (foc_obser.smo_used) {
 		return foc_obser.smo_angle;
 	}
+#if 0
+	return (foc_obser.enc_est_angle * foc_obser.fusion_ceof + foc_obser.smo_angle * (1.0f - foc_obser.fusion_ceof));
+#else
 	return foc_obser.enc_angle;
+#endif
 }
 
 float foc_observer_speed(void) {

Applications/foc/core/foc_observer.h

@@ -13,6 +13,7 @@ typedef struct {
 	float smo_est_angle; //通过smo速度，估计当前角度
 	bool  smo_enabled;
 	bool  smo_used;
+	float fusion_ceof;  //融合系数
 }foc_observer_t;
 void foc_observer_init(void);
 float foc_observer_update(float uAlp, float uBeta, float iAlp, float iBeta);

Applications/foc/core/smo_observer.c

@@ -144,3 +144,4 @@ float smo_observer_est_angle(void) {
 float smo_observer_est_rpm(void) {
 	return smo.est_rpm;
 }
+

Applications/foc/core/torque.c

@@ -93,9 +93,15 @@ void request_torque(float thro_ration) {
 	
 	if (!mc_throttle_released()) {
 		if (curr_rpm <= CONFIG_ZERO_SPEED_RPM) {
+			g_trq_mn.torque_req = eCtrl_get_FinalTorque() * FINAL_DQ_CEFO;
 			ref_trq  = MAX(eCtrl_get_FinalTorque() * FINAL_DQ_CEFO, ref_trq );
 		}
-		PMSM_FOC_Set_Torque(ref_trq );
+		if (ref_trq > g_trq_mn.torque_req) { //加扭矩step给定
+			step_towards(&g_trq_mn.torque_req, ref_trq, 1.0f);
+		}else { //减扭矩，直接给定
+			g_trq_mn.torque_req = ref_trq;
+		}
+		PMSM_FOC_Set_Torque(g_trq_mn.torque_req);
 	}else if (mc_throttle_released() && eCtrl_get_FinalTorque() != 0){
 		float real_trq = PMSM_FOC_Get_Real_Torque() * REAL_DQ_CEOF;
 		float ref_now = min(real_trq, eCtrl_get_RefTorque());
@@ -114,6 +120,7 @@ void request_speed(float thro_ration) {
 void throttle_process(u8 run_mode, float f_throttle) {
 	if (mc_throttle_released()){
 		g_trq_mn.throttle_value = 0;
+		g_trq_mn.torque_req = 0;
 	}else {
 		LowPass_Filter(g_trq_mn.throttle_value, f_throttle, THRO_REF_LP_CEOF);
 	}

Applications/foc/core/torque.h

@@ -4,6 +4,7 @@
 #include "foc/core/PMSM_FOC_Core.h"
 typedef struct {
 	bool  accl;
+	float torque_req;
 	float throttle_value;
 	float thro_ration; //油门开度百分比
 	float spd_filted;

Applications/foc/motor/motor_param.c

@@ -3,7 +3,7 @@
 #include "math/fast_math.h"
 
 #if CONFIG_MOT_TYPE==MOTOR_BLUESHARK_ZD_100
-static motor_map_t mot_map[] = {
+static rpm_trq_map_t mot_map[] = {
 	{4500, 200},
 	{4740, 170},
 	{5050, 125},

Applications/foc/motor/motor_param.h

@@ -7,7 +7,7 @@
 typedef struct {
 	s16 rpm;
 	s16 torque;
-}motor_map_t;
+}rpm_trq_map_t;
 
 s16 get_max_torque_for_rpm(s16 rpm);
 
@@ -18,6 +18,8 @@ s16 get_max_torque_for_rpm(s16 rpm);
 #define MOTOR_POLES  4
 #define MOTOR_ENC_OFFSET 0.0F
 
+#define CONFIG_MAX_MOTOR_CURR 400.0F
+
 #define TRQ_PI_KP 0.14F
 #define TRQ_PI_KI 0.15F
 
@@ -28,6 +30,9 @@ s16 get_max_torque_for_rpm(s16 rpm);
 #define MOTOR_Lq (0.000320f*0.5f)
 #define MOTOR_POLES  4
 #define MOTOR_ENC_OFFSET 250.0F
+
+#define CONFIG_MAX_MOTOR_CURR 400.0F
+
 #define TRQ_PI_KP 0.14F
 #define TRQ_PI_KI 0.15F
 
@@ -39,6 +44,8 @@ s16 get_max_torque_for_rpm(s16 rpm);
 #define MOTOR_Lq (0.000091f*0.5f)
 #define MOTOR_POLES  5
 #define MOTOR_ENC_OFFSET 145.0F
+#define CONFIG_MAX_MOTOR_CURR 300.0F
+
 #define TRQ_PI_KP 0.6F //0.13f
 #define TRQ_PI_KI 0.5F
 #define MOTOR_NR 0x13
@@ -49,6 +56,8 @@ s16 get_max_torque_for_rpm(s16 rpm);
 #define MOTOR_Lq (0.000205f*0.5f)
 #define MOTOR_POLES  5
 #define MOTOR_ENC_OFFSET 180.0F
+#define CONFIG_MAX_MOTOR_CURR 200.0F
+
 #define TRQ_PI_KP 0.14F
 #define TRQ_PI_KI 0.15F
 
@@ -60,6 +69,8 @@ s16 get_max_torque_for_rpm(s16 rpm);
 #define MOTOR_Ld 0.000032f
 #define MOTOR_Lq 0.000032f
 #define MOTOR_POLES  10
+#define CONFIG_MAX_MOTOR_CURR 20.0F
+
 #define TRQ_PI_KP 0.14F
 #define TRQ_PI_KI 0.15F
 

Applications/foc/ntc.c

@@ -13,8 +13,8 @@ static const u16 kty_table[] = {
 #define NTC_TEMP_INTVAL 10
 #define NTC_TEMP_OFFSET 1
 static const u16 ntc_table[] = {
-	43117, 27493, 18015, 12100, 8300, 5825, 4160, 3023,  2232, 1672, 1270, 977, 599,  
-	476, 382, 310, 254, 210,  174,  146, 123, 105, 90, 77, 66, 58, 50, 44};
+	43117, 27493, 18015, 12100, 8300, 5825, 4160, 3023,  2232, 1672, 1270, 977, 761, 599,  
+	476, 382, 310, 254, 210, 174, 146, 123, 105, 90, 77, 66, 58, 50, 44};
 
 s16 ntc_get_mos_temp(u16 r) {
 	int i = 0;

Applications/foc/samples.c

@@ -19,6 +19,7 @@ typedef struct {
 
 static u32 sample_task(void *);
 static samples_t _vbus;
+static samples_t _vref;
 #ifdef THROTTLE_CHAN
 static samples_t _throttle;
 #endif
@@ -31,6 +32,7 @@ static samples_t motor_temp;
 #ifdef MOS_TEMP_ADC_CHAN
 static samples_t mos_temp;
 #endif
+#undef MOS_TEMP1_ADC_CHAN
 #ifdef MOS_TEMP1_ADC_CHAN
 static samples_t mos_temp1;
 #endif
@@ -41,6 +43,10 @@ static samples_t acc_vol;
 static samples_t _ibus;
 #endif
 void samples_init(void){
+	_vref.filted_value = 0;
+	_vref.value = 0;
+	_vref.lowpass = 0.01f;
+	sample_vref();
 	_vbus.filted_value = (CONFIG_RATED_DC_VOL);
 	_vbus.value = (CONFIG_RATED_DC_VOL);
 	_vbus.lowpass = (0.01f);
@@ -115,7 +121,7 @@ int get_acc_vol(void) {
 float get_vbus_current(void) {
 #ifdef VBUS_I_CHAN
 	s16 ibus = _ibus.filted_value * 10.0f;
-	return -(float)ibus/10.0f;
+	return (float)ibus/10.0f;
 #else
 	return 0;
 #endif
@@ -143,6 +149,11 @@ float get_throttle_float(void) {
 	return 0.0f;
 #endif
 }
+
+float get_adc_vref(void) {
+	return _vref.filted_value;
+}
+
 static u32 sample_task(void *param) {
 	sample_vbus();
 	sample_ibus();
@@ -150,9 +161,16 @@ static u32 sample_task(void *param) {
 	sample_uvw_phase();
 	sample_motor_temp();
 	sample_mos_temp();
+	sample_vref();
 	return 0;
 }
 
+void sample_vref(void) {
+	float vadc = adc_get_vref();
+	_vref.value = ADC_REFERENCE_VOLTAGE * vadc / ADC_FULL_MAX;
+	LowPass_Filter(_vref.filted_value, _vref.value, _vref.lowpass);
+}
+
 void sample_vbus(void){
 	u32 ticks = task_ticks_abs();
 	s16 vadc = adc_get_vbus();
@@ -169,7 +187,7 @@ void sample_vbus(void){
 
 void sample_ibus(void) {
 #ifdef VBUS_I_CHAN
-	s16 vadc = adc_get_ibus() - _ibus.adc_offset;
+	s16 vadc = _ibus.adc_offset - adc_get_ibus();
 	_ibus.value = (float)vadc * VBUS_I_CEOF;
 	LowPass_Filter(_ibus.filted_value, _ibus.value, _ibus.lowpass);
 #endif

Applications/foc/samples.h

@@ -16,12 +16,14 @@ void sample_ibus(void);
 void sample_throttle(void);
 void sample_motor_temp(void);
 void sample_mos_temp(void);
+void sample_vref(void);
 s16 get_motor_temp(void);
 s16 get_mos_temp(void);
 int get_acc_vol(void);
 s16 get_mos_temp2(void);
 float get_vbus_current(void);
 void sample_ibus_offset(u16 offset);
+float get_adc_vref(void);
 
 #endif /* _SAMPLES_H__ */
 

Applications/prot/can_foc_msg.c

@@ -85,7 +85,7 @@ void can_report_foc_status(u8 can) {
 	u32 errMask = mc_get_critical_error();
 	encode_u32(data + 3, errMask);
 	encode_s16(data + 7, get_motor_temp());
-	encode_s16(data + 9, get_mos_temp());
+	encode_s16(data + 9, MAX(get_mos_temp(), get_mos_temp2()));
 	encode_u32(data + 11, shark_get_seconds());
 	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
 }