Fixed current offset fault bug, added support for multiple IMUs and ICM-20948

CHANGELOG

@@ -1,3 +1,9 @@
+=== FW 3.56 ===
+* Fixed current offset fault bug in non-FOC mode.
+* Multiple IMU support.
+* Added support for the ICM-20948 IMU.
+* Decreased ERPM cut in open loop flux linkage measurement.
+
 === FW 3.55 ===
 * Initial sin/cos encoder support.
 * New ADC control mode.

conf_general.c

@@ -829,7 +829,7 @@ bool conf_general_measure_flux_linkage_openloop(float current, float duty,
 			break;
 		}
 
-		if (rpm_now >= 20000) {
+		if (rpm_now >= 12000) {
 			break;
 		}
 	}

conf_general.h

@@ -22,7 +22,7 @@
 
 // Firmware version
 #define FW_VERSION_MAJOR		3
-#define FW_VERSION_MINOR		55
+#define FW_VERSION_MINOR		56
 
 #include "datatypes.h"
 

hwconf/hw_60.h

@@ -221,7 +221,7 @@
 #define MPU9X50_SDA_PIN			2
 #define MPU9X50_SCL_GPIO		GPIOA
 #define MPU9X50_SCL_PIN			15
-#define MPU9x50_FLIP
+#define IMU_FLIP
 
 // Measurement macros
 #define ADC_V_L1				ADC_Value[ADC_IND_SENS1]

hwconf/hw_binar_v1.c

@@ -53,6 +53,12 @@ void hw_init_gpio(void) {
 			PAL_MODE_OUTPUT_PUSHPULL |
 			PAL_STM32_OSPEED_HIGHEST);
 
+	// Switch on second 3.3v net (TODO: Expose this functionality?)
+	palSetPadMode(GPIOB, 2,
+			PAL_MODE_OUTPUT_PUSHPULL |
+			PAL_STM32_OSPEED_HIGHEST);
+	palSetPad(GPIOB, 2);
+
 	ENABLE_GATE();
 
 	// GPIOA Configuration: Channel 1 to 3 as alternate function push-pull

hwconf/hw_binar_v1.h

@@ -188,12 +188,12 @@
 #define DRV8301_CS_GPIO			GPIOC
 #define DRV8301_CS_PIN			9
 
-// MPU9250
-#define MPU9X50_SDA_GPIO		GPIOB
-#define MPU9X50_SDA_PIN			7
-#define MPU9X50_SCL_GPIO		GPIOB
-#define MPU9X50_SCL_PIN			6
-#define MPU9x50_FLIP
+// ICM20948
+#define ICM20948_SDA_GPIO		GPIOB
+#define ICM20948_SDA_PIN		7
+#define ICM20948_SCL_GPIO		GPIOB
+#define ICM20948_SCL_PIN		6
+#define ICM20948_AD0_VAL		0
 
 // Measurement macros
 #define ADC_V_L1				ADC_Value[ADC_IND_SENS1]

i2c_bb.c

@@ -41,6 +41,7 @@ static bool clock_stretch_timeout(i2c_bb_state *s);
 static void i2c_delay(void);
 
 void i2c_bb_init(i2c_bb_state *s) {
+	chMtxObjectInit(&s->mutex);
 	palSetPadMode(s->sda_gpio, s->sda_pin, PAL_MODE_OUTPUT_OPENDRAIN);
 	palSetPadMode(s->scl_gpio, s->scl_pin, PAL_MODE_OUTPUT_OPENDRAIN);
 	s->has_started = false;
@@ -48,6 +49,8 @@ void i2c_bb_init(i2c_bb_state *s) {
 }
 
 void i2c_bb_restore_bus(i2c_bb_state *s) {
+	chMtxLock(&s->mutex);
+
 	SCL_HIGH();
 	SDA_HIGH();
 
@@ -66,9 +69,13 @@ void i2c_bb_restore_bus(i2c_bb_state *s) {
 	i2c_stop_cond(s);
 
 	s->has_error = false;
+
+	chMtxUnlock(&s->mutex);
 }
 
 bool i2c_bb_tx_rx(i2c_bb_state *s, uint16_t addr, uint8_t *txbuf, size_t txbytes, uint8_t *rxbuf, size_t rxbytes) {
+	chMtxLock(&s->mutex);
+
 	i2c_write_byte(s, true, false, addr << 1);
 
 	for (unsigned int i = 0;i < txbytes;i++) {
@@ -85,6 +92,8 @@ bool i2c_bb_tx_rx(i2c_bb_state *s, uint16_t addr, uint8_t *txbuf, size_t txbytes
 
 	i2c_stop_cond(s);
 
+	chMtxUnlock(&s->mutex);
+
 	return !s->has_error;
 }
 

i2c_bb.h

@@ -32,6 +32,7 @@ typedef struct {
 	int scl_pin;
 	bool has_started;
 	bool has_error;
+	mutex_t mutex;
 } i2c_bb_state;
 
 void i2c_bb_init(i2c_bb_state *s);

imu/icm20948.c

@@ -0,0 +1,182 @@
+/*
+	Copyright 2019 Benjamin Vedder	benjamin@vedder.se
+
+	This file is part of the VESC firmware.
+
+	The VESC firmware is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    The VESC firmware is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+    */
+
+#include "icm20948.h"
+#include "terminal.h"
+#include "commands.h"
+#include "utils.h"
+
+#include <stdio.h>
+#include <string.h>
+
+// Threads
+static THD_FUNCTION(icm_thread, arg);
+
+// Private functions
+static bool reset_init_icm(ICM20948_STATE *s);
+static void terminal_read_reg(int argc, const char **argv);
+static uint8_t read_single_reg(ICM20948_STATE *s, uint8_t reg);
+static bool write_single_reg(ICM20948_STATE *s, uint8_t reg, uint8_t value);
+
+// Function pointers
+static void(*m_read_callback)(float *accel, float *gyro, float *mag) = 0;
+
+// Private variables
+static ICM20948_STATE *m_terminal_state = 0;
+
+void icm20948_init(ICM20948_STATE *s, i2c_bb_state *i2c_state, int ad0_val,
+		stkalign_t *work_area, size_t work_area_size) {
+
+	s->i2cs = i2c_state;
+	s->i2c_address = ad0_val ? 0x69 : 0x68;
+
+	if (reset_init_icm(s)) {
+		chThdCreateStatic(work_area, work_area_size, NORMALPRIO, icm_thread, s);
+	}
+
+	// Only register terminal command for the first instance of this driver.
+	if (m_terminal_state == 0) {
+		m_terminal_state = s;
+		terminal_register_command_callback(
+				"icm_read_reg",
+				"Read register of the ICM-20948",
+				"[bank] [reg]",
+				terminal_read_reg);
+	}
+}
+
+void icm20948_set_read_callback(void(*func)(float *accel, float *gyro, float *mag)) {
+	m_read_callback = func;
+}
+
+static void terminal_read_reg(int argc, const char **argv) {
+	if (argc == 3) {
+		int bank = -1;
+		int reg = -1;
+		sscanf(argv[1], "%d", &bank);
+		sscanf(argv[2], "%d", &reg);
+
+		if (reg >= 0 && (bank == 0 || bank == 1 || bank == 2)) {
+			write_single_reg(m_terminal_state, ICM20948_BANK_SEL, bank << 4);
+			unsigned int res = read_single_reg(m_terminal_state, reg);
+			char bl[9];
+
+			write_single_reg(m_terminal_state, ICM20948_BANK_SEL, 0 << 4);
+
+			utils_byte_to_binary(res & 0xFF, bl);
+
+			commands_printf("Reg 0x%02x: %s (0x%02x)\n", reg, bl, res);
+		} else {
+			commands_printf("Invalid argument(s).\n");
+		}
+	} else {
+		commands_printf("This command requires one argument.\n");
+	}
+}
+
+static bool write_single_reg(ICM20948_STATE *s, uint8_t reg, uint8_t value) {
+	uint8_t txb[2];
+
+	txb[0] = reg;
+	txb[1] = value;
+
+	bool res = i2c_bb_tx_rx(s->i2cs, s->i2c_address, txb, 2, 0, 0);
+	return res;
+}
+
+static uint8_t read_single_reg(ICM20948_STATE *s, uint8_t reg) {
+	uint8_t rxb[1];
+	uint8_t txb[1];
+
+	txb[0] = reg;
+	bool res = i2c_bb_tx_rx(s->i2cs, s->i2c_address, txb, 1, rxb, 1);
+
+	if (res) {
+		return rxb[0];
+	} else {
+		return 0;
+	}
+}
+
+static bool reset_init_icm(ICM20948_STATE *s) {
+	i2c_bb_restore_bus(s->i2cs);
+
+	chThdSleep(1);
+
+	// TODO: Check for errors
+
+	// Set clock source to auto
+	write_single_reg(s, ICM20948_BANK_SEL, 0 << 4);
+	write_single_reg(s, ICM20948_PWR_MGMT_1, 1);
+
+	// Set accelerometer to +-16 G and disable lp filter
+	write_single_reg(s, ICM20948_BANK_SEL, 2 << 4);
+	write_single_reg(s, ICM20948_ACCEL_CONFIG, 0b00000110);
+
+	// Set gyro to +-2000 dps and disable lp filter
+	write_single_reg(s, ICM20948_BANK_SEL, 2 << 4);
+	write_single_reg(s, ICM20948_GYRO_CONFIG_1, 0b00000110);
+
+	// I2C bypass to access magnetometer directly
+//	write_single_reg(s, ICM20948_BANK_SEL, 0);
+//	write_single_reg(s, ICM20948_PIN_CFG, 2);
+
+	// Select bank0 so that data can be polled.
+	write_single_reg(s, ICM20948_BANK_SEL, 0 << 4);
+
+	return true;
+}
+
+static THD_FUNCTION(icm_thread, arg) {
+	ICM20948_STATE *s = (ICM20948_STATE*)arg;
+
+	chRegSetThreadName("ICM Sampling");
+
+	for(;;) {
+		uint8_t txb[1];
+		uint8_t rxb[12];
+		txb[0] = ICM20948_ACCEL_XOUT_H;
+
+		bool res = i2c_bb_tx_rx(s->i2cs, s->i2c_address, txb, 1, rxb, 12);
+
+		if (res) {
+			float accel[3], gyro[3], mag[3];
+
+			accel[0] = (float)((int16_t)((int16_t)rxb[0] << 8 | (int16_t)rxb[1])) * 16.0 / 32768.0;
+			accel[1] = (float)((int16_t)((int16_t)rxb[2] << 8 | (int16_t)rxb[3])) * 16.0 / 32768.0;
+			accel[2] = (float)((int16_t)((int16_t)rxb[4] << 8 | (int16_t)rxb[5])) * 16.0 / 32768.0;
+
+			gyro[0] = (float)((int16_t)((int16_t)rxb[6] << 8 | (int16_t)rxb[7])) * 2000.0 / 32768.0 ;
+			gyro[1] = (float)((int16_t)((int16_t)rxb[8] << 8 | (int16_t)rxb[9])) * 2000.0 / 32768.0;
+			gyro[2] = (float)((int16_t)((int16_t)rxb[10] << 8 | (int16_t)rxb[11])) * 2000.0 / 32768.0;
+
+			// TODO: Read magnetometer as well
+			memset(mag, 0, sizeof(mag));
+
+			if (m_read_callback) {
+				m_read_callback(accel, gyro, mag);
+			}
+		} else {
+			reset_init_icm(s);
+			chThdSleepMilliseconds(10);
+		}
+
+		chThdSleepMilliseconds(5);
+	}
+}

imu/icm20948.h

@@ -0,0 +1,52 @@
+/*
+	Copyright 2019 Benjamin Vedder	benjamin@vedder.se
+
+	This file is part of the VESC firmware.
+
+	The VESC firmware is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    The VESC firmware is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+    */
+
+#ifndef IMU_ICM20948_H_
+#define IMU_ICM20948_H_
+
+#include "ch.h"
+#include "hal.h"
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "i2c_bb.h"
+
+typedef struct {
+	i2c_bb_state *i2cs;
+	uint8_t i2c_address;
+} ICM20948_STATE;
+
+void icm20948_init(ICM20948_STATE *s, i2c_bb_state *i2c_state, int ad0_val,
+		stkalign_t *work_area, size_t work_area_size);
+void icm20948_set_read_callback(void(*func)(float *accel, float *gyro, float *mag));
+
+// All banks
+#define ICM20948_BANK_SEL						0x7F
+
+// Bank 0 registers
+#define ICM20948_PWR_MGMT_1						0x06
+#define ICM20948_PIN_CFG						0x0F
+#define ICM20948_ACCEL_XOUT_H					0x2D
+
+// Bank 2 registers
+#define ICM20948_ACCEL_CONFIG					0x14
+#define ICM20948_GYRO_CONFIG_1					0x01
+
+#endif /* IMU_ICM20948_H_ */

imu/imu.c

@@ -24,17 +24,20 @@
 #include "timer.h"
 #include "terminal.h"
 #include "commands.h"
+#include "icm20948.h"
 
 #include <math.h>
 #include <string.h>
 
 // Private variables
 static ATTITUDE_INFO m_att;
-static bool m_attitude_init_done;
 static float m_accel[3], m_gyro[3], m_mag[3];
+static stkalign_t m_thd_work_area[THD_WORKING_AREA_SIZE(2048) / sizeof(stkalign_t)];
+static i2c_bb_state m_i2c_bb;
+static ICM20948_STATE m_icm20948_state;
 
 // Private functions
-static void mpu_read_callback(void);
+static void imu_read_callback(float *accel, float *gyro, float *mag);
 static void terminal_rpy(int argc, const char **argv);
 
 void imu_init(void) {
@@ -45,6 +48,11 @@ void imu_init(void) {
 			MPU9X50_SCL_GPIO, MPU9X50_SCL_PIN);
 #endif
 
+#ifdef ICM20948_SDA_GPIO
+	imu_init_icm20948(ICM20948_SDA_GPIO, ICM20948_SDA_PIN,
+			ICM20948_SCL_GPIO, ICM20948_SCL_PIN, ICM20948_AD0_VAL);
+#endif
+
 	terminal_register_command_callback(
 			"imu_rpy",
 			"Print 100 roll/pitch/yaw samples at 10 Hz",
@@ -52,10 +60,32 @@ void imu_init(void) {
 			terminal_rpy);
 }
 
-void imu_init_mpu9x50(stm32_gpio_t *sda_gpio, int sda_pin, stm32_gpio_t *scl_gpio, int scl_pin) {
+i2c_bb_state *imu_get_i2c(void) {
+	return &m_i2c_bb;
+}
+
+void imu_init_mpu9x50(stm32_gpio_t *sda_gpio, int sda_pin,
+		stm32_gpio_t *scl_gpio, int scl_pin) {
+
 	mpu9150_init(sda_gpio, sda_pin,
-			scl_gpio, scl_pin);
-	mpu9150_set_read_callback(mpu_read_callback);
+			scl_gpio, scl_pin,
+			m_thd_work_area, sizeof(m_thd_work_area));
+	mpu9150_set_read_callback(imu_read_callback);
+}
+
+void imu_init_icm20948(stm32_gpio_t *sda_gpio, int sda_pin,
+		stm32_gpio_t *scl_gpio, int scl_pin, int ad0_val) {
+
+	m_i2c_bb.sda_gpio = sda_gpio;
+	m_i2c_bb.sda_pin = sda_pin;
+	m_i2c_bb.scl_gpio = scl_gpio;
+	m_i2c_bb.scl_pin = scl_pin;
+	i2c_bb_init(&m_i2c_bb);
+
+	icm20948_init(&m_icm20948_state,
+			&m_i2c_bb, ad0_val,
+			m_thd_work_area, sizeof(m_thd_work_area));
+	icm20948_set_read_callback(imu_read_callback);
 }
 
 float imu_get_roll(void) {
@@ -119,38 +149,35 @@ void imu_get_quaternions(float *q) {
 	q[3] = m_att.q3;
 }
 
-static void mpu_read_callback(void) {
+static void imu_read_callback(float *accel, float *gyro, float *mag) {
 	static uint32_t last_time = 0;
 	float dt = timer_seconds_elapsed_since(last_time);
 	last_time = timer_time_now();
 
-	float tmp_accel[3], tmp_gyro[3], tmp_mag[3];
-	mpu9150_get_accel_gyro_mag(tmp_accel, tmp_gyro, tmp_mag);
+#ifdef IMU_FLIP
+	m_accel[0] = -accel[0];
+	m_accel[1] = accel[1];
+	m_accel[2] = -accel[2];
 
-#ifdef MPU9x50_FLIP
-	m_accel[0] = -tmp_accel[0];
-	m_accel[1] = tmp_accel[1];
-	m_accel[2] = -tmp_accel[2];
+	m_gyro[0] = -gyro[0];
+	m_gyro[1] = gyro[1];
+	m_gyro[2] = -gyro[2];
 
-	m_gyro[0] = -tmp_gyro[0];
-	m_gyro[1] = tmp_gyro[1];
-	m_gyro[2] = -tmp_gyro[2];
-
-	m_mag[0] = -tmp_mag[0];
-	m_mag[1] = tmp_mag[1];
-	m_mag[2] = -tmp_mag[2];
+	m_mag[0] = -mag[0];
+	m_mag[1] = mag[1];
+	m_mag[2] = -mag[2];
 #else
-	m_accel[0] = tmp_accel[0];
-	m_accel[1] = tmp_accel[1];
-	m_accel[2] = tmp_accel[2];
+	m_accel[0] = accel[0];
+	m_accel[1] = accel[1];
+	m_accel[2] = accel[2];
 
-	m_gyro[0] = tmp_gyro[0];
-	m_gyro[1] = tmp_gyro[1];
-	m_gyro[2] = tmp_gyro[2];
+	m_gyro[0] = gyro[0];
+	m_gyro[1] = gyro[1];
+	m_gyro[2] = gyro[2];
 
-	m_mag[0] = tmp_mag[0];
-	m_mag[1] = tmp_mag[1];
-	m_mag[2] = tmp_mag[2];
+	m_mag[0] = mag[0];
+	m_mag[1] = mag[1];
+	m_mag[2] = mag[2];
 #endif
 
 	float gyro_rad[3];
@@ -158,12 +185,7 @@ static void mpu_read_callback(void) {
 	gyro_rad[1] = m_gyro[1] * M_PI / 180.0;
 	gyro_rad[2] = m_gyro[2] * M_PI / 180.0;
 
-	if (!m_attitude_init_done) {
-		ahrs_update_initial_orientation(m_accel, m_mag, (ATTITUDE_INFO*)&m_att);
-		m_attitude_init_done = true;
-	} else {
-		ahrs_update_madgwick_imu(gyro_rad, m_accel, dt, (ATTITUDE_INFO*)&m_att);
-	}
+	ahrs_update_madgwick_imu(gyro_rad, m_accel, dt, (ATTITUDE_INFO*)&m_att);
 }
 
 static void terminal_rpy(int argc, const char **argv) {

imu/imu.h

@@ -22,9 +22,14 @@
 
 #include "ch.h"
 #include "hal.h"
+#include "i2c_bb.h"
 
 void imu_init(void);
-void imu_init_mpu9x50(stm32_gpio_t *sda_gpio, int sda_pin, stm32_gpio_t *scl_gpio, int scl_pin);
+i2c_bb_state *imu_get_i2c(void);
+void imu_init_mpu9x50(stm32_gpio_t *sda_gpio, int sda_pin,
+		stm32_gpio_t *scl_gpio, int scl_pin);
+void imu_init_icm20948(stm32_gpio_t *sda_gpio, int sda_pin,
+		stm32_gpio_t *scl_gpio, int scl_pin, int ad0_val);
 float imu_get_roll(void);
 float imu_get_pitch(void);
 float imu_get_yaw(void);

imu/imu.mk

@@ -1,4 +1,5 @@
 IMUSRC = 	imu/mpu9150.c \
+			imu/icm20948.c \
 			imu/ahrs.c \
 			imu/imu.c
 

imu/mpu9150.c

@@ -47,7 +47,6 @@
 // Private variables
 static unsigned char rx_buf[100];
 static unsigned char tx_buf[100];
-static THD_WORKING_AREA(mpu_thread_wa, 2048);
 static volatile int16_t raw_accel_gyro_mag[9];
 static volatile int16_t raw_accel_gyro_mag_no_offset[9];
 static volatile int failed_reads;
@@ -74,9 +73,12 @@ static void terminal_read_reg(int argc, const char **argv);
 static thread_t *mpu_tp = 0;
 
 // Function pointers
-static void(*read_callback)(void) = 0;
+static void(*read_callback)(float *accel, float *gyro, float *mag) = 0;
+
+void mpu9150_init(stm32_gpio_t *sda_gpio, int sda_pin,
+		stm32_gpio_t *scl_gpio, int scl_pin,
+		stkalign_t *work_area, size_t work_area_size) {
 
-void mpu9150_init(stm32_gpio_t *sda_gpio, int sda_pin, stm32_gpio_t *scl_gpio, int scl_pin) {
 	failed_reads = 0;
 	failed_mag_reads = 0;
 	read_callback = 0;
@@ -112,7 +114,7 @@ void mpu9150_init(stm32_gpio_t *sda_gpio, int sda_pin, stm32_gpio_t *scl_gpio, i
 	if (res == 0x68 || res == 0x69 || res == 0x71) {
 		mpu_found = true;
 		if (!mpu_tp) {
-			chThdCreateStatic(mpu_thread_wa, sizeof(mpu_thread_wa), NORMALPRIO, mpu_thread, NULL);
+			chThdCreateStatic(work_area, work_area_size, NORMALPRIO, mpu_thread, NULL);
 		}
 	} else {
 		mpu_found = false;
@@ -167,7 +169,7 @@ bool mpu9150_is_mpu9250(void) {
 	return is_mpu9250;
 }
 
-void mpu9150_set_read_callback(void(*func)(void)) {
+void mpu9150_set_read_callback(void(*func)(float *accel, float *gyro, float *mag)) {
 	read_callback = func;
 }
 
@@ -303,7 +305,9 @@ static THD_FUNCTION(mpu_thread, arg) {
 				last_update_time = chVTGetSystemTime();
 
 				if (read_callback) {
-					read_callback();
+					float tmp_accel[3], tmp_gyro[3], tmp_mag[3];
+					mpu9150_get_accel_gyro_mag(tmp_accel, tmp_gyro, tmp_mag);
+					read_callback(tmp_accel, tmp_gyro, tmp_mag);
 				}
 
 #if USE_MAGNETOMETER

imu/mpu9150.h

@@ -24,7 +24,9 @@
 #include "hal.h"
 
 // Functions
-void mpu9150_init(stm32_gpio_t *sda_gpio, int sda_pin, stm32_gpio_t *scl_gpio, int scl_pin);
+void mpu9150_init(stm32_gpio_t *sda_gpio, int sda_pin,
+		stm32_gpio_t *scl_gpio, int scl_pin,
+		stkalign_t *work_area, size_t work_area_size);
 bool mpu9150_is_mpu9250(void);
 void mpu9150_cmd_print(BaseSequentialStream *chp, int argc, char *argv[]);
 void mpu9150_cmd_sample_offsets(BaseSequentialStream *chp, int argc, char *argv[]);
@@ -34,7 +36,7 @@ void mpu9150_get_gyro(float *gyro);
 void mpu9150_get_mag(float *mag);
 void mpu9150_get_accel_gyro_mag(float *accel, float *gyro, float *mag);
 void mpu9150_sample_gyro_offsets(uint32_t iteratons);
-void mpu9150_set_read_callback(void(*func)(void));
+void mpu9150_set_read_callback(void(*func)(float *accel, float *gyro, float *mag));
 uint32_t mpu9150_get_time_since_update(void);
 float mpu9150_get_last_sample_duration(void);
 int mpu9150_get_failed_reads(void);

mc_interface.c

@@ -1812,23 +1812,26 @@ static THD_FUNCTION(timer_thread, arg) {
 				mc_interface_fault_stop(FAULT_CODE_ENCODER_SINCOS_ABOVE_MAX_AMPLITUDE);
 		}
 
-		int m_curr0_offset;
-		int m_curr1_offset;
-		int m_curr2_offset;
+		// TODO: Implement for BLDC and GPDRIVE
+		if(m_conf.motor_type == MOTOR_TYPE_FOC) {
+			int curr0_offset;
+			int curr1_offset;
+			int curr2_offset;
 
-		mcpwm_foc_get_current_offsets(&m_curr0_offset, &m_curr1_offset, &m_curr2_offset);
+			mcpwm_foc_get_current_offsets(&curr0_offset, &curr1_offset, &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);
-		}
+			if (abs(curr0_offset - 2048) > HW_MAX_CURRENT_OFFSET) {
+				mc_interface_fault_stop(FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_1);
+			}
+			if (abs(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);
-		}
+			if (abs(curr2_offset - 2048) > HW_MAX_CURRENT_OFFSET) {
+				mc_interface_fault_stop(FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_3);
+			}
 #endif
+		}
 
 		chThdSleepMilliseconds(1);
 	}

mcpwm_foc.c

@@ -1041,7 +1041,7 @@ float mcpwm_foc_get_vq(void) {
  * this is used by the virtual motor to save the current offsets,
  * when it is connected
  */
-void mcpwm_foc_get_current_offsets(volatile int *curr0_offset, volatile int *curr1_offset, volatile int *curr2_offset){
+void mcpwm_foc_get_current_offsets(volatile int *curr0_offset, volatile int *curr1_offset, volatile int *curr2_offset) {
 	*curr0_offset = m_curr0_offset;
 	*curr1_offset = m_curr1_offset;
 #ifdef HW_HAS_3_SHUNTS

terminal.c

@@ -438,6 +438,16 @@ void terminal_process_string(char *str) {
 				STM32_UUID_8[4], STM32_UUID_8[5], STM32_UUID_8[6], STM32_UUID_8[7],
 				STM32_UUID_8[8], STM32_UUID_8[9], STM32_UUID_8[10], STM32_UUID_8[11]);
 		commands_printf("Permanent NRF found: %s", conf_general_permanent_nrf_found ? "Yes" : "No");
+
+		int curr0_offset;
+		int curr1_offset;
+		int curr2_offset;
+
+		mcpwm_foc_get_current_offsets(&curr0_offset, &curr1_offset, &curr2_offset);
+
+		commands_printf("FOC Current Offsets: %d %d %d",
+				curr0_offset, curr1_offset, curr2_offset);
+
 		commands_printf(" ");
 	} else if (strcmp(argv[0], "foc_openloop") == 0) {
 		if (argc == 3) {