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

Author: vedderb

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.

build_all/410_o_411_o_412/VESC_0005ohm.bin

@@ -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;
 		}
 	}

build_all/410_o_411_o_412/VESC_default.bin

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

build_all/410_o_411_o_412/VESC_default_no_hw_limits.bin

@@ -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]

build_all/410_o_411_o_412/VESC_servoout.bin

@@ -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

build_all/410_o_411_o_412/VESC_ws2811.bin

@@ -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]

build_all/46_o_47/VESC_0005ohm.bin

@@ -41,13 +41,16 @@ 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;
 	s->has_error = false;
 }
 
 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;
 }
 

build_all/46_o_47/VESC_33k.bin

@@ -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);

build_all/46_o_47/VESC_default.bin

@@ -0,0 +1,182 @@
+/*
+	Copyright 2019 Benjamin Vedder	[email protected]
+
+	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);
+	}
+}

build_all/46_o_47/VESC_servoout.bin

@@ -0,0 +1,52 @@
+/*
+	Copyright 2019 Benjamin Vedder	[email protected]
+
+	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_ */