Merge pull request #190 from EsbenFR/BMI160_SPI

Bmi160 spi support

Makefile

@@ -155,6 +155,7 @@ CSRC = $(STARTUPSRC) \
        confgenerator.c \
        timer.c \
        i2c_bb.c \
+       spi_bb.c \
        virtual_motor.c \
        shutdown.c \
        mempools.c \

hwconf/hw_das_rs.h

@@ -201,6 +201,16 @@
 #define HW_SPI_PORT_MISO		GPIOA
 #define HW_SPI_PIN_MISO			6
 
+//BMI160 SPI pins
+#define BMI160_SPI_PORT_NSS		GPIOC
+#define BMI160_SPI_PIN_NSS		9
+#define BMI160_SPI_PORT_SCK		GPIOC
+#define BMI160_SPI_PIN_SCK		10
+#define BMI160_SPI_PORT_MOSI	GPIOC
+#define BMI160_SPI_PIN_MOSI		12
+#define BMI160_SPI_PORT_MISO	GPIOC
+#define BMI160_SPI_PIN_MISO		11								
+
 // Measurement macros
 #define ADC_V_L1				ADC_Value[ADC_IND_SENS1]
 #define ADC_V_L2				ADC_Value[ADC_IND_SENS2]

imu/bmi160_wrapper.h

@@ -27,6 +27,7 @@
 #include <stdbool.h>
 
 #include "i2c_bb.h"
+#include "spi_bb.h"
 #include "bmi160.h"
 
 typedef struct {

imu/imu.c

@@ -37,6 +37,7 @@ static ATTITUDE_INFO m_att;
 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 spi_bb_state m_spi_bb;
 static ICM20948_STATE m_icm20948_state;
 static BMI_STATE m_bmi_state;
 static imu_config m_settings;
@@ -50,6 +51,8 @@ static void terminal_gyro_info(int argc, const char **argv);
 static void rotate(float *input, float *rotation, float *output);
 int8_t user_i2c_read(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len);
 int8_t user_i2c_write(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len);
+int8_t user_spi_read(uint8_t dev_id, uint8_t reg_addr, uint8_t *data, uint16_t len);
+int8_t user_spi_write(uint8_t dev_id, uint8_t reg_addr, uint8_t *data, uint16_t len);
 
 void imu_init(imu_config *set) {
 	m_settings = *set;
@@ -80,7 +83,7 @@ void imu_init(imu_config *set) {
 #endif
 
 #ifdef BMI160_SDA_GPIO
-		imu_init_bmi160(BMI160_SDA_GPIO, BMI160_SDA_PIN,
+		imu_init_bmi160_i2c(BMI160_SDA_GPIO, BMI160_SDA_PIN,
 				BMI160_SCL_GPIO, BMI160_SCL_PIN);
 #endif
 
@@ -88,6 +91,14 @@ void imu_init(imu_config *set) {
 		imu_init_lsm6ds3(LSM6DS3_SDA_GPIO, LSM6DS3_SDA_PIN,
 				LSM6DS3_SCL_GPIO, LSM6DS3_SCL_PIN);
 #endif
+
+#ifdef BMI160_SPI_PORT_NSS
+		imu_init_bmi160_spi(
+				BMI160_SPI_PORT_NSS, BMI160_SPI_PIN_NSS,
+				BMI160_SPI_PORT_SCK, BMI160_SPI_PIN_SCK,
+				BMI160_SPI_PORT_MOSI, BMI160_SPI_PIN_MOSI,
+				BMI160_SPI_PORT_MISO, BMI160_SPI_PIN_MISO);
+#endif
 	} else if (set->type == IMU_TYPE_EXTERNAL_MPU9X50) {
 		imu_init_mpu9x50(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
 				HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
@@ -95,11 +106,14 @@ void imu_init(imu_config *set) {
 		imu_init_icm20948(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
 				HW_I2C_SCL_PORT, HW_I2C_SCL_PIN, 0);
 	} else if (set->type == IMU_TYPE_EXTERNAL_BMI160) {
-		imu_init_bmi160(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
+		imu_init_bmi160_i2c(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
 				HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
-	}else if(set->type == IMU_TYPE_EXTERNAL_LSM6DS3){
+	} else if(set->type == IMU_TYPE_EXTERNAL_LSM6DS3) {
 		imu_init_lsm6ds3(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
 				HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
+	} else if (set->type == IMU_TYPE_EXTERNAL_BMI160) {
+		imu_init_bmi160_i2c(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
+				HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
 	}
 
 	terminal_register_command_callback(
@@ -139,7 +153,7 @@ void imu_init_icm20948(stm32_gpio_t *sda_gpio, int sda_pin,
 	icm20948_set_read_callback(&m_icm20948_state, imu_read_callback);
 }
 
-void imu_init_bmi160(stm32_gpio_t *sda_gpio, int sda_pin,
+void imu_init_bmi160_i2c(stm32_gpio_t *sda_gpio, int sda_pin,
 		stm32_gpio_t *scl_gpio, int scl_pin) {
 	imu_stop();
 
@@ -158,6 +172,32 @@ void imu_init_bmi160(stm32_gpio_t *sda_gpio, int sda_pin,
 	bmi160_wrapper_set_read_callback(&m_bmi_state, imu_read_callback);
 }
 
+void imu_init_bmi160_spi(stm32_gpio_t *nss_gpio, int nss_pin,
+		stm32_gpio_t *sck_gpio, int sck_pin, stm32_gpio_t *mosi_gpio, int mosi_pin,
+		stm32_gpio_t *miso_gpio, int miso_pin) {
+	imu_stop();
+
+	m_spi_bb.nss_gpio = nss_gpio;
+	m_spi_bb.nss_pin = nss_pin;
+	m_spi_bb.sck_gpio = sck_gpio;
+	m_spi_bb.sck_pin = sck_pin;
+	m_spi_bb.mosi_gpio = mosi_gpio;
+	m_spi_bb.mosi_pin = mosi_pin;
+	m_spi_bb.miso_gpio = miso_gpio;
+	m_spi_bb.miso_pin = miso_pin;
+
+	spi_bb_init(&m_spi_bb);
+
+	m_bmi_state.sensor.id = 0;
+	m_bmi_state.sensor.interface = BMI160_SPI_INTF;
+	m_bmi_state.sensor.read = user_spi_read;
+	m_bmi_state.sensor.write = user_spi_write;
+
+	bmi160_wrapper_init(&m_bmi_state, m_thd_work_area, sizeof(m_thd_work_area));
+
+	bmi160_wrapper_set_read_callback(&m_bmi_state, imu_read_callback);
+}
+
 void imu_init_lsm6ds3(stm32_gpio_t *sda_gpio, int sda_pin,
 		stm32_gpio_t *scl_gpio, int scl_pin) {
 
@@ -240,7 +280,7 @@ void imu_get_quaternions(float *q) {
 	q[3] = m_att.q3;
 }
 
-void imu_get_calibration(float yaw, float *imu_cal){
+void imu_get_calibration(float yaw, float *imu_cal) {
 	// Backup current settings
 	float backup_sample_rate = m_settings.sample_rate_hz;
 	AHRS_MODE backup_ahrs_mode = m_settings.mode;
@@ -280,8 +320,8 @@ void imu_get_calibration(float yaw, float *imu_cal){
 	m_gyro_offset[2] = 0;
 
 	// Sample gyro for offsets
-	float original_gyro_offsets[3] = {0,0,0};
+	float original_gyro_offsets[3] = {0, 0, 0};
-	for(int i = 0; i < 1000; i++){
+	for (int i = 0; i < 1000; i++) {
 		original_gyro_offsets[0] += m_gyro[0];
 		original_gyro_offsets[1] += m_gyro[1];
 		original_gyro_offsets[2] += m_gyro[2];
@@ -302,14 +342,14 @@ void imu_get_calibration(float yaw, float *imu_cal){
 
 	// Sample roll
 	float roll_sample = 0;
-	for(int i = 0; i < 250; i++){
+	for (int i = 0; i < 250; i++) {
 		roll_sample += imu_get_roll();
 		chThdSleepMilliseconds(1);
 	}
-	roll_sample = roll_sample/250;
+	roll_sample = roll_sample / 250;
 
 	// Set roll rotations to level out roll axis
-	m_settings.rot_roll = -roll_sample * (180/M_PI);
+	m_settings.rot_roll = -roll_sample * (180 / M_PI);
 
 	// Rotate gyro offsets to match new IMU orientation
 	float rotation1[3] = {m_settings.rot_roll, m_settings.rot_pitch, m_settings.rot_yaw};
@@ -321,14 +361,14 @@ void imu_get_calibration(float yaw, float *imu_cal){
 
 	// Sample pitch
 	float pitch_sample = 0;
-	for(int i = 0; i < 250; i++){
+	for (int i = 0; i < 250; i++) {
 		pitch_sample += imu_get_pitch();
 		chThdSleepMilliseconds(1);
 	}
-	pitch_sample = pitch_sample/250;
+	pitch_sample = pitch_sample / 250;
 
 	// Set pitch rotation to level out pitch axis
-	m_settings.rot_pitch = pitch_sample * (180/M_PI);
+	m_settings.rot_pitch = pitch_sample * (180 / M_PI);
 
 	// Rotate imu offsets to match
 	float rotation2[3] = {m_settings.rot_roll, m_settings.rot_pitch, m_settings.rot_yaw};
@@ -385,7 +425,7 @@ static void imu_read_callback(float *accel, float *gyro, float *mag) {
 	float dt = timer_seconds_elapsed_since(last_time);
 	last_time = timer_time_now();
 
-	if(!imu_ready && ST2MS(chVTGetSystemTimeX() - init_time) > 1000){
+	if (!imu_ready && ST2MS(chVTGetSystemTimeX() - init_time) > 1000) {
 		ahrs_update_all_parameters(
 				m_settings.accel_confidence_decay,
 				m_settings.mahony_kp,
@@ -438,7 +478,7 @@ static void imu_read_callback(float *accel, float *gyro, float *mag) {
 	m_mag[2] = mag[0] * m31 + mag[1] * m32 + mag[2] * m33;
 
 	// Accelerometer and Gyro offset compensation and estimation
-	for (int i = 0;i < 3;i++) {
+	for (int i = 0; i < 3; i++) {
 		m_accel[i] -= m_settings.accel_offsets[i];
 		m_gyro[i] -= m_settings.gyro_offsets[i];
 
@@ -457,12 +497,12 @@ static void imu_read_callback(float *accel, float *gyro, float *mag) {
 	gyro_rad[1] = m_gyro[1] * M_PI / 180.0;
 	gyro_rad[2] = m_gyro[2] * M_PI / 180.0;
 
-	switch (m_settings.mode){
+	switch (m_settings.mode) {
 		case (AHRS_MODE_MADGWICK):
-			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);
 			break;
 		case (AHRS_MODE_MAHONY):
-			ahrs_update_mahony_imu(gyro_rad, m_accel, dt, (ATTITUDE_INFO*)&m_att);
+			ahrs_update_mahony_imu(gyro_rad, m_accel, dt, (ATTITUDE_INFO *)&m_att);
 			break;
 	}
 }
@@ -477,7 +517,7 @@ static void terminal_gyro_info(int argc, const char **argv) {
 			(double)(m_settings.gyro_offsets[2] + m_gyro_offset[2]));
 }
 
-void rotate(float *input, float *rotation, float *output){
+void rotate(float *input, float *rotation, float *output) {
 	// Rotate imu offsets to match
 	float s1 = sinf(rotation[2] * M_PI / 180.0);
 	float c1 = cosf(rotation[2] * M_PI / 180.0);
@@ -511,3 +551,53 @@ int8_t user_i2c_write(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_
 	memcpy(txbuf + 1, data, len);
 	return i2c_bb_tx_rx(&m_i2c_bb, dev_addr, txbuf, len + 1, 0, 0) ? BMI160_OK : BMI160_E_COM_FAIL;
 }
+
+int8_t user_spi_read(uint8_t dev_id, uint8_t reg_addr, uint8_t *data, uint16_t len) {
+	//commands_printf("user spi reading");
+	
+	int8_t rslt = BMI160_OK; // Return 0 for Success, non-zero for failure 
+
+	//reg_addr = BMI160_CHIP_ID_ADDR; 
+	//len = 1;
+	reg_addr = (reg_addr | BMI160_SPI_RD_MASK);
+
+	chThdSleepMicroseconds(200); // #FIXME Wont work without this- Why?
+
+
+	chMtxLock(&m_spi_bb.mutex);
+	spi_begin(&m_spi_bb);
+	//spi_exchange_8(reg_addr);
+	spi_exchange_8(&m_spi_bb, reg_addr);
+	spi_delay();
+	for (int i = 0; i < len; i++)
+	{
+		//data[i] = spi_exchange_8(0);
+		data[i] = spi_exchange_8(&m_spi_bb, 0);;
+	}
+
+	spi_end(&m_spi_bb);
+	chMtxUnlock(&m_spi_bb.mutex);
+	return rslt;
+}
+
+int8_t user_spi_write(uint8_t dev_id, uint8_t reg_addr, uint8_t *data, uint16_t len) {
+
+	int8_t rslt = BMI160_OK; /* Return 0 for Success, non-zero for failure */
+	chMtxLock(&m_spi_bb.mutex);
+	spi_begin(&m_spi_bb);
+	reg_addr = (reg_addr & BMI160_SPI_WR_MASK);
+	//spi_exchange_8(reg_addr);
+	spi_exchange_8(&m_spi_bb, reg_addr);
+	spi_delay();
+	for (int i = 0; i < len; i++)
+	{
+
+		//spi_exchange_8(*data);
+		spi_exchange_8(&m_spi_bb, *data);
+		data++;
+	}
+	spi_end(&m_spi_bb);
+	chMtxUnlock(&m_spi_bb.mutex);
+
+	return rslt;
+}

imu/imu.h

@@ -23,6 +23,7 @@
 #include "ch.h"
 #include "hal.h"
 #include "i2c_bb.h"
+#include "spi_bb.h"
 
 void imu_init(imu_config *set);
 i2c_bb_state *imu_get_i2c(void);
@@ -30,10 +31,15 @@ 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);
-void imu_init_bmi160(stm32_gpio_t *sda_gpio, int sda_pin,
+void imu_init_bmi160_i2c(stm32_gpio_t *sda_gpio, int sda_pin,
 		stm32_gpio_t *scl_gpio, int scl_pin);
 void imu_init_lsm6ds3(stm32_gpio_t *sda_gpio, int sda_pin,
 		stm32_gpio_t *scl_gpio, int scl_pin);
+void imu_init_bmi160_spi(
+        stm32_gpio_t *nss_gpio, int nss_pin,
+        stm32_gpio_t *sck_gpio, int sck_pin,
+        stm32_gpio_t *mosi_gpio, int mosi_pin,
+        stm32_gpio_t *miso_gpio, int miso_pin);
 void imu_stop(void);
 bool imu_startup_done(void);
 float imu_get_roll(void);

spi_bb.c

@@ -0,0 +1,102 @@
+/*
+	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 "spi_bb.h"
+#include "timer.h"
+
+// Software SPI
+
+void spi_bb_init(spi_bb_state *s) {
+	chMtxObjectInit(&s->mutex);
+
+	palSetPadMode(s->miso_gpio, s->miso_pin, PAL_MODE_INPUT_PULLUP);
+	palSetPadMode(s->sck_gpio, s->sck_pin, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+	palSetPadMode(s->nss_gpio, s->nss_pin, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+	palSetPadMode(s->mosi_gpio, s->mosi_pin, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+	palSetPad(s->mosi_gpio, s->mosi_pin);
+	palSetPad(s->nss_gpio, s->nss_pin);
+
+	s->has_started = false;
+	s->has_error = false;
+}
+
+
+uint8_t spi_exchange_8(spi_bb_state *s, uint8_t x) {
+	uint8_t rx;
+	spi_transfer_8(s ,&rx, &x, 1);
+	return rx;
+}
+
+void spi_transfer_8(spi_bb_state *s, uint8_t *in_buf, const uint8_t *out_buf, int length) {
+	for (int i = 0; i < length; i++) {
+		uint8_t send = out_buf ? out_buf[i] : 0xFF;
+		uint8_t receive = 0;
+
+		for (int bit = 0; bit < 8; bit++) {
+				palWritePad(s->mosi_gpio, s->mosi_pin, send >> 7);
+			send <<= 1;
+
+			palSetPad(s->sck_gpio, s->sck_pin);
+			spi_delay();	
+
+			int samples = 0;
+			samples += palReadPad(s->miso_gpio, s->miso_pin);
+			__NOP();
+			samples += palReadPad(s->miso_gpio, s->miso_pin);
+			__NOP();
+			samples += palReadPad(s->miso_gpio, s->miso_pin);	
+			__NOP();
+			samples += palReadPad(s->miso_gpio, s->miso_pin);
+			__NOP();
+			samples += palReadPad(s->miso_gpio, s->miso_pin);
+			
+			palClearPad(s->sck_gpio, s->sck_pin);
+
+			// does 5 samples of each pad read, to minimize noise
+			receive <<= 1;
+			if (samples > 2) {
+				receive |= 1;
+			}
+
+			spi_delay();
+		}
+
+		if (in_buf)	{
+			in_buf[i] = receive;
+		}
+	}
+}
+
+void spi_begin(spi_bb_state *s) {
+	spi_delay();
+	palClearPad(s->nss_gpio, s->nss_pin);
+	spi_delay();
+}
+
+void spi_end(spi_bb_state *s) {
+	spi_delay();
+	palSetPad(s->nss_gpio, s->nss_pin);
+	spi_delay();
+}
+
+void spi_delay(void) {
+	for (volatile int i = 0; i < 40; i++) {
+		__NOP();
+	}
+}

spi_bb.h

@@ -0,0 +1,50 @@
+/*
+	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 SPI_BB_H_
+#define SPI_BB_H_
+
+#include "ch.h"
+#include "hal.h"
+#include "stdint.h"
+#include "stdbool.h"
+
+typedef struct {
+	stm32_gpio_t *nss_gpio;
+	int nss_pin;
+	stm32_gpio_t *sck_gpio;
+	int sck_pin;
+    stm32_gpio_t *mosi_gpio;
+	int mosi_pin;
+    stm32_gpio_t *miso_gpio;
+	int miso_pin;
+	bool has_started;
+	bool has_error;
+	mutex_t mutex;
+} spi_bb_state;
+
+void spi_bb_init(spi_bb_state *s);
+uint8_t spi_exchange_8(spi_bb_state *s, uint8_t x);
+void spi_transfer_8(spi_bb_state *s, uint8_t *in_buf, const uint8_t *out_buf, int length);
+
+void spi_begin(spi_bb_state *s);
+void spi_end(spi_bb_state *s);
+void spi_delay(void);
+
+#endif /* SPI_BB_H_ */