Leave debug mode on NRF5x after SWD upload, more TS5700N8501 encoder …

…features, more CAN baud rates

CHANGELOG

@@ -1,3 +1,10 @@
+=== FW 4.01 ===
+* Leave debug mode on NRF5x after disconnect to avoid excess power consumption before power cycle.
+* Added encoder_clear_errors and encoder_clear_multiturn terminal commands.
+* Initialize current offsets to 2048 to avoid a fault code to be logged at boot.
+* Added 10K, 20K, 50K and 75K CAN baud rates.
+* Added very basic TS5700N8501 multiturn mode.
+
 === FW 4.00 ===
 * Added support for HFI to track motor position at 0 speed without sensors. This is the main new feature of FW 4.
 * Fixed CAN-bug in VESC Remote.

blackmagic/bm_if.c

@@ -24,6 +24,8 @@
 #include "commands.h"
 #include "terminal.h"
 #include "exception.h"
+#include "target_internal.h"
+#include "adiv5.h"
 
 // Global variables
 long cortexm_wait_timeout = 2000; /* Timeout to wait for Cortex to react on halt command. */
@@ -141,17 +143,25 @@ static int idcode_to_device(uint32_t idcode) {
 	return ret;
 }
 
+static int swdp_scan_twice(void) {
+	int devs = adiv5_swdp_scan();
+
+	if(devs <= 0) {
+		devs = adiv5_swdp_scan();
+	}
+
+	return devs;
+}
+
 // Terminal commands
 static void terminal_swdp_scan(int argc, const char **argv) {
 	(void)argc;
 	(void)argv;
 
 	target_print_en = true;
-
 	bm_set_enabled(true);
-	target_print_en = true;
 
-	int devs = adiv5_swdp_scan();
+	int devs = swdp_scan_twice();
 
 	if(devs <= 0) {
 		commands_printf("SW-DP scan failed!");
@@ -356,7 +366,7 @@ int bm_connect(void) {
 	bm_set_enabled(true);
 	target_print_en = false;
 
-	int devs = adiv5_swdp_scan();
+	int devs = swdp_scan_twice();
 
 	if (devs > 0) {
 		cur_target = target_attach_n(1, &gdb_controller);
@@ -512,6 +522,26 @@ int bm_reboot(void) {
 	return ret;
 }
 
+/**
+ * Leave debug mode of NRF5x device. Will reduce the sleep power consumption
+ * significantly.
+ */
+void bm_leave_nrf_debug_mode(void) {
+	bm_set_enabled(true);
+
+	if (!target_list) {
+		swdp_scan_twice();
+	}
+
+	if (target_list) {
+		if (strncmp(target_list[0].driver, "Nordic", 6) == 0) {
+			adiv5_dp_write(((ADIv5_AP_t**)target_list[0].priv)[0]->dp, ADIV5_DP_CTRLSTAT, 0);
+		}
+	}
+
+	bm_set_enabled(false);
+}
+
 /**
  * Disconnect from target and release SWD bus
  */
@@ -523,6 +553,8 @@ void bm_disconnect(void) {
 		cur_target = 0;
 	}
 
+	bm_leave_nrf_debug_mode();
+
 	bm_set_enabled(false);
 }
 

blackmagic/bm_if.h

@@ -31,6 +31,7 @@ int bm_erase_flash_all(void);
 int bm_write_flash(uint32_t addr, const void *data, uint32_t len);
 int bm_mem_read(uint32_t addr, void *data, uint32_t len);
 int bm_reboot(void);
+void bm_leave_nrf_debug_mode(void);
 void bm_disconnect(void);
 void bm_change_swd_pins(stm32_gpio_t *swdio_port, int swdio_pin,
 		stm32_gpio_t *swclk_port, int swclk_pin);

comm_can.c

@@ -132,6 +132,10 @@ void comm_can_set_baud(CAN_BAUD baud) {
 	case CAN_BAUD_250K:	set_timing(7, 14, 4); break;
 	case CAN_BAUD_500K:	set_timing(5, 9, 2); break;
 	case CAN_BAUD_1M:	set_timing(2, 9, 2); break;
+	case CAN_BAUD_10K:	set_timing(299, 10, 1); break;
+	case CAN_BAUD_20K:	set_timing(149, 10, 1); break;
+	case CAN_BAUD_50K:	set_timing(59, 10, 1); break;
+	case CAN_BAUD_75K:	set_timing(39, 10, 1); break;
 	default: break;
 	}
 }
@@ -1226,6 +1230,24 @@ static void send_packet_wrapper(unsigned char *data, unsigned int len) {
 }
 #endif
 
+/**
+ * Set the CAN timing. The CAN is clocked at 42 MHz, and the baud rate can be
+ * calculated with
+ *
+ * 42000000 / ((brp + 1) * (ts1 + ts2 + 3))
+ *
+ * ts1 should be larger than ts2 in general to take the sample after the
+ * signal had time to stabilize.
+ *
+ * @param brp
+ * Prescaler.
+ *
+ * @param ts1
+ * TS1.
+ *
+ * @param ts2
+ * TS2.
+ */
 static void set_timing(int brp, int ts1, int ts2) {
 	brp &= 0b1111111111;
 	ts1 &= 0b1111;

commands.c

@@ -1504,6 +1504,7 @@ static THD_FUNCTION(blocking_thread, arg) {
 
 		case COMM_BM_DISCONNECT: {
 			bm_disconnect();
+			bm_leave_nrf_debug_mode();
 
 			int32_t ind = 0;
 			send_buffer[ind++] = packet_id;

conf_general.h

@@ -22,7 +22,7 @@
 
 // Firmware version
 #define FW_VERSION_MAJOR		4
-#define FW_VERSION_MINOR		00
+#define FW_VERSION_MINOR		01
 
 #include "datatypes.h"
 

confgenerator.h

@@ -8,8 +8,8 @@
 #include <stdbool.h>
 
 // Constants
-#define MCCONF_SIGNATURE		657785302
-#define APPCONF_SIGNATURE		511269002
+#define MCCONF_SIGNATURE		3632471335
+#define APPCONF_SIGNATURE		2964134872
 
 // Functions
 int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf);

datatypes.h

@@ -156,7 +156,8 @@ typedef enum {
 	SENSOR_PORT_MODE_AS5047_SPI,
 	SENSOR_PORT_MODE_AD2S1205,
 	SENSOR_PORT_MODE_SINCOS,
-	SENSOR_PORT_MODE_TS5700N8501
+	SENSOR_PORT_MODE_TS5700N8501,
+	SENSOR_PORT_MODE_TS5700N8501_MULTITURN
 } sensor_port_mode;
 
 typedef struct {
@@ -191,7 +192,11 @@ typedef enum {
 	CAN_BAUD_125K = 0,
 	CAN_BAUD_250K,
 	CAN_BAUD_500K,
-	CAN_BAUD_1M
+	CAN_BAUD_1M,
+	CAN_BAUD_10K,
+	CAN_BAUD_20K,
+	CAN_BAUD_50K,
+	CAN_BAUD_75K
 } CAN_BAUD;
 
 typedef enum {

encoder.c

@@ -24,7 +24,7 @@
 #include "hw.h"
 #include "mc_interface.h"
 #include "utils.h"
-#include "math.h"
+#include <math.h>
 
 // Defines
 #define AS5047P_READ_ANGLECOM		(0x3FFF | 0x4000 | 0x8000) // This is just ones
@@ -34,7 +34,6 @@
 #define SINCOS_MIN_AMPLITUDE		1.0			// sqrt(sin^2 + cos^2) has to be larger than this
 #define SINCOS_MAX_AMPLITUDE		1.65		// sqrt(sin^2 + cos^2) has to be smaller than this
 
-
 #if (AS5047_USE_HW_SPI_PINS || AD2S1205_USE_HW_SPI_PINS)
 #ifdef HW_SPI_DEV
 #define SPI_SW_MISO_GPIO			HW_SPI_PORT_MISO
@@ -112,13 +111,15 @@ static THD_WORKING_AREA(ts5700n8501_thread_wa, 512);
 static volatile bool ts5700n8501_stop_now = true;
 static volatile bool ts5700n8501_is_running = false;
 static volatile uint8_t ts5700n8501_raw_status[8] = {0};
+static volatile bool ts5700n8501_reset_errors = false;
+static volatile bool ts5700n8501_reset_multiturn = false;
 
 // Private functions
 static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length);
 static void spi_begin(void);
 static void spi_end(void);
 static void spi_delay(void);
-void TS5700N8501_send_byte(uint8_t b);
+static void TS5700N8501_send_byte(uint8_t b);
 
 uint32_t encoder_spi_get_error_cnt(void) {
 	return spi_error_cnt;
@@ -176,10 +177,17 @@ uint8_t* encoder_ts5700n8501_get_raw_status(void) {
 	return (uint8_t*)ts5700n8501_raw_status;
 }
 
-uint32_t encoder_ts57n8501_get_abm(void) {
-	return (uint32_t)ts5700n8501_raw_status[4] +
-			((uint32_t)ts5700n8501_raw_status[5] << 8) +
-			((uint32_t)ts5700n8501_raw_status[6] << 16);
+int16_t encoder_ts57n8501_get_abm(void) {
+	return (uint16_t)ts5700n8501_raw_status[4] |
+			((uint16_t)ts5700n8501_raw_status[5] << 8);
+}
+
+void encoder_ts57n8501_reset_errors(void) {
+	ts5700n8501_reset_errors = true;
+}
+
+void encoder_ts57n8501_reset_multiturn(void) {
+	ts5700n8501_reset_multiturn = true;
 }
 
 void encoder_deinit(void) {
@@ -451,6 +459,26 @@ float encoder_read_deg(void) {
 	return angle;
 }
 
+/*
+ * Note: This is not a good solution and needs a proper implementation later...
+ */
+float encoder_read_deg_multiturn(void) {
+	if (mode == ENCODER_MODE_TS5700N8501) {
+		encoder_ts57n8501_get_abm();
+		float ts_mt = (float)encoder_ts57n8501_get_abm();
+		if (fabsf(ts_mt) > 5000.0) {
+			ts_mt = 0;
+			encoder_ts57n8501_reset_multiturn();
+		}
+
+		ts_mt += 5000;
+
+		return encoder_read_deg() / 10000.0 + (360 * ts_mt) / 10000.0;
+	} else {
+		return encoder_read_deg();
+	}
+}
+
 /**
  * Reset the encoder counter. Should be called from the index interrupt.
  */
@@ -678,7 +706,7 @@ void TS5700N8501_delay_uart(void) {
  * the system is locked, but it should finish fast enough to not cause problems for other
  * things due to the high baud rate.
  */
-void TS5700N8501_send_byte(uint8_t b) {
+static void TS5700N8501_send_byte(uint8_t b) {
 	utils_sys_lock_cnt();
 #ifdef HW_ADC_EXT_GPIO
 	palSetPad(HW_ADC_EXT_GPIO, HW_ADC_EXT_PIN);
@@ -736,9 +764,27 @@ static THD_FUNCTION(ts5700n8501_thread, arg) {
 			return;
 		}
 
+		if (ts5700n8501_reset_errors) {
+			for (int i = 0;i < 20;i++) {
+				TS5700N8501_send_byte(0b01011101);
+				chThdSleep(2);
+			}
+
+			ts5700n8501_reset_errors = false;
+		}
+
+		if (ts5700n8501_reset_multiturn) {
+			for (int i = 0;i < 20;i++) {
+				TS5700N8501_send_byte(0b01000110);
+				chThdSleep(2);
+			}
+
+			ts5700n8501_reset_multiturn = false;
+		}
+
 		TS5700N8501_send_byte(0b01011000);
 
-		chThdSleep(1);
+		chThdSleep(2);
 
 		uint8_t reply[11];
 		int reply_ind = 0;

encoder.h

@@ -32,6 +32,7 @@ void encoder_init_sincos(float sin_gain, float sin_offset,
 void encoder_init_ts5700n8501(void);
 bool encoder_is_configured(void);
 float encoder_read_deg(void);
+float encoder_read_deg_multiturn(void);
 void encoder_reset(void);
 void encoder_tim_isr(void);
 void encoder_set_counts(uint32_t counts);
@@ -51,6 +52,8 @@ uint32_t encoder_sincos_get_signal_above_max_error_cnt(void);
 float encoder_sincos_get_signal_below_min_error_rate(void);
 float encoder_sincos_get_signal_above_max_error_rate(void);
 uint8_t* encoder_ts5700n8501_get_raw_status(void);
-uint32_t encoder_ts57n8501_get_abm(void);
+int16_t encoder_ts57n8501_get_abm(void);
+void encoder_ts57n8501_reset_errors(void);
+void encoder_ts57n8501_reset_multiturn(void);
 
 #endif /* ENCODER_H_ */

mc_interface.c

@@ -192,6 +192,7 @@ void mc_interface_init(mc_configuration *configuration) {
 		break;
 
 	case SENSOR_PORT_MODE_TS5700N8501:
+	case SENSOR_PORT_MODE_TS5700N8501_MULTITURN:
 		conf_general_read_app_configuration(&m_tmp_appconf);
 		if (m_tmp_appconf.app_to_use == APP_ADC ||
 				m_tmp_appconf.app_to_use == APP_UART ||
@@ -255,7 +256,8 @@ void mc_interface_set_configuration(mc_configuration *configuration) {
 								m_conf.foc_encoder_sincos_filter_constant);
 			break;
 
-		case SENSOR_PORT_MODE_TS5700N8501: {
+		case SENSOR_PORT_MODE_TS5700N8501:
+		case SENSOR_PORT_MODE_TS5700N8501_MULTITURN: {
 			m_tmp_appconf = *app_get_configuration();
 			if (m_tmp_appconf.app_to_use == APP_ADC ||
 					m_tmp_appconf.app_to_use == APP_UART ||

mcpwm_foc.c

@@ -297,7 +297,9 @@ void mcpwm_foc_init(volatile mc_configuration *configuration) {
 	m_duty3_next = 0;
 	m_duty_next_set = false;
 	memset((void*)m_curr_sum, 0, sizeof(m_curr_sum));
-	memset((void*)m_curr_ofs, 0, sizeof(m_curr_ofs));
+	m_curr_ofs[0] = 2048;
+	m_curr_ofs[1] = 2048;
+	m_curr_ofs[2] = 2048;
 	m_speed_est_fast = 0.0;
 	m_hfi_plot_en = 0;
 
@@ -2348,7 +2350,11 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
 	// TODO: Have another look at this.
 	float angle_now = 0.0;
 	if (encoder_is_configured()) {
-		angle_now = enc_ang;
+		if (m_conf->m_sensor_port_mode == SENSOR_PORT_MODE_TS5700N8501_MULTITURN) {
+			angle_now = encoder_read_deg_multiturn();
+		} else {
+			angle_now = enc_ang;
+		}
 	} else {
 		angle_now = m_motor_state.phase * (180.0 / M_PI);
 	}