Merge branch 'master' of https://github.com/vedderb/bldc

datatypes.h

@@ -89,7 +89,8 @@ typedef enum {
 	FAULT_CODE_GATE_DRIVER_OVER_VOLTAGE,
 	FAULT_CODE_GATE_DRIVER_UNDER_VOLTAGE,
 	FAULT_CODE_MCU_UNDER_VOLTAGE,
-	FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET
+	FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET,
+	FAULT_CODE_ENCODER,
 } mc_fault_code;
 
 typedef enum {

encoder.c

@@ -74,6 +74,9 @@ static bool index_found = false;
 static uint32_t enc_counts = 10000;
 static encoder_mode mode = ENCODER_MODE_NONE;
 static float last_enc_angle = 0.0;
+uint16_t spi_val = 0;
+uint32_t spi_error_cnt = 0;
+float spi_error_rate = 0.0;
 
 // Private functions
 static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length);
@@ -81,6 +84,20 @@ static void spi_begin(void);
 static void spi_end(void);
 static void spi_delay(void);
 
+
+uint32_t encoder_spi_get_error_cnt(void) {
+    return spi_error_cnt;
+}
+
+uint16_t encoder_spi_get_val(void) {
+    return spi_val;
+}
+
+float encoder_spi_get_error_rate(void) {
+    return spi_error_rate;
+}
+
+
 void encoder_deinit(void) {
 	nvicDisableVector(HW_ENC_EXTI_CH);
 	nvicDisableVector(HW_ENC_TIM_ISR_CH);
@@ -97,6 +114,7 @@ void encoder_deinit(void) {
 	index_found = false;
 	mode = ENCODER_MODE_NONE;
 	last_enc_angle = 0.0;
+    spi_error_rate = 0.0;
 }
 
 void encoder_init_abi(uint32_t counts) {
@@ -179,6 +197,7 @@ void encoder_init_as5047p_spi(void) {
 
 	mode = ENCODER_MODE_AS5047P_SPI;
 	index_found = true;
+	spi_error_rate = 0.0;
 }
 
 void encoder_init_ad2s1205_spi(void) {
@@ -293,6 +312,16 @@ void encoder_reset(void) {
 	}
 }
 
+// returns true for even number of ones (no parity error according to AS5047 datasheet
+bool spi_check_parity(uint16_t x)
+{
+    x ^= x >> 8;
+    x ^= x >> 4;
+    x ^= x >> 2;
+    x ^= x >> 1;
+    return (~x) & 1;
+}
+
 /**
  * Timer interrupt
  */
@@ -304,8 +333,15 @@ void encoder_tim_isr(void) {
 		spi_transfer(&pos, 0, 1);
 		spi_end();
 
-		pos &= 0x3FFF;
-		last_enc_angle = ((float)pos * 360.0) / 16384.0;
+		spi_val = pos;
+		if(spi_check_parity(pos) && pos!=0xffff && pos!=0) {  // all ones = disconnect, all zeros = short to gnd
+			pos &= 0x3FFF;
+			last_enc_angle = ((float)pos * 360.0) / 16384.0;
+			UTILS_LP_FAST(spi_error_rate, 0.0, 1./AS5047_SAMPLE_RATE_HZ);
+		} else {
+			++spi_error_cnt;
+			UTILS_LP_FAST(spi_error_rate, 1.0, 1./AS5047_SAMPLE_RATE_HZ);
+		}		
 	}
 
 	if(mode == RESOLVER_MODE_AD2S1205) {

encoder.h

@@ -34,4 +34,8 @@ void encoder_tim_isr(void);
 void encoder_set_counts(uint32_t counts);
 bool encoder_index_found(void);
 
+uint16_t encoder_spi_get_val(void);
+uint32_t encoder_spi_get_error_cnt(void);
+float encoder_spi_get_error_rate(void);
+
 #endif /* ENCODER_H_ */

mc_interface.c

@@ -1733,6 +1733,15 @@ static THD_FUNCTION(timer_thread, arg) {
 				break;
 		}
 
+
+		// Trigger encoder error rate fault, using 1% errors as threshold.
+		// Relevant only in FOC mode with encoder enabled
+		if(m_conf.motor_type == MOTOR_TYPE_FOC &&
+			m_conf.foc_sensor_mode == FOC_SENSOR_MODE_ENCODER &&
+			encoder_spi_get_error_rate() > 0.01) {
+			mc_interface_fault_stop(FAULT_CODE_ENCODER);
+		}
+
 		chThdSleepMilliseconds(1);
 	}
 }

terminal.c

@@ -638,6 +638,10 @@ void terminal_process_string(char *str) {
 		} else {
 			commands_printf("This command requires one argument.\n");
 		}
+	} else if (strcmp(argv[0], "encoder") == 0) {
+        commands_printf("SPI val:%x, errors:%d, rate:%.5f", (unsigned int)encoder_spi_get_val(),
+                                                            encoder_spi_get_error_cnt(),
+                                                            (double)encoder_spi_get_error_rate());
 	}
 
 	// The help command
@@ -746,6 +750,9 @@ void terminal_process_string(char *str) {
 		commands_printf("foc_detect_apply_all_can [max_power_loss_W]");
 		commands_printf("  Detect and apply all motor settings, based on maximum resistive motor power losses. Also");
 		commands_printf("  initiates detection in all VESCs found on the CAN-bus.");
+		
+		commands_printf("encoder");
+		commands_printf("  Prints the status of the AS5047 encoder.");
 
 		for (int i = 0;i < callback_write;i++) {
 			if (callbacks[i].arg_names) {