Merge pull request #88 from paltatech/current-sensor-fault-detection

Detect current sensor failures

datatypes.h

@@ -93,7 +93,11 @@ typedef enum {
 	FAULT_CODE_ENCODER_SPI,
 	FAULT_CODE_ENCODER_SINCOS_BELOW_MIN_AMPLITUDE,
 	FAULT_CODE_ENCODER_SINCOS_ABOVE_MAX_AMPLITUDE,
-	FAULT_CODE_FLASH_CORRUPTION
+	FAULT_CODE_FLASH_CORRUPTION,
+	FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_1,
+	FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_2,
+	FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_3,
+	FAULT_CODE_UNBALANCED_CURRENTS
 } mc_fault_code;
 
 typedef enum {

hwconf/hw.h

@@ -266,6 +266,16 @@
 #define GET_CURRENT3()		ADC_Value[ADC_IND_CURR3]
 #endif
 #endif
+#ifndef HW_MAX_CURRENT_OFFSET
+#define HW_MAX_CURRENT_OFFSET 				4095
+#endif
+#ifndef MCCONF_MAX_CURRENT_UNBALANCE
+#define MCCONF_MAX_CURRENT_UNBALANCE		200.0
+#endif
+#ifndef MCCONF_MAX_CURRENT_UNBALANCE_RATE
+#define MCCONF_MAX_CURRENT_UNBALANCE_RATE	1.0
+#endif
+
 
 // NRF SW SPI (default to spi header pins)
 #ifndef NRF_PORT_CSN

hwconf/hw_palta.h

@@ -116,6 +116,10 @@
 #define CURRENT_SHUNT_RES		1.000 // Unity gain so we use a single transfer function defined as CURRENT_AMP_GAIN
 #endif
 
+#define HW_MAX_CURRENT_OFFSET					620		// More than this offset (0.5 Vdc) trips the offset fault (likely a sensor disconnected)
+#define MCCONF_MAX_CURRENT_UNBALANCE			130.0	// [Amp] More than this unbalance trips the fault (likely a sensor disconnected)
+#define MCCONF_MAX_CURRENT_UNBALANCE_RATE		0.3		// Fault if more than 30% of the time the motor is unbalanced
+
 // Input voltage
 #define GET_INPUT_VOLTAGE()		((V_REG / 4095.0) * (float)ADC_Value[ADC_IND_VIN_SENS] * ((VIN_R1 + VIN_R2) / VIN_R2))
 

mc_interface.c

@@ -35,6 +35,7 @@
 #include "buffer.h"
 #include "gpdrive.h"
 #include <math.h>
+#include <stdlib.h>
 
 // Macros
 #define DIR_MULT		(m_conf.m_invert_direction ? -1.0 : 1.0)
@@ -66,6 +67,8 @@ static volatile float m_position_set;
 static volatile float m_temp_fet;
 static volatile float m_temp_motor;
 static volatile float m_gate_driver_voltage;
+static volatile float m_motor_current_unbalance;
+static volatile float m_motor_current_unbalance_error_rate;
 
 // Sampling variables
 #define ADC_SAMPLE_MAX_LEN		2000
@@ -125,6 +128,8 @@ void mc_interface_init(mc_configuration *configuration) {
 	m_temp_fet = 0.0;
 	m_temp_motor = 0.0;
 	m_gate_driver_voltage = 0.0;
+	m_motor_current_unbalance = 0.0;
+	m_motor_current_unbalance_error_rate = 0.0;
 
 	m_sample_len = 1000;
 	m_sample_int = 1;
@@ -367,6 +372,11 @@ const char* mc_interface_fault_to_string(mc_fault_code fault) {
 	case FAULT_CODE_ENCODER_SPI: return "FAULT_CODE_ENCODER_SPI"; break;
 	case FAULT_CODE_ENCODER_SINCOS_BELOW_MIN_AMPLITUDE: return "FAULT_CODE_ENCODER_SINCOS_BELOW_MIN_AMPLITUDE"; break;
 	case FAULT_CODE_ENCODER_SINCOS_ABOVE_MAX_AMPLITUDE: return "FAULT_CODE_ENCODER_SINCOS_ABOVE_MAX_AMPLITUDE"; break;
+    case FAULT_CODE_FLASH_CORRUPTION: return "FAULT_CODE_FLASH_CORRUPTION";
+    case FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_1: return "FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_1";
+    case FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_2: return "FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_2";
+    case FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_3: return "FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_3";
+    case FAULT_CODE_UNBALANCED_CURRENTS: return "FAULT_CODE_UNBALANCED_CURRENTS";
 	default: return "FAULT_UNKNOWN"; break;
 	}
 }
@@ -871,6 +881,26 @@ float mc_interface_get_tot_current_in_filtered(void) {
 	return ret;
 }
 
+float mc_interface_get_abs_motor_current_unbalance(void) {
+	float ret = 0.0;
+
+#ifdef HW_HAS_PHASE_SHUNTS
+	switch (m_conf.motor_type) {
+	case MOTOR_TYPE_BLDC:
+	case MOTOR_TYPE_DC:
+		break;
+
+	case MOTOR_TYPE_FOC:
+		ret = mcpwm_foc_get_abs_motor_current_unbalance();
+		break;
+
+	default:
+		break;
+	}
+#endif
+	return ret;
+}
+
 int mc_interface_get_tachometer_value(bool reset) {
 	int ret = 0;
 
@@ -1277,6 +1307,21 @@ void mc_interface_mc_timer_isr(void) {
 	} else {
 		wrong_voltage_iterations = 0;
 	}
+#ifdef HW_HAS_PHASE_SHUNTS
+	// Monitor currents balance. The sum of the 3 currents should be zero
+	m_motor_current_unbalance = mc_interface_get_abs_motor_current_unbalance();
+
+	if ( m_motor_current_unbalance > MCCONF_MAX_CURRENT_UNBALANCE ) {
+		UTILS_LP_FAST(m_motor_current_unbalance_error_rate, 1.0, (1 / 20000.0));
+	}
+	else {
+		UTILS_LP_FAST(m_motor_current_unbalance_error_rate, 0.0, (1 / 20000.0));
+	}
+
+	if (m_motor_current_unbalance_error_rate > MCCONF_MAX_CURRENT_UNBALANCE_RATE) {
+		mc_interface_fault_stop(FAULT_CODE_UNBALANCED_CURRENTS);
+	}
+#endif
 
 	if (mc_interface_get_state() == MC_STATE_RUNNING) {
 		m_cycles_running++;
@@ -1764,6 +1809,25 @@ static THD_FUNCTION(timer_thread, arg) {
 			if (encoder_sincos_get_signal_above_max_error_rate() > 0.05)
 				mc_interface_fault_stop(FAULT_CODE_ENCODER_SINCOS_ABOVE_MAX_AMPLITUDE);
 		}
+#ifdef HW_HAS_PHASE_SHUNTS
+		int m_curr0_offset;
+		int m_curr1_offset;
+		int m_curr2_offset;
+
+		mcpwm_foc_get_current_offsets(&m_curr0_offset, &m_curr1_offset, &m_curr2_offset);
+
+		if (abs(m_curr0_offset - 2048) > HW_MAX_CURRENT_OFFSET) {
+			mc_interface_fault_stop(FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_1);
+		}
+		if (abs(m_curr1_offset - 2048) > HW_MAX_CURRENT_OFFSET) {
+			mc_interface_fault_stop(FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_2);
+		}
+#ifdef HW_HAS_3_SHUNTS
+		if (abs(m_curr2_offset - 2048) > HW_MAX_CURRENT_OFFSET) {
+			mc_interface_fault_stop(FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_3);
+		}
+#endif
+#endif
 
 		chThdSleepMilliseconds(1);
 	}

mcpwm_foc.c

@@ -85,6 +85,7 @@ static volatile int m_curr1_sum;
 static volatile int m_curr_samples;
 static volatile int m_curr0_offset;
 static volatile int m_curr1_offset;
+static volatile int m_curr_unbalance;
 static volatile bool m_phase_override;
 static volatile float m_phase_now_override;
 static volatile float m_duty_cycle_set;
@@ -206,6 +207,7 @@ void mcpwm_foc_init(volatile mc_configuration *configuration) {
 	m_control_mode = CONTROL_MODE_NONE;
 	m_curr0_sum = 0;
 	m_curr1_sum = 0;
+	m_curr_unbalance = 0.0;
 	m_curr_samples = 0;
 	m_dccal_done = false;
 	m_phase_override = false;
@@ -854,6 +856,16 @@ float mcpwm_foc_get_abs_motor_current(void) {
 	return m_motor_state.i_abs;
 }
 
+/**
+ * Get the magnitude of the motor current unbalance
+ *
+ * @return
+ * The magnitude of the phase currents unbalance.
+ */
+float mcpwm_foc_get_abs_motor_current_unbalance(void) {
+	return (float)(m_curr_unbalance) * FAC_CURRENT;
+}
+
 /**
  * Get the magnitude of the motor voltage.
  *
@@ -1696,6 +1708,7 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
 	curr1 -= m_curr1_offset;
 #ifdef HW_HAS_3_SHUNTS
 	curr2 -= m_curr2_offset;
+	m_curr_unbalance = curr0 + curr1 + curr2;
 #endif
 
 	m_curr_samples++;

mcpwm_foc.h

@@ -53,6 +53,7 @@ float mcpwm_foc_get_rpm(void);
 float mcpwm_foc_get_tot_current(void);
 float mcpwm_foc_get_tot_current_filtered(void);
 float mcpwm_foc_get_abs_motor_current(void);
+float mcpwm_foc_get_abs_motor_current_unbalance(void);
 float mcpwm_foc_get_abs_motor_voltage(void);
 float mcpwm_foc_get_abs_motor_current_filtered(void);
 float mcpwm_foc_get_tot_current_directional(void);