Lowpass filter input voltage and battery level

Author: vedderb

CHANGELOG

@@ -22,6 +22,7 @@
 * Added custom UI support.
 * Limit hall sensor angle rate of change based on ERPM.
 * Added p_pid_gain_dec_angle parameter.
+* Low pass filter input voltage.
 
 === FW 5.02 ===
 * IMU calibration improvement.

comm_can.c

@@ -1705,7 +1705,7 @@ static void send_status5(uint8_t id, bool replace) {
 	int32_t send_index = 0;
 	uint8_t buffer[8];
 	buffer_append_int32(buffer, mc_interface_get_tachometer_value(false), &send_index);
-	buffer_append_int16(buffer, (int16_t)(GET_INPUT_VOLTAGE() * 1e1), &send_index);
+	buffer_append_int16(buffer, (int16_t)(mc_interface_get_input_voltage_filtered() * 1e1), &send_index);
 	buffer_append_int16(buffer, 0, &send_index); // Reserved for now
 	comm_can_transmit_eid_replace(id | ((uint32_t)CAN_PACKET_STATUS_5 << 8),
 			buffer, send_index, replace);

commands.c

@@ -376,7 +376,7 @@ void commands_process_packet(unsigned char *data, unsigned int len,
 			buffer_append_float32(send_buffer, mc_interface_get_rpm(), 1e0, &ind);
 		}
 		if (mask & ((uint32_t)1 << 8)) {
-			buffer_append_float16(send_buffer, GET_INPUT_VOLTAGE(), 1e1, &ind);
+			buffer_append_float16(send_buffer, mc_interface_get_input_voltage_filtered(), 1e1, &ind);
 		}
 		if (mask & ((uint32_t)1 << 9)) {
 			buffer_append_float32(send_buffer, mc_interface_get_amp_hours(false), 1e4, &ind);
@@ -823,7 +823,7 @@ void commands_process_packet(unsigned char *data, unsigned int len,
 			buffer_append_float32(send_buffer, mc_interface_get_speed(), 1e3, &ind);
 		}
 		if (mask & ((uint32_t)1 << 7)) {
-			buffer_append_float16(send_buffer, GET_INPUT_VOLTAGE(), 1e1, &ind);
+			buffer_append_float16(send_buffer, mc_interface_get_input_voltage_filtered(), 1e1, &ind);
 		}
 		if (mask & ((uint32_t)1 << 8)) {
 			buffer_append_float16(send_buffer, battery_level, 1e3, &ind);

conf_general.h

@@ -24,7 +24,7 @@
 #define FW_VERSION_MAJOR			5
 #define FW_VERSION_MINOR			03
 // Set to 0 for building a release and iterate during beta test builds
-#define FW_TEST_VERSION_NUMBER		27
+#define FW_TEST_VERSION_NUMBER		28
 
 #include "datatypes.h"
 

libcanard/canard_driver.c

@@ -336,7 +336,7 @@ void canard_driver_init(void) {
 static void calculateTotalCurrent(void) {
 	// Calculate total current being consumed by the ESCs on the system
 	float totalCurrent = mc_interface_get_tot_current();
-	float avgVoltage = GET_INPUT_VOLTAGE();
+	float avgVoltage = mc_interface_get_input_voltage_filtered();
 	float totalSysCurrent = 0;
 
 	uint8_t escTotal = 1;
@@ -409,7 +409,7 @@ static void sendEscStatus(void) {
 			mc_interface_get_configuration()->l_current_max_scale) * 100.0;
 	status.rpm = mc_interface_get_rpm();
 	status.temperature = mc_interface_temp_fet_filtered() + 273.15;
-	status.voltage = GET_INPUT_VOLTAGE();
+	status.voltage = mc_interface_get_input_voltage_filtered();
 
 	uavcan_equipment_esc_Status_encode(&status, buffer);
 

mc_interface.c

@@ -83,6 +83,8 @@ typedef struct {
 	float m_motor_current_unbalance;
 	float m_motor_current_unbalance_error_rate;
 	float m_f_samp_now;
+	float m_input_voltage_filtered;
+	float m_input_voltage_filtered_slower;
 
 	// Backup data counters
 	uint64_t m_odometer_last;
@@ -1092,6 +1094,10 @@ float mc_interface_get_tot_current_in_filtered(void) {
 	return ret;
 }
 
+float mc_interface_get_input_voltage_filtered(void) {
+	return motor_now()->m_input_voltage_filtered;
+}
+
 float mc_interface_get_abs_motor_current_unbalance(void) {
 	float ret = 0.0;
 
@@ -1353,7 +1359,7 @@ float mc_interface_temp_motor_filtered(void) {
  */
 float mc_interface_get_battery_level(float *wh_left) {
 	const volatile mc_configuration *conf = mc_interface_get_configuration();
-	const float v_in = GET_INPUT_VOLTAGE();
+	const float v_in = motor_now()->m_input_voltage_filtered_slower;
 	float battery_avg_voltage = 0.0;
 	float battery_avg_voltage_left = 0.0;
 	float ah_left = 0;
@@ -1551,6 +1557,7 @@ void mc_interface_mc_timer_isr(bool is_second_motor) {
 
 	volatile mc_configuration *conf_now = &motor->m_conf;
 	const float input_voltage = GET_INPUT_VOLTAGE();
+	UTILS_LP_FAST(motor->m_input_voltage_filtered, input_voltage, 0.02);
 
 	// Check for faults that should stop the motor
 	static int wrong_voltage_iterations = 0;
@@ -1865,7 +1872,7 @@ void mc_interface_adc_inj_int_handler(void) {
 static void update_override_limits(volatile motor_if_state_t *motor, volatile mc_configuration *conf) {
 	bool is_motor_1 = motor == &m_motor_1;
 
-	const float v_in = GET_INPUT_VOLTAGE();
+	const float v_in = motor->m_input_voltage_filtered;
 	float rpm_now = 0.0;
 
 	if (motor->m_conf.motor_type == MOTOR_TYPE_FOC) {
@@ -2118,6 +2125,8 @@ static void run_timer_tasks(volatile motor_if_state_t *motor) {
 	bool is_motor_1 = motor == &m_motor_1;
 	mc_interface_select_motor_thread(is_motor_1 ? 1 : 2);
 
+	UTILS_LP_FAST(motor->m_input_voltage_filtered_slower, motor->m_input_voltage_filtered, 0.01);
+
 	// Update backup data (for motor 1 only)
 	if (is_motor_1) {
 		uint64_t odometer = mc_interface_get_distance_abs();

mc_interface.h

@@ -65,6 +65,7 @@ float mc_interface_get_tot_current_directional(void);
 float mc_interface_get_tot_current_directional_filtered(void);
 float mc_interface_get_tot_current_in(void);
 float mc_interface_get_tot_current_in_filtered(void);
+float mc_interface_get_input_voltage_filtered(void);
 int mc_interface_get_tachometer_value(bool reset);
 int mc_interface_get_tachometer_abs_value(bool reset);
 float mc_interface_get_last_inj_adc_isr_duration(void);

mcpwm.c

@@ -746,7 +746,7 @@ mc_state mcpwm_get_state(void) {
  * The KV value.
  */
 float mcpwm_get_kv(void) {
-	return rpm_now / (GET_INPUT_VOLTAGE() * fabsf(dutycycle_now));
+	return rpm_now / (mc_interface_get_input_voltage_filtered() * fabsf(dutycycle_now));
 }
 
 /**
@@ -1204,7 +1204,7 @@ static void run_pid_control_speed(void) {
 	}
 #else
 	// Compensation for supply voltage variations
-	float scale = 1.0 / GET_INPUT_VOLTAGE();
+	float scale = 1.0 / mc_interface_get_input_voltage_filtered();
 
 	// Compute parameters
 	p_term = error * conf->s_pid_kp * scale;

mcpwm_foc.c

@@ -1761,9 +1761,9 @@ float mcpwm_foc_measure_inductance(float duty, int samples, float *curr, float *
 	stop_pwm_hw(motor);
 
 	motor->m_conf->foc_sensor_mode = FOC_SENSOR_MODE_HFI;
-	motor->m_conf->foc_hfi_voltage_start = duty * GET_INPUT_VOLTAGE() * (2.0 / 3.0);
-	motor->m_conf->foc_hfi_voltage_run = duty * GET_INPUT_VOLTAGE() * (2.0 / 3.0);
-	motor->m_conf->foc_hfi_voltage_max = duty * GET_INPUT_VOLTAGE() * (2.0 / 3.0);
+	motor->m_conf->foc_hfi_voltage_start = duty * mc_interface_get_input_voltage_filtered() * (2.0 / 3.0);
+	motor->m_conf->foc_hfi_voltage_run = duty * mc_interface_get_input_voltage_filtered() * (2.0 / 3.0);
+	motor->m_conf->foc_hfi_voltage_max = duty * mc_interface_get_input_voltage_filtered() * (2.0 / 3.0);
 	motor->m_conf->foc_sl_erpm_hfi = 20000.0;
 	motor->m_conf->foc_sample_v0_v7 = false;
 	motor->m_conf->foc_hfi_samples = HFI_SAMPLES_32;
@@ -2622,7 +2622,7 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
 				// Truncating the duty cycle here would be dangerous, so run a PID controller.
 
 				// Compensation for supply voltage variations
-				float scale = 1.0 / GET_INPUT_VOLTAGE();
+				float scale = 1.0 / motor_now->m_motor_state.v_bus;
 
 				// Compute error
 				float error = duty_set - motor_now->m_motor_state.duty_now;

terminal.c

@@ -198,7 +198,7 @@ void terminal_process_string(char *str) {
 		commands_printf("Current 1 sample: %u", current1_samp);
 		commands_printf("Current 2 sample: %u\n", current2_samp);
 	} else if (strcmp(argv[0], "volt") == 0) {
-		commands_printf("Input voltage: %.2f\n", (double)GET_INPUT_VOLTAGE());
+		commands_printf("Input voltage: %.2f\n", (double)mc_interface_get_input_voltage_filtered());
 #ifdef HW_HAS_GATE_DRIVER_SUPPLY_MONITOR
 		commands_printf("Gate driver power supply output voltage: %.2f\n", (double)GET_GATE_DRIVER_SUPPLY_VOLTAGE());
 #endif