diff --git a/CHANGELOG b/CHANGELOG
index 4a0f7941..1f34ff33 100644
--- a/CHANGELOG
+++ b/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.
diff --git a/build_all/410_o_411_o_412/VESC_0005ohm.bin b/build_all/410_o_411_o_412/VESC_0005ohm.bin
index 2d22c5f3..67fdc98d 100755
Binary files a/build_all/410_o_411_o_412/VESC_0005ohm.bin and b/build_all/410_o_411_o_412/VESC_0005ohm.bin differ
diff --git a/build_all/410_o_411_o_412/VESC_default.bin b/build_all/410_o_411_o_412/VESC_default.bin
index 280ab2ee..03eca070 100755
Binary files a/build_all/410_o_411_o_412/VESC_default.bin and b/build_all/410_o_411_o_412/VESC_default.bin differ
diff --git a/build_all/410_o_411_o_412/VESC_default_no_hw_limits.bin b/build_all/410_o_411_o_412/VESC_default_no_hw_limits.bin
index 8cec04b7..9d10384c 100755
Binary files a/build_all/410_o_411_o_412/VESC_default_no_hw_limits.bin and b/build_all/410_o_411_o_412/VESC_default_no_hw_limits.bin differ
diff --git a/build_all/410_o_411_o_412/VESC_servoout.bin b/build_all/410_o_411_o_412/VESC_servoout.bin
index d8d5d249..60789727 100755
Binary files a/build_all/410_o_411_o_412/VESC_servoout.bin and b/build_all/410_o_411_o_412/VESC_servoout.bin differ
diff --git a/build_all/410_o_411_o_412/VESC_ws2811.bin b/build_all/410_o_411_o_412/VESC_ws2811.bin
index df10c26e..825ff681 100755
Binary files a/build_all/410_o_411_o_412/VESC_ws2811.bin and b/build_all/410_o_411_o_412/VESC_ws2811.bin differ
diff --git a/build_all/46_o_47/VESC_0005ohm.bin b/build_all/46_o_47/VESC_0005ohm.bin
index c74b7e31..0e4b3b36 100755
Binary files a/build_all/46_o_47/VESC_0005ohm.bin and b/build_all/46_o_47/VESC_0005ohm.bin differ
diff --git a/build_all/46_o_47/VESC_33k.bin b/build_all/46_o_47/VESC_33k.bin
index 7eb34a85..d82bc193 100755
Binary files a/build_all/46_o_47/VESC_33k.bin and b/build_all/46_o_47/VESC_33k.bin differ
diff --git a/build_all/46_o_47/VESC_default.bin b/build_all/46_o_47/VESC_default.bin
index 1aaac897..c01b1b4c 100755
Binary files a/build_all/46_o_47/VESC_default.bin and b/build_all/46_o_47/VESC_default.bin differ
diff --git a/build_all/46_o_47/VESC_servoout.bin b/build_all/46_o_47/VESC_servoout.bin
index 85ef0a2d..c01197f1 100755
Binary files a/build_all/46_o_47/VESC_servoout.bin and b/build_all/46_o_47/VESC_servoout.bin differ
diff --git a/build_all/46_o_47/VESC_ws2811.bin b/build_all/46_o_47/VESC_ws2811.bin
index f7f9eca5..e873f7ad 100755
Binary files a/build_all/46_o_47/VESC_ws2811.bin and b/build_all/46_o_47/VESC_ws2811.bin differ
diff --git a/build_all/46_o_47/VESC_ws2811_33k.bin b/build_all/46_o_47/VESC_ws2811_33k.bin
index a63ae46c..a3c7f26a 100755
Binary files a/build_all/46_o_47/VESC_ws2811_33k.bin and b/build_all/46_o_47/VESC_ws2811_33k.bin differ
diff --git a/build_all/48/VESC_0005ohm.bin b/build_all/48/VESC_0005ohm.bin
index 6c355326..0318bb03 100755
Binary files a/build_all/48/VESC_0005ohm.bin and b/build_all/48/VESC_0005ohm.bin differ
diff --git a/build_all/48/VESC_default.bin b/build_all/48/VESC_default.bin
index 469b4951..874df9c1 100755
Binary files a/build_all/48/VESC_default.bin and b/build_all/48/VESC_default.bin differ
diff --git a/build_all/48/VESC_servoout.bin b/build_all/48/VESC_servoout.bin
index 430e1d44..f77413e0 100755
Binary files a/build_all/48/VESC_servoout.bin and b/build_all/48/VESC_servoout.bin differ
diff --git a/build_all/48/VESC_ws2811.bin b/build_all/48/VESC_ws2811.bin
index 4fda3f20..afb70610 100755
Binary files a/build_all/48/VESC_ws2811.bin and b/build_all/48/VESC_ws2811.bin differ
diff --git a/build_all/60/VESC_default.bin b/build_all/60/VESC_default.bin
index 4519dda0..7fabcf5c 100755
Binary files a/build_all/60/VESC_default.bin and b/build_all/60/VESC_default.bin differ
diff --git a/build_all/60/VESC_default_no_hw_limits.bin b/build_all/60/VESC_default_no_hw_limits.bin
index 561a46d1..dbf4b3cf 100755
Binary files a/build_all/60/VESC_default_no_hw_limits.bin and b/build_all/60/VESC_default_no_hw_limits.bin differ
diff --git a/build_all/60/VESC_servoout.bin b/build_all/60/VESC_servoout.bin
index 56367743..bc0c62b3 100755
Binary files a/build_all/60/VESC_servoout.bin and b/build_all/60/VESC_servoout.bin differ
diff --git a/build_all/60/VESC_ws2811.bin b/build_all/60/VESC_ws2811.bin
index cc70f28d..11b5a73f 100755
Binary files a/build_all/60/VESC_ws2811.bin and b/build_all/60/VESC_ws2811.bin differ
diff --git a/build_all/75_300/VESC_default.bin b/build_all/75_300/VESC_default.bin
index a225d67b..3840de46 100755
Binary files a/build_all/75_300/VESC_default.bin and b/build_all/75_300/VESC_default.bin differ
diff --git a/build_all/75_300/VESC_default_no_hw_limits.bin b/build_all/75_300/VESC_default_no_hw_limits.bin
index 71903908..3c61cda3 100755
Binary files a/build_all/75_300/VESC_default_no_hw_limits.bin and b/build_all/75_300/VESC_default_no_hw_limits.bin differ
diff --git a/build_all/75_300/VESC_servoout.bin b/build_all/75_300/VESC_servoout.bin
index cf27f3be..4860fd3d 100755
Binary files a/build_all/75_300/VESC_servoout.bin and b/build_all/75_300/VESC_servoout.bin differ
diff --git a/build_all/75_300/VESC_ws2811.bin b/build_all/75_300/VESC_ws2811.bin
index 39c1d8f0..92b81c7a 100755
Binary files a/build_all/75_300/VESC_ws2811.bin and b/build_all/75_300/VESC_ws2811.bin differ
diff --git a/build_all/75_300_R2/VESC_default.bin b/build_all/75_300_R2/VESC_default.bin
index 4df3c28e..5a51ad9c 100755
Binary files a/build_all/75_300_R2/VESC_default.bin and b/build_all/75_300_R2/VESC_default.bin differ
diff --git a/build_all/75_300_R2/VESC_default_no_hw_limits.bin b/build_all/75_300_R2/VESC_default_no_hw_limits.bin
index 4c00e232..3ac2ae65 100755
Binary files a/build_all/75_300_R2/VESC_default_no_hw_limits.bin and b/build_all/75_300_R2/VESC_default_no_hw_limits.bin differ
diff --git a/build_all/75_300_R2/VESC_servoout.bin b/build_all/75_300_R2/VESC_servoout.bin
index be60f09c..f3bf8b30 100755
Binary files a/build_all/75_300_R2/VESC_servoout.bin and b/build_all/75_300_R2/VESC_servoout.bin differ
diff --git a/build_all/75_300_R2/VESC_ws2811.bin b/build_all/75_300_R2/VESC_ws2811.bin
index e94698f3..4eae5614 100755
Binary files a/build_all/75_300_R2/VESC_ws2811.bin and b/build_all/75_300_R2/VESC_ws2811.bin differ
diff --git a/build_all/DAS_RS/VESC_default.bin b/build_all/DAS_RS/VESC_default.bin
index 43c2295b..45cac2c2 100755
Binary files a/build_all/DAS_RS/VESC_default.bin and b/build_all/DAS_RS/VESC_default.bin differ
diff --git a/build_all/PALTA/VESC_default.bin b/build_all/PALTA/VESC_default.bin
index 22aa7396..4b5c4328 100755
Binary files a/build_all/PALTA/VESC_default.bin and b/build_all/PALTA/VESC_default.bin differ
diff --git a/conf_general.c b/conf_general.c
index 6e040c21..e28085be 100644
--- a/conf_general.c
+++ b/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;
}
}
diff --git a/conf_general.h b/conf_general.h
index 0cdd7932..2d8b54b2 100644
--- a/conf_general.h
+++ b/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"
diff --git a/hwconf/hw_60.h b/hwconf/hw_60.h
index 045973c3..b43ad0eb 100644
--- a/hwconf/hw_60.h
+++ b/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]
diff --git a/hwconf/hw_binar_v1.c b/hwconf/hw_binar_v1.c
index c257a3f1..efb191b4 100644
--- a/hwconf/hw_binar_v1.c
+++ b/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
diff --git a/hwconf/hw_binar_v1.h b/hwconf/hw_binar_v1.h
index 8e558587..5e2d6bbd 100644
--- a/hwconf/hw_binar_v1.h
+++ b/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]
diff --git a/i2c_bb.c b/i2c_bb.c
index ea5d0a0d..2a4da471 100644
--- a/i2c_bb.c
+++ b/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;
}
diff --git a/i2c_bb.h b/i2c_bb.h
index 399be9d7..96332864 100644
--- a/i2c_bb.h
+++ b/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);
diff --git a/imu/icm20948.c b/imu/icm20948.c
new file mode 100644
index 00000000..d04ba571
--- /dev/null
+++ b/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 .
+ */
+
+#include "icm20948.h"
+#include "terminal.h"
+#include "commands.h"
+#include "utils.h"
+
+#include
+#include
+
+// 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", ®);
+
+ 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);
+ }
+}
diff --git a/imu/icm20948.h b/imu/icm20948.h
new file mode 100644
index 00000000..6e405282
--- /dev/null
+++ b/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 .
+ */
+
+#ifndef IMU_ICM20948_H_
+#define IMU_ICM20948_H_
+
+#include "ch.h"
+#include "hal.h"
+
+#include
+#include
+
+#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_ */
diff --git a/imu/imu.c b/imu/imu.c
index b5a84129..1f38580c 100644
--- a/imu/imu.c
+++ b/imu/imu.c
@@ -24,17 +24,20 @@
#include "timer.h"
#include "terminal.h"
#include "commands.h"
+#include "icm20948.h"
#include
#include
// 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) {
diff --git a/imu/imu.h b/imu/imu.h
index f00d84a4..1fa1932a 100644
--- a/imu/imu.h
+++ b/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);
diff --git a/imu/imu.mk b/imu/imu.mk
index 57f50038..9e3bc30b 100644
--- a/imu/imu.mk
+++ b/imu/imu.mk
@@ -1,4 +1,5 @@
IMUSRC = imu/mpu9150.c \
+ imu/icm20948.c \
imu/ahrs.c \
imu/imu.c
diff --git a/imu/mpu9150.c b/imu/mpu9150.c
index 31328c9e..2ddea1d6 100644
--- a/imu/mpu9150.c
+++ b/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
diff --git a/imu/mpu9150.h b/imu/mpu9150.h
index 84e84c1b..a05ae5e6 100644
--- a/imu/mpu9150.h
+++ b/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);
diff --git a/mc_interface.c b/mc_interface.c
index a0c44d8e..4f8e8a79 100644
--- a/mc_interface.c
+++ b/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);
}
diff --git a/mcpwm_foc.c b/mcpwm_foc.c
index d53afc57..8024530b 100644
--- a/mcpwm_foc.c
+++ b/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
diff --git a/terminal.c b/terminal.c
index c02f8cbb..12f5855e 100644
--- a/terminal.c
+++ b/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) {