FW 3.37: FOC KI temp comp, configurable foc current filter

CHANGELOG

@@ -1,5 +1,10 @@
+=== FW 3.37 ===
+* Temperature compensation on KI in addition to the observer resistance.
+* Configurable FOC current filter (useful for slow abs max current setting).
+
 === FW 3.36 ===
 * Added handbrake current commands to the simple CAN interface.
+* Added D-term filter to position and speed controllers.
 
 === FW 3.35 ===
 * Added option to disable nRF transmission (option in Transmit Power parameter).

build_all/rebuild_all

@@ -179,3 +179,41 @@ cd $FWPATH
 make clean
 cd $DIR
 
+#################### HW 75_300 ########################
+
+COPYDIR=75_300
+rm $COPYDIR/*
+
+# default
+cd $FWPATH
+touch conf_general.h
+make -j8 build_args="-DHW_VERSION_75_300"
+cd $DIR
+cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_default.bin
+
+# default with HW limits disables
+cd $FWPATH
+touch conf_general.h
+make -j8 build_args="-DHW_VERSION_75_300 -DDISABLE_HW_LIMITS"
+cd $DIR
+cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_default_no_hw_limits.bin
+
+# ws2811
+cd $FWPATH
+touch conf_general.h
+make -j8 build_args="-DWS2811_ENABLE=1 -DHW_VERSION_75_300"
+cd $DIR
+cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_ws2811.bin
+
+# servoout
+cd $FWPATH
+touch conf_general.h
+make -j8 build_args="-DSERVO_OUT_ENABLE=1 -DHW_VERSION_75_300"
+cd $DIR
+cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_servoout.bin
+
+# Clean
+cd $FWPATH
+make clean
+cd $DIR
+

commands.c

@@ -327,16 +327,19 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
 		mcconf.foc_sat_comp = buffer_get_float32_auto(data, &ind);
 		mcconf.foc_temp_comp = data[ind++];
 		mcconf.foc_temp_comp_base_temp = buffer_get_float32_auto(data, &ind);
+		mcconf.foc_current_filter_const = buffer_get_float32_auto(data, &ind);
 
 		mcconf.s_pid_kp = buffer_get_float32_auto(data, &ind);
 		mcconf.s_pid_ki = buffer_get_float32_auto(data, &ind);
 		mcconf.s_pid_kd = buffer_get_float32_auto(data, &ind);
+		mcconf.s_pid_kd_filter = buffer_get_float32_auto(data, &ind);
 		mcconf.s_pid_min_erpm = buffer_get_float32_auto(data, &ind);
 		mcconf.s_pid_allow_braking = data[ind++];
 
 		mcconf.p_pid_kp = buffer_get_float32_auto(data, &ind);
 		mcconf.p_pid_ki = buffer_get_float32_auto(data, &ind);
 		mcconf.p_pid_kd = buffer_get_float32_auto(data, &ind);
+		mcconf.p_pid_kd_filter = buffer_get_float32_auto(data, &ind);
 		mcconf.p_pid_ang_div = buffer_get_float32_auto(data, &ind);
 
 		mcconf.cc_startup_boost_duty = buffer_get_float32_auto(data, &ind);
@@ -482,16 +485,19 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
 		buffer_append_float32_auto(send_buffer, mcconf.foc_sat_comp, &ind);
 		send_buffer[ind++] = mcconf.foc_temp_comp;
 		buffer_append_float32_auto(send_buffer, mcconf.foc_temp_comp_base_temp, &ind);
+		buffer_append_float32_auto(send_buffer, mcconf.foc_current_filter_const, &ind);
 
 		buffer_append_float32_auto(send_buffer, mcconf.s_pid_kp, &ind);
 		buffer_append_float32_auto(send_buffer, mcconf.s_pid_ki, &ind);
 		buffer_append_float32_auto(send_buffer, mcconf.s_pid_kd, &ind);
+		buffer_append_float32_auto(send_buffer, mcconf.s_pid_kd_filter, &ind);
 		buffer_append_float32_auto(send_buffer, mcconf.s_pid_min_erpm, &ind);
 		send_buffer[ind++] = mcconf.s_pid_allow_braking;
 
 		buffer_append_float32_auto(send_buffer, mcconf.p_pid_kp, &ind);
 		buffer_append_float32_auto(send_buffer, mcconf.p_pid_ki, &ind);
 		buffer_append_float32_auto(send_buffer, mcconf.p_pid_kd, &ind);
+		buffer_append_float32_auto(send_buffer, mcconf.p_pid_kd_filter, &ind);
 		buffer_append_float32_auto(send_buffer, mcconf.p_pid_ang_div, &ind);
 
 		buffer_append_float32_auto(send_buffer, mcconf.cc_startup_boost_duty, &ind);

conf_general.c

@@ -256,16 +256,19 @@ void conf_general_get_default_mc_configuration(mc_configuration *conf) {
 	conf->foc_sat_comp = MCCONF_FOC_SAT_COMP;
 	conf->foc_temp_comp = MCCONF_FOC_TEMP_COMP;
 	conf->foc_temp_comp_base_temp = MCCONF_FOC_TEMP_COMP_BASE_TEMP;
+	conf->foc_current_filter_const = MCCONF_FOC_CURRENT_FILTER_CONST;
 
 	conf->s_pid_kp = MCCONF_S_PID_KP;
 	conf->s_pid_ki = MCCONF_S_PID_KI;
 	conf->s_pid_kd = MCCONF_S_PID_KD;
+	conf->s_pid_kd_filter = MCCONF_S_PID_KD_FILTER;
 	conf->s_pid_min_erpm = MCCONF_S_PID_MIN_RPM;
 	conf->s_pid_allow_braking = MCCONF_S_PID_ALLOW_BRAKING;
 
 	conf->p_pid_kp = MCCONF_P_PID_KP;
 	conf->p_pid_ki = MCCONF_P_PID_KI;
 	conf->p_pid_kd = MCCONF_P_PID_KD;
+	conf->p_pid_kd_filter = MCCONF_P_PID_KD_FILTER;
 	conf->p_pid_ang_div = MCCONF_P_PID_ANG_DIV;
 
 	conf->cc_startup_boost_duty = MCCONF_CC_STARTUP_BOOST_DUTY;

conf_general.h

@@ -22,7 +22,7 @@
 
 // Firmware version
 #define FW_VERSION_MAJOR		3
-#define FW_VERSION_MINOR		35
+#define FW_VERSION_MINOR		37
 
 #include "datatypes.h"
 
@@ -51,7 +51,7 @@
 	!defined(HW_VERSION_48) && !defined(HW_VERSION_49) && !defined(HW_VERSION_410) && \
 	!defined(HW_VERSION_60) && !defined(HW_VERSION_R2) && !defined(HW_VERSION_VICTOR_R1A) && \
 	!defined(HW_VERSION_DAS_RS) && !defined(HW_VERSION_PALTA) && !defined(HW_VERSION_RH) && \
-	!defined(HW_VERSION_TP)
+	!defined(HW_VERSION_TP) && !defined(HW_VERSION_75_300)
 //#define HW_VERSION_40
 //#define HW_VERSION_45
 //#define HW_VERSION_46 // Also for 4.7
@@ -65,6 +65,7 @@
 //#define HW_VERSION_PALTA
 //#define HW_VERSION_RH
 //#define HW_VERSION_TP
+//#define HW_VERSION_75_300
 #endif
 
 /*

datatypes.h

@@ -212,16 +212,19 @@ typedef struct {
 	float foc_sat_comp;
 	bool foc_temp_comp;
 	float foc_temp_comp_base_temp;
+	float foc_current_filter_const;
 	// Speed PID
 	float s_pid_kp;
 	float s_pid_ki;
 	float s_pid_kd;
+	float s_pid_kd_filter;
 	float s_pid_min_erpm;
 	bool s_pid_allow_braking;
 	// Pos PID
 	float p_pid_kp;
 	float p_pid_ki;
 	float p_pid_kd;
+	float p_pid_kd_filter;
 	float p_pid_ang_div;
 	// Current controller
 	float cc_startup_boost_duty;

hwconf/hw.h

@@ -54,6 +54,8 @@
 #include "hw_rh.h"
 #elif defined HW_VERSION_TP
 #include "hw_tp.h"
+#elif defined HW_VERSION_75_300
+#include "hw_75_300.h"
 #else
 #error "No hardware version defined"
 #endif

hwconf/hwconf.mk

@@ -12,6 +12,7 @@ HWSRC =	hwconf/hw_40.c \
 	hwconf/drv8305.c \
 	hwconf/hw_palta.c \
 	hwconf/hw_rh.c \
-	hwconf/hw_tp.c
+	hwconf/hw_tp.c \
+	hwconf/hw_75_300.c
 
 HWINC = hwconf

mcconf/mcconf_default.h

@@ -118,6 +118,9 @@
 #ifndef MCCONF_S_PID_KD
 #define MCCONF_S_PID_KD					0.0001	// Derivative gain
 #endif
+#ifndef MCCONF_S_PID_KD_FILTER
+#define MCCONF_S_PID_KD_FILTER			0.2	// Derivative filter
+#endif
 #ifndef MCCONF_S_PID_MIN_RPM
 #define MCCONF_S_PID_MIN_RPM			900.0	// Minimum allowed RPM
 #endif
@@ -135,6 +138,9 @@
 #ifndef MCCONF_P_PID_KD
 #define MCCONF_P_PID_KD					0.0004	// Derivative gain
 #endif
+#ifndef MCCONF_P_PID_KD_FILTER
+#define MCCONF_P_PID_KD_FILTER			0.2		// Derivative filter
+#endif
 #ifndef MCCONF_P_PID_ANG_DIV
 #define MCCONF_P_PID_ANG_DIV			1.0		// Divide angle by this value
 #endif
@@ -314,6 +320,9 @@
 #ifndef MCCONF_FOC_TEMP_COMP_BASE_TEMP
 #define MCCONF_FOC_TEMP_COMP_BASE_TEMP	25.0	// Motor temperature compensation base temperature
 #endif
+#ifndef MCCONF_FOC_CURRENT_FILTER_CONST
+#define MCCONF_FOC_CURRENT_FILTER_CONST	0.1		// Filter constant for the filtered currents
+#endif
 
 // Misc
 #ifndef MCCONF_M_FAULT_STOP_TIME

mcpwm.c

@@ -1151,17 +1151,17 @@ static void run_pid_control_speed(void) {
 	i_term += error * (conf->s_pid_ki * MCPWM_PID_TIME_K) * (1.0 / 20.0);
 	d_term = (error - prev_error) * (conf->s_pid_kd / MCPWM_PID_TIME_K) * (1.0 / 20.0);
 
+	// Filter D
+	static float d_filter = 0.0;
+	UTILS_LP_FAST(d_filter, d_term, conf->p_pid_kd_filter);
+	d_term = d_filter;
+
 	// I-term wind-up protection
 	utils_truncate_number(&i_term, -1.0, 1.0);
 
 	// Store previous error
 	prev_error = error;
 
-	// Some d_term filtering
-	static float d_filtered = 0.0;
-	UTILS_LP_FAST(d_filtered, d_term, 0.1);
-	d_term = d_filtered;
-
 	// Calculate output
 	float output = p_term + i_term + d_term;
 	utils_truncate_number(&output, -1.0, 1.0);
@@ -1191,6 +1191,11 @@ static void run_pid_control_speed(void) {
 	i_term += error * (conf->s_pid_ki * MCPWM_PID_TIME_K) * scale;
 	d_term = (error - prev_error) * (conf->s_pid_kd / MCPWM_PID_TIME_K) * scale;
 
+	// Filter D
+	static float d_filter = 0.0;
+	UTILS_LP_FAST(d_filter, d_term, conf->s_pid_kd_filter);
+	d_term = d_filter;
+
 	// I-term wind-up protection
 	utils_truncate_number(&i_term, -1.0, 1.0);
 
@@ -1239,6 +1244,11 @@ static void run_pid_control_pos(float dt) {
 	i_term += error * (conf->p_pid_ki * dt);
 	d_term = (error - prev_error) * (conf->p_pid_kd / dt);
 
+	// Filter D
+	static float d_filter = 0.0;
+	UTILS_LP_FAST(d_filter, d_term, conf->p_pid_kd_filter);
+	d_term = d_filter;
+
 	// I-term wind-up protection
 	utils_truncate_number(&i_term, -1.0, 1.0);
 

mcpwm_foc.c

@@ -2193,16 +2193,24 @@ static void control_current(volatile motor_state_t *state_m, float dt) {
 
 	state_m->id = c * state_m->i_alpha + s * state_m->i_beta;
 	state_m->iq = c * state_m->i_beta  - s * state_m->i_alpha;
-	UTILS_LP_FAST(state_m->id_filter, state_m->id, MCPWM_FOC_I_FILTER_CONST);
-	UTILS_LP_FAST(state_m->iq_filter, state_m->iq, MCPWM_FOC_I_FILTER_CONST);
+	UTILS_LP_FAST(state_m->id_filter, state_m->id, m_conf->foc_current_filter_const);
+	UTILS_LP_FAST(state_m->iq_filter, state_m->iq, m_conf->foc_current_filter_const);
 
 	float Ierr_d = state_m->id_target - state_m->id;
 	float Ierr_q = state_m->iq_target - state_m->iq;
 
 	state_m->vd = state_m->vd_int + Ierr_d * m_conf->foc_current_kp;
 	state_m->vq = state_m->vq_int + Ierr_q * m_conf->foc_current_kp;
-	state_m->vd_int += Ierr_d * (m_conf->foc_current_ki * dt);
-	state_m->vq_int += Ierr_q * (m_conf->foc_current_ki * dt);
+
+	// Temperature compensation
+	const float t = mc_interface_temp_motor_filtered();
+	float ki = m_conf->foc_current_ki;
+	if (m_conf->foc_temp_comp && t > -5.0) {
+		ki += ki * 0.00386 * (t - m_conf->foc_temp_comp_base_temp);
+	}
+
+	state_m->vd_int += Ierr_d * (ki * dt);
+	state_m->vq_int += Ierr_q * (ki * dt);
 
 	// Saturation
 	utils_saturate_vector_2d((float*)&state_m->vd, (float*)&state_m->vq,
@@ -2425,6 +2433,12 @@ static void run_pid_control_pos(float angle_now, float angle_set, float dt) {
 		dt_int = 0.0;
 	}
 
+	// Filter D
+	static float d_filter = 0.0;
+	UTILS_LP_FAST(d_filter, d_term, m_conf->p_pid_kd_filter);
+	d_term = d_filter;
+
+
 	// I-term wind-up protection
 	utils_truncate_number_abs(&p_term, 1.0);
 	utils_truncate_number_abs(&i_term, 1.0 - fabsf(p_term));
@@ -2476,6 +2490,11 @@ static void run_pid_control_speed(float dt) {
 	i_term += error * (m_conf->s_pid_ki * dt) * (1.0 / 20.0);
 	d_term = (error - prev_error) * (m_conf->s_pid_kd / dt) * (1.0 / 20.0);
 
+	// Filter D
+	static float d_filter = 0.0;
+	UTILS_LP_FAST(d_filter, d_term, m_conf->s_pid_kd_filter);
+	d_term = d_filter;
+
 	// I-term wind-up protection
 	utils_truncate_number(&i_term, -1.0, 1.0);
 

mcpwm_foc.h

@@ -80,7 +80,6 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags);
 // Defines
 #define MCPWM_FOC_INDUCTANCE_SAMPLE_CNT_OFFSET		10 // Offset for the inductance measurement sample time in timer ticks
 #define MCPWM_FOC_INDUCTANCE_SAMPLE_RISE_COMP		50 // Current rise time compensation
-#define MCPWM_FOC_I_FILTER_CONST					0.1 // Filter constant for the current filters
 #define MCPWM_FOC_CURRENT_SAMP_OFFSET				(2) // Offset from timer top for injected ADC samples
 
 #endif /* MCPWM_FOC_H_ */

mcuconf.h

@@ -209,7 +209,8 @@
  */
 #define STM32_ICU_USE_TIM1                  FALSE
 #define STM32_ICU_USE_TIM2                  FALSE
-#if defined(HW_VERSION_60) || defined(HW_VERSION_DAS_RS) || defined(HW_VERSION_PALTA) || defined(HW_VERSION_RH)
+#if defined(HW_VERSION_60) || defined(HW_VERSION_DAS_RS) || defined(HW_VERSION_PALTA) || \
+	defined(HW_VERSION_RH) || defined(HW_VERSION_75_300)
 #define STM32_ICU_USE_TIM3                  FALSE
 #define STM32_ICU_USE_TIM4                  TRUE
 #else