From f6621e810d1434649f869acbc7777c6acdb3645e Mon Sep 17 00:00:00 2001 From: Esben Rasmussen Date: Mon, 29 Jun 2020 16:26:39 +0200 Subject: [PATCH] Added BMI160 SPI support --- Makefile | 1 + hwconf/hw_das_rs.h | 10 ++ imu/bmi160_wrapper.h | 1 + imu/imu.c | 235 +++++++++++++++++++++++++++++++------------ imu/imu.h | 8 +- spi_bb.c | 112 +++++++++++++++++++++ spi_bb.h | 50 +++++++++ 7 files changed, 354 insertions(+), 63 deletions(-) create mode 100644 spi_bb.c create mode 100644 spi_bb.h diff --git a/Makefile b/Makefile index ba8a7de6..1f636a06 100644 --- a/Makefile +++ b/Makefile @@ -155,6 +155,7 @@ CSRC = $(STARTUPSRC) \ confgenerator.c \ timer.c \ i2c_bb.c \ + spi_bb.c \ virtual_motor.c \ shutdown.c \ mempools.c \ diff --git a/hwconf/hw_das_rs.h b/hwconf/hw_das_rs.h index c0aeda1f..996f6016 100644 --- a/hwconf/hw_das_rs.h +++ b/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] diff --git a/imu/bmi160_wrapper.h b/imu/bmi160_wrapper.h index d3fb28e4..f91e2754 100644 --- a/imu/bmi160_wrapper.h +++ b/imu/bmi160_wrapper.h @@ -27,6 +27,7 @@ #include #include "i2c_bb.h" +#include "spi_bb.h" #include "bmi160.h" typedef struct { diff --git a/imu/imu.c b/imu/imu.c index c7006e63..5869c3d0 100644 --- a/imu/imu.c +++ b/imu/imu.c @@ -36,6 +36,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; @@ -49,6 +50,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; @@ -69,34 +72,46 @@ void imu_init(imu_config *set) { if (set->type == IMU_TYPE_INTERNAL) { #ifdef MPU9X50_SDA_GPIO imu_init_mpu9x50(MPU9X50_SDA_GPIO, MPU9X50_SDA_PIN, - MPU9X50_SCL_GPIO, MPU9X50_SCL_PIN); + 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); + ICM20948_SCL_GPIO, ICM20948_SCL_PIN, ICM20948_AD0_VAL); #endif #ifdef BMI160_SDA_GPIO - imu_init_bmi160(BMI160_SDA_GPIO, BMI160_SDA_PIN, - BMI160_SCL_GPIO, BMI160_SCL_PIN); + imu_init_bmi160_i2c(BMI160_SDA_GPIO, BMI160_SDA_PIN, + BMI160_SCL_GPIO, BMI160_SCL_PIN); #endif - } else if (set->type == IMU_TYPE_EXTERNAL_MPU9X50) { + +#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); - } else if (set->type == IMU_TYPE_EXTERNAL_ICM20948) { + HW_I2C_SCL_PORT, HW_I2C_SCL_PIN); + } + else if (set->type == IMU_TYPE_EXTERNAL_ICM20948) { 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, - HW_I2C_SCL_PORT, HW_I2C_SCL_PIN); + HW_I2C_SCL_PORT, HW_I2C_SCL_PIN, 0); + } + 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( - "imu_gyro_info", - "Print gyro offsets", - 0, - terminal_gyro_info); + "imu_gyro_info", + "Print gyro offsets", + 0, + terminal_gyro_info); } i2c_bb_state *imu_get_i2c(void) { @@ -104,17 +119,18 @@ 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) { + stm32_gpio_t *scl_gpio, int scl_pin) { imu_stop(); mpu9150_init(sda_gpio, sda_pin, - scl_gpio, scl_pin, - m_thd_work_area, sizeof(m_thd_work_area)); + 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) { + stm32_gpio_t *scl_gpio, int scl_pin, + int ad0_val) { imu_stop(); m_i2c_bb.sda_gpio = sda_gpio; @@ -124,13 +140,14 @@ void imu_init_icm20948(stm32_gpio_t *sda_gpio, int sda_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)); + &m_i2c_bb, ad0_val, + m_thd_work_area, sizeof(m_thd_work_area)); icm20948_set_read_callback(&m_icm20948_state, imu_read_callback); } -void imu_init_bmi160(stm32_gpio_t *sda_gpio, int sda_pin, - stm32_gpio_t *scl_gpio, int scl_pin) { +void imu_init_bmi160_i2c(stm32_gpio_t *sda_gpio, int sda_pin, + stm32_gpio_t *scl_gpio, int scl_pin) +{ imu_stop(); m_i2c_bb.sda_gpio = sda_gpio; @@ -148,7 +165,37 @@ 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_stop(void) { +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_stop(void) +{ mpu9150_stop(); icm20948_stop(&m_icm20948_state); bmi160_wrapper_stop(&m_bmi_state); @@ -219,7 +266,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; @@ -259,8 +306,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}; - for(int i = 0; i < 1000; i++){ + float original_gyro_offsets[3] = {0, 0, 0}; + 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]; @@ -280,14 +327,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}; @@ -298,14 +345,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}; @@ -338,10 +385,10 @@ void imu_get_calibration(float yaw, float *imu_cal){ m_settings.sample_rate_hz = backup_sample_rate; m_settings.mode = backup_ahrs_mode; ahrs_update_all_parameters( - m_settings.accel_confidence_decay, - m_settings.mahony_kp, - m_settings.mahony_ki, - m_settings.madgwick_beta); + m_settings.accel_confidence_decay, + m_settings.mahony_kp, + m_settings.mahony_ki, + m_settings.madgwick_beta); m_settings.rot_roll = backup_roll; m_settings.rot_pitch = backup_pitch; m_settings.rot_yaw = backup_yaw; @@ -356,17 +403,18 @@ void imu_get_calibration(float yaw, float *imu_cal){ m_settings.gyro_offset_comp_fact[2] = backup_gyro_comp_z; } -static void imu_read_callback(float *accel, float *gyro, float *mag) { +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(); - 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, - m_settings.mahony_ki, - m_settings.madgwick_beta); + m_settings.accel_confidence_decay, + m_settings.mahony_kp, + m_settings.mahony_ki, + m_settings.madgwick_beta); imu_ready = true; } @@ -397,9 +445,15 @@ static void imu_read_callback(float *accel, float *gyro, float *mag) { float s3 = sinf(m_settings.rot_roll * M_PI / 180.0); float c3 = cosf(m_settings.rot_roll * M_PI / 180.0); - float m11 = c1 * c2; float m12 = c1 * s2 * s3 - c3 * s1; float m13 = s1 * s3 + c1 * c3 * s2; - float m21 = c2 * s1; float m22 = c1 * c3 + s1 * s2 * s3; float m23 = c3 * s1 * s2 - c1 * s3; - float m31 = -s2; float m32 = c2 * s3; float m33 = c2 * c3; + float m11 = c1 * c2; + float m12 = c1 * s2 * s3 - c3 * s1; + float m13 = s1 * s3 + c1 * c3 * s2; + float m21 = c2 * s1; + float m22 = c1 * c3 + s1 * s2 * s3; + float m23 = c3 * s1 * s2 - c1 * s3; + float m31 = -s2; + float m32 = c2 * s3; + float m33 = c2 * c3; m_accel[0] = accel[0] * m11 + accel[1] * m12 + accel[2] * m13; m_accel[1] = accel[0] * m21 + accel[1] * m22 + accel[2] * m23; @@ -414,14 +468,15 @@ 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]; if (m_settings.gyro_offset_comp_fact[i] > 0.0) { utils_step_towards(&m_gyro_offset[i], m_gyro[i], m_settings.gyro_offset_comp_fact[i] * dt); utils_truncate_number_abs(&m_gyro_offset[i], m_settings.gyro_offset_comp_clamp); - } else { + } + else { m_gyro_offset[i] = 0.0; } @@ -433,13 +488,13 @@ 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){ - case (AHRS_MODE_MADGWICK): - 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); - break; + switch (m_settings.mode) { + case (AHRS_MODE_MADGWICK): + 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); + break; } } @@ -448,12 +503,12 @@ static void terminal_gyro_info(int argc, const char **argv) { (void)argv; commands_printf("Gyro offsets: [%.3f %.3f %.3f]\n", - (double)(m_settings.gyro_offsets[0] + m_gyro_offset[0]), - (double)(m_settings.gyro_offsets[1] + m_gyro_offset[1]), - (double)(m_settings.gyro_offsets[2] + m_gyro_offset[2])); + (double)(m_settings.gyro_offsets[0] + m_gyro_offset[0]), + (double)(m_settings.gyro_offsets[1] + m_gyro_offset[1]), + (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); @@ -462,9 +517,15 @@ void rotate(float *input, float *rotation, float *output){ float s3 = sinf(rotation[0] * M_PI / 180.0); float c3 = cosf(rotation[0] * M_PI / 180.0); - float m11 = c1 * c2; float m12 = c1 * s2 * s3 - c3 * s1; float m13 = s1 * s3 + c1 * c3 * s2; - float m21 = c2 * s1; float m22 = c1 * c3 + s1 * s2 * s3; float m23 = c3 * s1 * s2 - c1 * s3; - float m31 = -s2; float m32 = c2 * s3; float m33 = c2 * c3; + float m11 = c1 * c2; + float m12 = c1 * s2 * s3 - c3 * s1; + float m13 = s1 * s3 + c1 * c3 * s2; + float m21 = c2 * s1; + float m22 = c1 * c3 + s1 * s2 * s3; + float m23 = c3 * s1 * s2 - c1 * s3; + float m31 = -s2; + float m32 = c2 * s3; + float m33 = c2 * c3; output[0] = input[0] * m11 + input[1] * m12 + input[2] * m13; output[1] = input[0] * m21 + input[1] * m22 + input[2] * m23; @@ -487,3 +548,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; +} diff --git a/imu/imu.h b/imu/imu.h index e7e315ff..05f5882f 100644 --- a/imu/imu.h +++ b/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,8 +31,13 @@ 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_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); diff --git a/spi_bb.c b/spi_bb.c new file mode 100644 index 00000000..f08aedea --- /dev/null +++ b/spi_bb.c @@ -0,0 +1,112 @@ +/* + 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 "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(); + } +} diff --git a/spi_bb.h b/spi_bb.h new file mode 100644 index 00000000..3673f355 --- /dev/null +++ b/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 . + */ + +#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_ */