Merge pull request #179 from powerdesigns/MTPA

MTPA support

confgenerator.c

@@ -72,6 +72,7 @@ int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *
 	buffer_append_float32_auto(buffer, conf->foc_pll_kp, &ind);
 	buffer_append_float32_auto(buffer, conf->foc_pll_ki, &ind);
 	buffer_append_float32_auto(buffer, conf->foc_motor_l, &ind);
+	buffer_append_float32_auto(buffer, conf->foc_motor_ld_lq_diff, &ind);
 	buffer_append_float32_auto(buffer, conf->foc_motor_r, &ind);
 	buffer_append_float32_auto(buffer, conf->foc_motor_flux_linkage, &ind);
 	buffer_append_float32_auto(buffer, conf->foc_observer_gain, &ind);
@@ -365,6 +366,7 @@ bool confgenerator_deserialize_mcconf(const uint8_t *buffer, mc_configuration *c
 	conf->foc_pll_kp = buffer_get_float32_auto(buffer, &ind);
 	conf->foc_pll_ki = buffer_get_float32_auto(buffer, &ind);
 	conf->foc_motor_l = buffer_get_float32_auto(buffer, &ind);
+	conf->foc_motor_ld_lq_diff = buffer_get_float32_auto(buffer, &ind);
 	conf->foc_motor_r = buffer_get_float32_auto(buffer, &ind);
 	conf->foc_motor_flux_linkage = buffer_get_float32_auto(buffer, &ind);
 	conf->foc_observer_gain = buffer_get_float32_auto(buffer, &ind);
@@ -654,6 +656,7 @@ void confgenerator_set_defaults_mcconf(mc_configuration *conf) {
 	conf->foc_pll_kp = MCCONF_FOC_PLL_KP;
 	conf->foc_pll_ki = MCCONF_FOC_PLL_KI;
 	conf->foc_motor_l = MCCONF_FOC_MOTOR_L;
+	conf->foc_motor_ld_lq_diff = MCCONF_FOC_MOTOR_LD_LQ_DIFF;
 	conf->foc_motor_r = MCCONF_FOC_MOTOR_R;
 	conf->foc_motor_flux_linkage = MCCONF_FOC_MOTOR_FLUX_LINKAGE;
 	conf->foc_observer_gain = MCCONF_FOC_OBSERVER_GAIN;

confgenerator.h

@@ -8,8 +8,8 @@
 #include <stdbool.h>
 
 // Constants
-#define MCCONF_SIGNATURE		1775793947
-#define APPCONF_SIGNATURE		2662993821
+#define MCCONF_SIGNATURE		2013508732
+#define APPCONF_SIGNATURE		2460147246
 
 // Functions
 int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf);

datatypes.h

@@ -277,6 +277,7 @@ typedef struct {
 	float foc_encoder_cos_gain;
 	float foc_encoder_sincos_filter_constant;
 	float foc_motor_l;
+	float foc_motor_ld_lq_diff;
 	float foc_motor_r;
 	float foc_motor_flux_linkage;
 	float foc_observer_gain;

mc_interface.c

@@ -1211,7 +1211,7 @@ float mc_interface_read_reset_avg_id(void) {
 	float res = motor_now()->m_motor_id_sum / motor_now()->m_motor_id_iterations;
 	motor_now()->m_motor_id_sum = 0.0;
 	motor_now()->m_motor_id_iterations = 0.0;
-	return DIR_MULT * res; // TODO: DIR_MULT?
+	return res;
 }
 
 /**
@@ -1237,7 +1237,7 @@ float mc_interface_read_reset_avg_vd(void) {
 	float res = motor_now()->m_motor_vd_sum / motor_now()->m_motor_vd_iterations;
 	motor_now()->m_motor_vd_sum = 0.0;
 	motor_now()->m_motor_vd_iterations = 0.0;
-	return DIR_MULT * res;
+	return res;
 }
 
 /**

mcconf/mcconf_default.h

@@ -263,6 +263,9 @@
 #ifndef MCCONF_FOC_MOTOR_FLUX_LINKAGE
 #define MCCONF_FOC_MOTOR_FLUX_LINKAGE	0.00245
 #endif
+#ifndef MCCONF_FOC_MOTOR_LD_LQ_DIFF
+#define MCCONF_FOC_MOTOR_LD_LQ_DIFF		0.0
+#endif
 #ifndef MCCONF_FOC_OBSERVER_GAIN
 #define MCCONF_FOC_OBSERVER_GAIN		9e7		// Can be something like 600 / L
 #endif

mcpwm_foc.c

@@ -195,6 +195,7 @@ static void terminal_plot_hfi(int argc, const char **argv);
 static void timer_update(volatile motor_all_state_t *motor, float dt);
 static void input_current_offset_measurement( void );
 static void hfi_update(volatile motor_all_state_t *motor);
+static void apply_mtpa(float *id, float *iq, volatile motor_all_state_t *motor);
 
 // Threads
 static THD_WORKING_AREA(timer_thread_wa, 1024);
@@ -1093,10 +1094,12 @@ float mcpwm_foc_get_tot_current_motor(bool is_second_motor) {
 float mcpwm_foc_get_tot_current_filtered_motor(bool is_second_motor) {
 #ifdef HW_HAS_DUAL_MOTORS
 	volatile motor_all_state_t *motor = is_second_motor ? &m_motor_2 : &m_motor_1;
-	return SIGN(motor->m_motor_state.vq) * motor->m_motor_state.iq_filter;
+	return SIGN(motor->m_motor_state.vq) *
+			sqrtf( SQ(motor->m_motor_state.iq_filter) + SQ(motor->m_motor_state.id_filter) );
 #else
 	(void)is_second_motor;
-	return SIGN(m_motor_1.m_motor_state.vq) * m_motor_1.m_motor_state.iq_filter;
+	return SIGN(m_motor_1.m_motor_state.vq) *
+			sqrtf( SQ(m_motor_1.m_motor_state.iq_filter) + SQ(m_motor_1.m_motor_state.id_filter) );
 #endif
 }
 
@@ -1462,10 +1465,12 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r
 	float offset_old = motor->m_conf->foc_encoder_offset;
 	float inverted_old = motor->m_conf->foc_encoder_inverted;
 	float ratio_old = motor->m_conf->foc_encoder_ratio;
+	float ldiff_old = motor->m_conf->foc_motor_ld_lq_diff;
 
 	motor->m_conf->foc_encoder_offset = 0.0;
 	motor->m_conf->foc_encoder_inverted = false;
 	motor->m_conf->foc_encoder_ratio = 1.0;
+	motor->m_conf->foc_motor_ld_lq_diff = 0.0;
 
 	// Find index
 	int cnt = 0;
@@ -1651,6 +1656,7 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r
 	motor->m_conf->foc_encoder_inverted = inverted_old;
 	motor->m_conf->foc_encoder_offset = offset_old;
 	motor->m_conf->foc_encoder_ratio = ratio_old;
+	motor->m_conf->foc_motor_ld_lq_diff = ldiff_old;
 
 	// Enable timeout
 	timeout_configure(tout, tout_c);
@@ -2541,6 +2547,8 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
 			motor_now->m_motor_state.phase = motor_now->m_phase_now_override;
 		}
 
+		apply_mtpa(&id_set_tmp, &iq_set_tmp, motor_now);
+
 		// Apply current limits
 		// TODO: Consider D axis current for the input current as well.
 		const float mod_q = motor_now->m_motor_state.mod_q;
@@ -3981,3 +3989,14 @@ static void terminal_plot_hfi(int argc, const char **argv) {
 		commands_printf("This command requires one argument.\n");
 	}
 }
+
+static void apply_mtpa(float *id, float *iq, volatile motor_all_state_t *motor) {
+	float ld_lq_diff = motor->m_conf->foc_motor_ld_lq_diff;
+
+	if(ld_lq_diff != 0.0){
+		float lambda = motor->m_conf->foc_motor_flux_linkage;
+
+		*id = (lambda - sqrtf(SQ(lambda) + 8.0 * SQ(ld_lq_diff) * SQ(*iq))) / (4.0 * ld_lq_diff);
+		*iq = SIGN(*iq) * sqrtf(SQ(*iq) - SQ(*id));
+	}
+}