FW4.00: HFI and many updates, see changelog

CHANGELOG

@@ -1,6 +1,13 @@
-=== FW 3.67 ===
-* Added support for HFI to track motor position at 0 speed without sensors.
+=== 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.
+* Reset current integrator when leaving duty cycle control mode. Fixes braking issue #125.
+* More accurate and faster inductance measurement.
+* Ability to measyre ld - lq. Useful for MTPA in future firmwares.
+* Reset shutdown when uploading FW data.
+* Added CAN-bride and COMM_CAN_FWD_FRAME.
+* Added CAN_PACKET_POLL_TS5700N8501_STATUS to poll most relevant data last received from the TS5700N8501 encoder.
+* Added TS57N8501 ABM, SF and ALMC to encoder terminal command.
 
 === FW 3.66 ===
 * Added support for HW 100/250.

appconf/appconf_default.h

@@ -33,8 +33,8 @@
 #ifndef APPCONF_SEND_CAN_STATUS
 #define APPCONF_SEND_CAN_STATUS				CAN_STATUS_DISABLED
 #endif
-#ifndef APPCONF_UAVCAN_ENABLE
-#define APPCONF_UAVCAN_ENABLE				false
+#ifndef APPCONF_CAN_MODE
+#define APPCONF_CAN_MODE					CAN_MODE_VESC
 #endif
 #ifndef APPCONF_UAVCAN_ESC_INDEX
 #define APPCONF_UAVCAN_ESC_INDEX			0

comm_can.c

@@ -1,5 +1,5 @@
 /*
-	Copyright 2016 - 2019 Benjamin Vedder	benjamin@vedder.se
+	Copyright 2016 - 2020 Benjamin Vedder	benjamin@vedder.se
 
 	This file is part of the VESC firmware.
 
@@ -33,6 +33,7 @@
 #include "packet.h"
 #include "hw.h"
 #include "canard_driver.h"
+#include "encoder.h"
 
 // Settings
 #define RX_FRAMES_SIZE	100
@@ -86,6 +87,7 @@ static void set_timing(int brp, int ts1, int ts2);
 
 // Function pointers
 static void(*sid_callback)(uint32_t id, uint8_t *data, uint8_t len) = 0;
+static void(*eid_callback)(uint32_t id, uint8_t *data, uint8_t len) = 0;
 
 void comm_can_init(void) {
 	for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
@@ -190,6 +192,17 @@ void comm_can_set_sid_rx_callback(void (*p_func)(uint32_t id, uint8_t *data, uin
 	sid_callback = p_func;
 }
 
+/**
+ * Set function to be called when extended CAN frames are received. Will only be called when
+ * the CAN mode is CAN_MODE_COMM_BRIDGE.
+ *
+ * @param p_func
+ * Pointer to the function.
+ */
+void comm_can_set_eid_rx_callback(void (*p_func)(uint32_t id, uint8_t *data, uint8_t len)) {
+	eid_callback = p_func;
+}
+
 /**
  * Send a buffer up to RX_BUFFER_SIZE bytes as fragments. If the buffer is 6 bytes or less
  * it will be sent in a single CAN frame, otherwise it will be split into
@@ -393,6 +406,10 @@ void comm_can_set_handbrake_rel(uint8_t controller_id, float current_rel) {
  */
 bool comm_can_ping(uint8_t controller_id) {
 #if CAN_ENABLE
+	if (app_get_configuration()->can_mode != CAN_MODE_VESC) {
+		return false;
+	}
+
 	ping_tp = chThdGetSelfX();
 	chEvtGetAndClearEvents(ALL_EVENTS);
 
@@ -748,7 +765,6 @@ static THD_FUNCTION(cancom_read_thread, arg) {
 		msg_t result = canReceive(&HW_CAN_DEV, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE);
 
 		while (result == MSG_OK) {
-
 			chMtxLock(&can_rx_mtx);
 			rx_frames[rx_frame_write++] = rxmsg;
 			if (rx_frame_write == RX_FRAMES_SIZE) {
@@ -781,7 +797,26 @@ static THD_FUNCTION(cancom_process_thread, arg) {
 	for(;;) {
 		chEvtWaitAny((eventmask_t)1);
 
-		if (app_get_configuration()->uavcan_enable) {
+		if (app_get_configuration()->can_mode == CAN_MODE_UAVCAN) {
+			continue;
+		} else if (app_get_configuration()->can_mode == CAN_MODE_COMM_BRIDGE) {
+			CANRxFrame *rxmsg_tmp;
+			while ((rxmsg_tmp = comm_can_get_rx_frame()) != 0) {
+				CANRxFrame rxmsg = *rxmsg_tmp;
+				commands_fwd_can_frame(rxmsg.DLC, rxmsg.data8,
+						rxmsg.IDE == CAN_IDE_EXT ? rxmsg.EID : rxmsg.SID,
+						rxmsg.IDE == CAN_IDE_EXT);
+
+				if (rxmsg.IDE == CAN_IDE_STD) {
+					if (sid_callback) {
+						sid_callback(rxmsg.SID, rxmsg.data8, rxmsg.DLC);
+					}
+				} else {
+					if (eid_callback) {
+						eid_callback(rxmsg.EID, rxmsg.data8, rxmsg.DLC);
+					}
+				}
+			}
 			continue;
 		}
 
@@ -1012,6 +1047,12 @@ static THD_FUNCTION(cancom_process_thread, arg) {
 						}
 					} break;
 
+					case CAN_PACKET_POLL_TS5700N8501_STATUS: {
+						comm_can_transmit_eid(app_get_configuration()->controller_id |
+								((uint32_t)CAN_PACKET_POLL_TS5700N8501_STATUS << 8),
+								encoder_ts5700n8501_get_raw_status(), 8);
+					} break;
+
 					default:
 						break;
 					}
@@ -1110,7 +1151,7 @@ static THD_FUNCTION(cancom_status_thread, arg) {
 	for(;;) {
 		const app_configuration *conf = app_get_configuration();
 
-		if (!conf->uavcan_enable) {
+		if (conf->can_mode == CAN_MODE_VESC) {
 			if (conf->send_can_status == CAN_STATUS_1 ||
 					conf->send_can_status == CAN_STATUS_1_2 ||
 					conf->send_can_status == CAN_STATUS_1_2_3 ||

comm_can.h

@@ -33,6 +33,7 @@ void comm_can_set_baud(CAN_BAUD baud);
 void comm_can_transmit_eid(uint32_t id, const uint8_t *data, uint8_t len);
 void comm_can_transmit_sid(uint32_t id, uint8_t *data, uint8_t len);
 void comm_can_set_sid_rx_callback(void (*p_func)(uint32_t id, uint8_t *data, uint8_t len));
+void comm_can_set_eid_rx_callback(void (*p_func)(uint32_t id, uint8_t *data, uint8_t len));
 void comm_can_send_buffer(uint8_t controller_id, uint8_t *data, unsigned int len, uint8_t send);
 void comm_can_set_duty(uint8_t controller_id, float duty);
 void comm_can_set_current(uint8_t controller_id, float current);

commands.c

@@ -264,6 +264,8 @@ void commands_process_packet(unsigned char *data, unsigned int len,
 		}
 		uint16_t flash_res = flash_helper_write_new_app_data(new_app_offset, data + ind, len - ind);
 
+		SHUTDOWN_RESET();
+
 		ind = 0;
 		uint8_t send_buffer[50];
 		send_buffer[ind++] = COMM_WRITE_NEW_APP_DATA;
@@ -964,6 +966,18 @@ void commands_process_packet(unsigned char *data, unsigned int len,
 		timeout_reset();
 	} break;
 
+	case COMM_CAN_FWD_FRAME: {
+		int32_t ind = 0;
+		uint32_t id = buffer_get_uint32(data, &ind);
+		bool is_ext = data[ind++];
+
+		if (is_ext) {
+			comm_can_transmit_eid(id, data + ind, len - ind);
+		} else {
+			comm_can_transmit_sid(id, data + ind, len - ind);
+		}
+	} break;
+
 	// Blocking commands. Only one of them runs at any given time, in their
 	// own thread. If other blocking commands come before the previous one has
 	// finished, they are discarded.
@@ -1044,6 +1058,21 @@ void commands_send_experiment_samples(float *samples, int len) {
 	commands_send_packet(buffer, index);
 }
 
+void commands_fwd_can_frame(int len, unsigned char *data, uint32_t id, bool is_extended) {
+	if (len > 8) {
+		len = 8;
+	}
+
+	uint8_t buffer[len + 6];
+	int32_t index = 0;
+	buffer[index++] = COMM_CAN_FWD_FRAME;
+	buffer_append_uint32(buffer, id, &index);
+	buffer[index++] = is_extended;
+	memcpy(buffer + index, data, len);
+	index += len;
+	commands_send_packet(buffer, index);
+}
+
 disp_pos_mode commands_get_disp_pos_mode(void) {
 	return display_position_mode;
 }
@@ -1508,7 +1537,6 @@ static THD_FUNCTION(blocking_thread, arg) {
 				send_func_blocking(send_buffer, ind);
 			}
 		} break;
-#endif
 
 		case COMM_BM_MEM_READ: {
 			int32_t ind = 0;
@@ -1528,6 +1556,7 @@ static THD_FUNCTION(blocking_thread, arg) {
 				send_func_blocking(send_buffer, ind + read_len);
 			}
 		} break;
+#endif
 
 		default:
 			break;

commands.h

@@ -32,6 +32,7 @@ void commands_process_packet(unsigned char *data, unsigned int len,
 void commands_printf(const char* format, ...);
 void commands_send_rotor_pos(float rotor_pos);
 void commands_send_experiment_samples(float *samples, int len);
+void commands_fwd_can_frame(int len, unsigned char *data, uint32_t id, bool is_extended);
 disp_pos_mode commands_get_disp_pos_mode(void);
 void commands_set_app_data_handler(void(*func)(unsigned char *data, unsigned int len));
 void commands_send_app_data(unsigned char *data, unsigned int len);

conf_general.c

@@ -1100,7 +1100,7 @@ int conf_general_detect_apply_all_foc(float max_power_loss,
 	}
 
 	float r = mcpwm_foc_measure_resistance(i_last, 100);
-	float l = mcpwm_foc_measure_inductance_current(i_last, 100, 0) * 1e-6;
+	float l = mcpwm_foc_measure_inductance_current(i_last, 100, 0, 0) * 1e-6;
 	float i_max = sqrtf(max_power_loss / r);
 	utils_truncate_number(&i_max, HW_LIM_CURRENT);
 
@@ -1127,7 +1127,7 @@ int conf_general_detect_apply_all_foc(float max_power_loss,
 		mcconf_old.l_current_max = i_max;
 		mcconf_old.l_current_min = -i_max;
 
-		float tc = 1000.0;
+		float tc = 4000.0;
 		float bw = 1.0 / (tc * 1e-6);
 		float kp = l * bw;
 		float ki = r * bw;

conf_general.h

@@ -21,8 +21,8 @@
 #define CONF_GENERAL_H_
 
 // Firmware version
-#define FW_VERSION_MAJOR		3
-#define FW_VERSION_MINOR		67
+#define FW_VERSION_MAJOR		4
+#define FW_VERSION_MINOR		00
 
 #include "datatypes.h"
 

confgenerator.c

@@ -101,9 +101,11 @@ int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *
 	buffer[ind++] = conf->foc_observer_type;
 	buffer_append_float32_auto(buffer, conf->foc_hfi_voltage_start, &ind);
 	buffer_append_float32_auto(buffer, conf->foc_hfi_voltage_run, &ind);
+	buffer_append_float32_auto(buffer, conf->foc_hfi_voltage_max, &ind);
 	buffer_append_float32_auto(buffer, conf->foc_sl_erpm_hfi, &ind);
 	buffer_append_uint16(buffer, conf->foc_hfi_start_samples, &ind);
 	buffer_append_float32_auto(buffer, conf->foc_hfi_obs_ovr_sec, &ind);
+	buffer[ind++] = conf->foc_hfi_samples;
 	buffer_append_int16(buffer, conf->gpd_buffer_notify_left, &ind);
 	buffer_append_int16(buffer, conf->gpd_buffer_interpol, &ind);
 	buffer_append_float32_auto(buffer, conf->gpd_current_filter_const, &ind);
@@ -163,7 +165,7 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration
 	buffer[ind++] = conf->pairing_done;
 	buffer[ind++] = conf->permanent_uart_enabled;
 	buffer[ind++] = conf->shutdown_mode;
-	buffer[ind++] = conf->uavcan_enable;
+	buffer[ind++] = conf->can_mode;
 	buffer[ind++] = (uint8_t)conf->uavcan_esc_index;
 	buffer[ind++] = conf->app_to_use;
 	buffer[ind++] = conf->app_ppm_conf.ctrl_type;
@@ -374,9 +376,11 @@ bool confgenerator_deserialize_mcconf(const uint8_t *buffer, mc_configuration *c
 	conf->foc_observer_type = buffer[ind++];
 	conf->foc_hfi_voltage_start = buffer_get_float32_auto(buffer, &ind);
 	conf->foc_hfi_voltage_run = buffer_get_float32_auto(buffer, &ind);
+	conf->foc_hfi_voltage_max = buffer_get_float32_auto(buffer, &ind);
 	conf->foc_sl_erpm_hfi = buffer_get_float32_auto(buffer, &ind);
 	conf->foc_hfi_start_samples = buffer_get_uint16(buffer, &ind);
 	conf->foc_hfi_obs_ovr_sec = buffer_get_float32_auto(buffer, &ind);
+	conf->foc_hfi_samples = buffer[ind++];
 	conf->gpd_buffer_notify_left = buffer_get_int16(buffer, &ind);
 	conf->gpd_buffer_interpol = buffer_get_int16(buffer, &ind);
 	conf->gpd_current_filter_const = buffer_get_float32_auto(buffer, &ind);
@@ -439,7 +443,7 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration
 	conf->pairing_done = buffer[ind++];
 	conf->permanent_uart_enabled = buffer[ind++];
 	conf->shutdown_mode = buffer[ind++];
-	conf->uavcan_enable = buffer[ind++];
+	conf->can_mode = buffer[ind++];
 	conf->uavcan_esc_index = buffer[ind++];
 	conf->app_to_use = buffer[ind++];
 	conf->app_ppm_conf.ctrl_type = buffer[ind++];
@@ -643,9 +647,11 @@ void confgenerator_set_defaults_mcconf(mc_configuration *conf) {
 	conf->foc_observer_type = MCCONF_FOC_OBSERVER_TYPE;
 	conf->foc_hfi_voltage_start = MCCONF_FOC_HFI_VOLTAGE_START;
 	conf->foc_hfi_voltage_run = MCCONF_FOC_HFI_VOLTAGE_RUN;
+	conf->foc_hfi_voltage_max = MCCONF_FOC_HFI_VOLTAGE_MAX;
 	conf->foc_sl_erpm_hfi = MCCONF_FOC_SL_ERPM_HFI;
 	conf->foc_hfi_start_samples = MCCONF_FOC_HFI_START_SAMPLES;
 	conf->foc_hfi_obs_ovr_sec = MCCONF_FOC_HFI_OBS_OVR_SEC;
+	conf->foc_hfi_samples = MCCONF_FOC_HFI_SAMPLES;
 	conf->gpd_buffer_notify_left = MCCONF_GPD_BUFFER_NOTIFY_LEFT;
 	conf->gpd_buffer_interpol = MCCONF_GPD_BUFFER_INTERPOL;
 	conf->gpd_current_filter_const = MCCONF_GPD_CURRENT_FILTER_CONST;
@@ -699,7 +705,7 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) {
 	conf->pairing_done = APPCONF_PAIRING_DONE;
 	conf->permanent_uart_enabled = APPCONF_PERMANENT_UART_ENABLED;
 	conf->shutdown_mode = APPCONF_SHUTDOWN_MODE;
-	conf->uavcan_enable = APPCONF_UAVCAN_ENABLE;
+	conf->can_mode = APPCONF_CAN_MODE;
 	conf->uavcan_esc_index = APPCONF_UAVCAN_ESC_INDEX;
 	conf->app_to_use = APPCONF_APP_TO_USE;
 	conf->app_ppm_conf.ctrl_type = APPCONF_PPM_CTRL_TYPE;

confgenerator.h

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

datatypes.h

@@ -200,6 +200,12 @@ typedef enum {
 	BATTERY_TYPE_LEAD_ACID
 } BATTERY_TYPE;
 
+typedef enum {
+	HFI_SAMPLES_8 = 0,
+	HFI_SAMPLES_16,
+	HFI_SAMPLES_32
+} foc_hfi_samples;
+
 typedef struct {
 	// Switching and drive
 	mc_pwm_mode pwm_mode;
@@ -291,9 +297,11 @@ typedef struct {
 	mc_foc_observer_type foc_observer_type;
 	float foc_hfi_voltage_start;
 	float foc_hfi_voltage_run;
+	float foc_hfi_voltage_max;
 	float foc_sl_erpm_hfi;
 	uint16_t foc_hfi_start_samples;
 	float foc_hfi_obs_ovr_sec;
+	foc_hfi_samples foc_hfi_samples;
 	// GPDrive
 	int gpd_buffer_notify_left;
 	int gpd_buffer_interpol;
@@ -599,6 +607,12 @@ typedef struct {
 	float gyro_offset_comp_clamp;
 } imu_config;
 
+typedef enum {
+	CAN_MODE_VESC = 0,
+	CAN_MODE_UAVCAN,
+	CAN_MODE_COMM_BRIDGE
+} CAN_MODE;
+
 typedef struct {
 	// Settings
 	uint8_t controller_id;
@@ -611,8 +625,8 @@ typedef struct {
 	bool permanent_uart_enabled;
 	SHUTDOWN_MODE shutdown_mode;
 
-	// UAVCAN
-	bool uavcan_enable;
+	// CAN modes
+	CAN_MODE can_mode;
 	uint8_t uavcan_esc_index;
 
 	// Application to use
@@ -726,7 +740,8 @@ typedef enum {
 	COMM_WRITE_NEW_APP_DATA_LZO,
 	COMM_WRITE_NEW_APP_DATA_ALL_CAN_LZO,
 	COMM_BM_WRITE_FLASH_LZO,
-	COMM_SET_CURRENT_REL
+	COMM_SET_CURRENT_REL,
+	COMM_CAN_FWD_FRAME
 } COMM_PACKET_ID;
 
 // CAN commands
@@ -758,7 +773,8 @@ typedef enum {
 	CAN_PACKET_CONF_STORE_CURRENT_LIMITS_IN,
 	CAN_PACKET_CONF_FOC_ERPMS,
 	CAN_PACKET_CONF_STORE_FOC_ERPMS,
-	CAN_PACKET_STATUS_5
+	CAN_PACKET_STATUS_5,
+	CAN_PACKET_POLL_TS5700N8501_STATUS
 } CAN_PACKET_ID;
 
 // Logged fault data

encoder.c

@@ -111,6 +111,7 @@ static THD_FUNCTION(ts5700n8501_thread, arg);
 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};
 
 // Private functions
 static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length);
@@ -171,6 +172,16 @@ float encoder_sincos_get_signal_above_max_error_rate(void) {
 	return sincos_signal_above_max_error_rate;
 }
 
+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);
+}
+
 void encoder_deinit(void) {
 	nvicDisableVector(HW_ENC_EXTI_CH);
 	nvicDisableVector(HW_ENC_TIM_ISR_CH);
@@ -725,11 +736,11 @@ static THD_FUNCTION(ts5700n8501_thread, arg) {
 			return;
 		}
 
-		TS5700N8501_send_byte(0b01000000);
+		TS5700N8501_send_byte(0b01011000);
 
 		chThdSleep(1);
 
-		uint8_t reply[6];
+		uint8_t reply[11];
 		int reply_ind = 0;
 
 		msg_t res = sdGetTimeout(&HW_UART_DEV, TIME_IMMEDIATE);
@@ -745,11 +756,20 @@ static THD_FUNCTION(ts5700n8501_thread, arg) {
 			crc = (reply[i] ^ crc);
 		}
 
-		if (reply_ind == 6 && crc == reply[reply_ind - 1]) {
+		if (reply_ind == 11 && crc == reply[reply_ind - 1]) {
 			uint32_t pos = (uint32_t)reply[2] + ((uint32_t)reply[3] << 8) + ((uint32_t)reply[4] << 16);
 			spi_val = pos;
 			last_enc_angle = (float)pos / 131072.0 * 360.0;
 			UTILS_LP_FAST(spi_error_rate, 0.0, 1.0 / AS5047_SAMPLE_RATE_HZ);
+
+			ts5700n8501_raw_status[0] = reply[1]; // SF
+			ts5700n8501_raw_status[1] = reply[2]; // ABS0
+			ts5700n8501_raw_status[2] = reply[3]; // ABS1
+			ts5700n8501_raw_status[3] = reply[4]; // ABS2
+			ts5700n8501_raw_status[4] = reply[6]; // ABM0
+			ts5700n8501_raw_status[5] = reply[7]; // ABM1
+			ts5700n8501_raw_status[6] = reply[8]; // ABM2
+			ts5700n8501_raw_status[7] = reply[9]; // ALMC
 		} else {
 			++spi_error_cnt;
 			UTILS_LP_FAST(spi_error_rate, 1.0, 1.0 / AS5047_SAMPLE_RATE_HZ);

encoder.h

@@ -50,5 +50,7 @@ uint32_t encoder_sincos_get_signal_below_min_error_cnt(void);
 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);
 
 #endif /* ENCODER_H_ */

libcanard/canard_driver.c

@@ -310,7 +310,7 @@ static THD_FUNCTION(canard_thread, arg) {
 	for (;;) {
 		const app_configuration *conf = app_get_configuration();
 
-		if (!conf->uavcan_enable) {
+		if (conf->can_mode != CAN_MODE_UAVCAN) {
 			chThdSleepMilliseconds(100);
 			continue;
 		}

mcconf/mcconf_default.h

@@ -156,7 +156,7 @@
 #define MCCONF_CC_GAIN					0.0046	// Current controller error gain
 #endif
 #ifndef MCCONF_CC_MIN_CURRENT
-#define MCCONF_CC_MIN_CURRENT			0.1		// Minimum allowed current
+#define MCCONF_CC_MIN_CURRENT			0.05	// Minimum allowed current
 #endif
 #ifndef MCCONF_CC_STARTUP_BOOST_DUTY
 #define MCCONF_CC_STARTUP_BOOST_DUTY	0.01	// The lowest duty cycle to use in current control mode (has to be > MCPWM_MIN_DUTY_CYCLE)
@@ -336,19 +336,25 @@
 #define MCCONF_FOC_OBSERVER_TYPE		FOC_OBSERVER_ORTEGA_ORIGINAL // Position observer type for FOC
 #endif
 #ifndef MCCONF_FOC_HFI_VOLTAGE_START
-#define MCCONF_FOC_HFI_VOLTAGE_START	25 // HFI voltage at start when resolving ambiguity
+#define MCCONF_FOC_HFI_VOLTAGE_START	20 // HFI voltage at start when resolving ambiguity
 #endif
 #ifndef MCCONF_FOC_HFI_VOLTAGE_RUN
-#define MCCONF_FOC_HFI_VOLTAGE_RUN		5 // HFI voltage during tracking
+#define MCCONF_FOC_HFI_VOLTAGE_RUN		4 // HFI voltage during tracking
+#endif
+#ifndef MCCONF_FOC_HFI_VOLTAGE_MAX
+#define MCCONF_FOC_HFI_VOLTAGE_MAX		10 // HFI voltage during tracking at max current
 #endif
 #ifndef MCCONF_FOC_SL_ERPM_HFI
-#define MCCONF_FOC_SL_ERPM_HFI			700.0	// ERPM above which only the observer is used
+#define MCCONF_FOC_SL_ERPM_HFI			2000.0	// ERPM above which only the observer is used
 #endif
 #ifndef MCCONF_FOC_HFI_START_SAMPLES
-#define MCCONF_FOC_HFI_START_SAMPLES	50 // Sample this often at start to resolve ambiguity
+#define MCCONF_FOC_HFI_START_SAMPLES	65 // Sample this often at start to resolve ambiguity
 #endif
 #ifndef MCCONF_FOC_HFI_OBS_OVR_SEC
-#define MCCONF_FOC_HFI_OBS_OVR_SEC		0.05 // Continue using observer for this long when entering HFI speed
+#define MCCONF_FOC_HFI_OBS_OVR_SEC		0.001 // Continue using observer for this long when entering HFI speed
+#endif
+#ifndef MCCONF_FOC_HFI_SAMPLES
+#define MCCONF_FOC_HFI_SAMPLES			HFI_SAMPLES_16 // Samples per motor revolution for HFI
 #endif
 
 // GPD

mcpwm_foc.h

@@ -1,5 +1,5 @@
 /*
-	Copyright 2016 Benjamin Vedder	benjamin@vedder.se
+	Copyright 2016 - 2020 Benjamin Vedder	benjamin@vedder.se
 
 	This file is part of the VESC firmware.
 
@@ -72,8 +72,8 @@ float mcpwm_foc_get_vd(void);
 float mcpwm_foc_get_vq(void);
 void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *ratio, bool *inverted);
 float mcpwm_foc_measure_resistance(float current, int samples);
-float mcpwm_foc_measure_inductance(float duty, int samples, float *curr);
-float mcpwm_foc_measure_inductance_current(float curr_goal, int samples, float *curr);
+float mcpwm_foc_measure_inductance(float duty, int samples, float *curr, float *ld_lq_diff);
+float mcpwm_foc_measure_inductance_current(float curr_goal, int samples, float *curr, float *ld_lq_diff);
 bool mcpwm_foc_measure_res_ind(float *res, float *ind);
 bool mcpwm_foc_hall_detect(float current, uint8_t *hall_table);
 void mcpwm_foc_print_state(void);
@@ -88,8 +88,6 @@ void mcpwm_foc_tim_sample_int_handler(void);
 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_CURRENT_SAMP_OFFSET				(2) // Offset from timer top for injected ADC samples
 
 #endif /* MCPWM_FOC_H_ */

terminal.c

@@ -1,5 +1,5 @@
 /*
-	Copyright 2016 - 2019 Benjamin Vedder	benjamin@vedder.se
+	Copyright 2016 - 2020 Benjamin Vedder	benjamin@vedder.se
 
 	This file is part of the VESC firmware.
 
@@ -315,12 +315,12 @@ void terminal_process_string(char *str) {
 
 			if (duty > 0.0 && duty < 0.9) {
 				mcconf.motor_type = MOTOR_TYPE_FOC;
-				mcconf.foc_f_sw = 3000.0;
 				mc_interface_set_configuration(&mcconf);
 
-				float curr;
-				float ind = mcpwm_foc_measure_inductance(duty, 200, &curr);
-				commands_printf("Inductance: %.2f microhenry (%.2f A)\n", (double)ind, (double)curr);
+				float curr, ld_lq_diff;
+				float ind = mcpwm_foc_measure_inductance(duty, 400, &curr, &ld_lq_diff);
+				commands_printf("Inductance: %.2f uH, ld_lq_diff: %.2f uH (%.2f A)\n",
+						(double)ind, (double)ld_lq_diff, (double)curr);
 
 				mc_interface_set_configuration(&mcconf_old);
 			} else {
@@ -685,10 +685,18 @@ void terminal_process_string(char *str) {
 		if (mcconf.m_sensor_port_mode == SENSOR_PORT_MODE_AS5047_SPI ||
 			mcconf.m_sensor_port_mode == SENSOR_PORT_MODE_AD2S1205 ||
 			mcconf.m_sensor_port_mode == SENSOR_PORT_MODE_TS5700N8501) {
-			commands_printf("SPI encoder value: %x, errors: %d, error rate: %.3f %%",
+			commands_printf("SPI encoder value: %d, errors: %d, error rate: %.3f %%",
 				(unsigned int)encoder_spi_get_val(),
 				encoder_spi_get_error_cnt(),
 				(double)encoder_spi_get_error_rate() * (double)100.0);
+
+			if (mcconf.m_sensor_port_mode == SENSOR_PORT_MODE_TS5700N8501) {
+				char sf[9];
+				char almc[9];
+				utils_byte_to_binary(encoder_ts5700n8501_get_raw_status()[0], sf);
+				utils_byte_to_binary(encoder_ts5700n8501_get_raw_status()[7], almc);
+				commands_printf("TS5700N8501 ABM: %d, SF: %s, ALMC: %s\n", encoder_ts57n8501_get_abm, sf, almc);
+			}
 		}
 
 		if (mcconf.m_sensor_port_mode == SENSOR_PORT_MODE_SINCOS) {

utils.c

@@ -639,6 +639,18 @@ uint32_t utils_crc32c(uint8_t *data, uint32_t len) {
 	return ~crc;
 }
 
+// Yes, this is only the average...
+void utils_fft32_bin0(float *real_in, float *real, float *imag) {
+	*real = 0.0;
+	*imag = 0.0;
+
+	for (int i = 0;i < 32;i++) {
+		*real += real_in[i];
+	}
+
+	*real /= 32.0;
+}
+
 void utils_fft32_bin1(float *real_in, float *real, float *imag) {
 	*real = 0.0;
 	*imag = 0.0;
@@ -661,6 +673,72 @@ void utils_fft32_bin2(float *real_in, float *real, float *imag) {
 	*imag /= 32.0;
 }
 
+void utils_fft16_bin0(float *real_in, float *real, float *imag) {
+	*real = 0.0;
+	*imag = 0.0;
+
+	for (int i = 0;i < 16;i++) {
+		*real += real_in[i];
+	}
+
+	*real /= 16.0;
+}
+
+void utils_fft16_bin1(float *real_in, float *real, float *imag) {
+	*real = 0.0;
+	*imag = 0.0;
+	for (int i = 0;i < 16;i++) {
+		*real += real_in[i] * utils_tab_cos_32_1[2 * i];
+		*imag -= real_in[i] * utils_tab_sin_32_1[2 * i];
+	}
+	*real /= 16.0;
+	*imag /= 16.0;
+}
+
+void utils_fft16_bin2(float *real_in, float *real, float *imag) {
+	*real = 0.0;
+	*imag = 0.0;
+	for (int i = 0;i < 16;i++) {
+		*real += real_in[i] * utils_tab_cos_32_2[2 * i];
+		*imag -= real_in[i] * utils_tab_sin_32_2[2 * i];
+	}
+	*real /= 16.0;
+	*imag /= 16.0;
+}
+
+void utils_fft8_bin0(float *real_in, float *real, float *imag) {
+	*real = 0.0;
+	*imag = 0.0;
+
+	for (int i = 0;i < 8;i++) {
+		*real += real_in[i];
+	}
+
+	*real /= 8.0;
+}
+
+void utils_fft8_bin1(float *real_in, float *real, float *imag) {
+	*real = 0.0;
+	*imag = 0.0;
+	for (int i = 0;i < 8;i++) {
+		*real += real_in[i] * utils_tab_cos_32_1[4 * i];
+		*imag -= real_in[i] * utils_tab_sin_32_1[4 * i];
+	}
+	*real /= 8.0;
+	*imag /= 8.0;
+}
+
+void utils_fft8_bin2(float *real_in, float *real, float *imag) {
+	*real = 0.0;
+	*imag = 0.0;
+	for (int i = 0;i < 8;i++) {
+		*real += real_in[i] * utils_tab_cos_32_2[4 * i];
+		*imag -= real_in[i] * utils_tab_sin_32_2[4 * i];
+	}
+	*real /= 8.0;
+	*imag /= 8.0;
+}
+
 const float utils_tab_sin_32_1[] = {
 	0.000000, 0.195090, 0.382683, 0.555570, 0.707107, 0.831470, 0.923880, 0.980785,
 	1.000000, 0.980785, 0.923880, 0.831470, 0.707107, 0.555570, 0.382683, 0.195090,

utils.h

@@ -50,8 +50,15 @@ float utils_throttle_curve(float val, float curve_acc, float curve_brake, int mo
 void utils_sys_lock_cnt(void);
 void utils_sys_unlock_cnt(void);
 uint32_t utils_crc32c(uint8_t *data, uint32_t len);
+void utils_fft32_bin0(float *real_in, float *real, float *imag);
 void utils_fft32_bin1(float *real_in, float *real, float *imag);
 void utils_fft32_bin2(float *real_in, float *real, float *imag);
+void utils_fft16_bin0(float *real_in, float *real, float *imag);
+void utils_fft16_bin1(float *real_in, float *real, float *imag);
+void utils_fft16_bin2(float *real_in, float *real, float *imag);
+void utils_fft8_bin0(float *real_in, float *real, float *imag);
+void utils_fft8_bin1(float *real_in, float *real, float *imag);
+void utils_fft8_bin2(float *real_in, float *real, float *imag);
 
 // Return the sign of the argument. -1 if negative, 1 if zero or positive.
 #define SIGN(x)				((x < 0) ? -1 : 1)