From d89f652f7d4b7b4bd84a49b851b72483244d4aae Mon Sep 17 00:00:00 2001 From: Maximiliano Cordoba Date: Sat, 2 May 2020 17:12:22 -0300 Subject: [PATCH 1/4] Add ramp input to the speed PID loop. This works together with a new config file of Vesc Tool, that adds 2 variables to the Speed Pid page. Signed-off-by: Maximiliano Cordoba --- confgenerator.c | 6 ++++++ confgenerator.h | 2 +- datatypes.h | 2 ++ mcconf/mcconf_default.h | 6 ++++++ mcpwm_foc.c | 17 ++++++++++++++++- 5 files changed, 31 insertions(+), 2 deletions(-) diff --git a/confgenerator.c b/confgenerator.c index 032dbd6f..95a234a0 100644 --- a/confgenerator.c +++ b/confgenerator.c @@ -118,6 +118,8 @@ int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration * buffer_append_float32_auto(buffer, conf->s_pid_kd_filter, &ind); buffer_append_float32_auto(buffer, conf->s_pid_min_erpm, &ind); buffer[ind++] = conf->s_pid_allow_braking; + buffer[ind++] = conf->s_pid_apply_input_ramp; + buffer_append_float32_auto(buffer, conf->s_pid_ramp_erpms_ms, &ind); buffer_append_float32_auto(buffer, conf->p_pid_kp, &ind); buffer_append_float32_auto(buffer, conf->p_pid_ki, &ind); buffer_append_float32_auto(buffer, conf->p_pid_kd, &ind); @@ -410,6 +412,8 @@ bool confgenerator_deserialize_mcconf(const uint8_t *buffer, mc_configuration *c conf->s_pid_kd_filter = buffer_get_float32_auto(buffer, &ind); conf->s_pid_min_erpm = buffer_get_float32_auto(buffer, &ind); conf->s_pid_allow_braking = buffer[ind++]; + conf->s_pid_apply_input_ramp = buffer[ind++]; + conf->s_pid_ramp_erpms_ms = buffer_get_float32_auto(buffer, &ind); conf->p_pid_kp = buffer_get_float32_auto(buffer, &ind); conf->p_pid_ki = buffer_get_float32_auto(buffer, &ind); conf->p_pid_kd = buffer_get_float32_auto(buffer, &ind); @@ -698,6 +702,8 @@ void confgenerator_set_defaults_mcconf(mc_configuration *conf) { conf->s_pid_kd_filter = MCCONF_S_PID_KD_FILTER; conf->s_pid_min_erpm = MCCONF_S_PID_MIN_RPM; conf->s_pid_allow_braking = MCCONF_S_PID_ALLOW_BRAKING; + conf->s_pid_apply_input_ramp = MCCONF_S_PID_APPLY_INPUT_RAMP; + conf->s_pid_ramp_erpms_ms = MCCONF_S_PID_RAMP_ERPMS_MS; conf->p_pid_kp = MCCONF_P_PID_KP; conf->p_pid_ki = MCCONF_P_PID_KI; conf->p_pid_kd = MCCONF_P_PID_KD; diff --git a/confgenerator.h b/confgenerator.h index 0385c4fd..49785c4e 100644 --- a/confgenerator.h +++ b/confgenerator.h @@ -8,7 +8,7 @@ #include // Constants -#define MCCONF_SIGNATURE 3698540221 +#define MCCONF_SIGNATURE 2111869092 #define APPCONF_SIGNATURE 2460147246 // Functions diff --git a/datatypes.h b/datatypes.h index a170f056..f8fa5d52 100644 --- a/datatypes.h +++ b/datatypes.h @@ -321,6 +321,8 @@ typedef struct { float s_pid_kd_filter; float s_pid_min_erpm; bool s_pid_allow_braking; + bool s_pid_apply_input_ramp; + float s_pid_ramp_erpms_ms; // Pos PID float p_pid_kp; float p_pid_ki; diff --git a/mcconf/mcconf_default.h b/mcconf/mcconf_default.h index fbd4337c..8f1b6543 100644 --- a/mcconf/mcconf_default.h +++ b/mcconf/mcconf_default.h @@ -136,6 +136,12 @@ #ifndef MCCONF_S_PID_ALLOW_BRAKING #define MCCONF_S_PID_ALLOW_BRAKING true // Allow braking in speed control mode #endif +#ifndef MCCONF_S_PID_APPLY_INPUT_RAMP +#define MCCONF_S_PID_APPLY_INPUT_RAMP false // Apply Input Ramp in speed control mode +#endif +#ifndef MCCONF_S_PID_RAMP_ERPMS_MS +#define MCCONF_S_PID_RAMP_ERPMS_MS 1.0 // Default Speed Input Ramp +#endif // Position PID parameters #ifndef MCCONF_P_PID_KP diff --git a/mcpwm_foc.c b/mcpwm_foc.c index c7154bfd..6510e78e 100644 --- a/mcpwm_foc.c +++ b/mcpwm_foc.c @@ -111,6 +111,7 @@ typedef struct { bool m_output_on; float m_pos_pid_set; float m_speed_pid_set_rpm; + float m_speed_command_rpm; float m_phase_now_observer; float m_phase_now_observer_override; bool m_phase_observer_override; @@ -165,6 +166,7 @@ typedef struct { float m_hall_dt_diff_now; } motor_all_state_t; + // Private variables static volatile bool m_dccal_done = false; static volatile float m_last_adc_isr_duration; @@ -798,8 +800,17 @@ void mcpwm_foc_set_duty_noramp(float dutyCycle) { * The electrical RPM goal value to use. */ void mcpwm_foc_set_pid_speed(float rpm) { + if( motor_now()->m_conf->s_pid_apply_input_ramp ){ + if( motor_now()->m_control_mode != CONTROL_MODE_SPEED || + motor_now()->m_state != MC_STATE_RUNNING ){ + motor_now()->m_speed_pid_set_rpm = mcpwm_foc_get_rpm(); + } + motor_now()->m_speed_command_rpm = rpm; + }else{ + motor_now()->m_speed_pid_set_rpm = rpm; + } + motor_now()->m_control_mode = CONTROL_MODE_SPEED; - motor_now()->m_speed_pid_set_rpm = rpm; if (motor_now()->m_state != MC_STATE_RUNNING) { motor_now()->m_state = MC_STATE_RUNNING; @@ -3632,6 +3643,10 @@ static void run_pid_control_speed(float dt, volatile motor_all_state_t *motor) { return; } + if(conf_now->s_pid_apply_input_ramp){ + utils_step_towards((float*)&motor->m_speed_pid_set_rpm, motor->m_speed_command_rpm, conf_now->s_pid_ramp_erpms_ms); + } + const float rpm = mcpwm_foc_get_rpm(); float error = motor->m_speed_pid_set_rpm - rpm; From 0264242af2f798271fc66a2057524868687e4b86 Mon Sep 17 00:00:00 2001 From: Maximiliano Cordoba Date: Mon, 11 May 2020 17:00:27 -0300 Subject: [PATCH 2/4] Only use ERPM per Second varialble to control the input ramp to the speed PID loop. A negative value will disable the ramp, and fed the speed command directly to the PID as it used to do. Signed-off-by: Maximiliano Cordoba --- confgenerator.c | 9 +++------ confgenerator.h | 2 +- datatypes.h | 3 +-- mcconf/mcconf_default.h | 7 ++----- mcpwm_foc.c | 6 +++--- 5 files changed, 10 insertions(+), 17 deletions(-) diff --git a/confgenerator.c b/confgenerator.c index 95a234a0..7be5d02d 100644 --- a/confgenerator.c +++ b/confgenerator.c @@ -118,8 +118,7 @@ int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration * buffer_append_float32_auto(buffer, conf->s_pid_kd_filter, &ind); buffer_append_float32_auto(buffer, conf->s_pid_min_erpm, &ind); buffer[ind++] = conf->s_pid_allow_braking; - buffer[ind++] = conf->s_pid_apply_input_ramp; - buffer_append_float32_auto(buffer, conf->s_pid_ramp_erpms_ms, &ind); + buffer_append_float32_auto(buffer, conf->s_pid_ramp_erpms_s, &ind); buffer_append_float32_auto(buffer, conf->p_pid_kp, &ind); buffer_append_float32_auto(buffer, conf->p_pid_ki, &ind); buffer_append_float32_auto(buffer, conf->p_pid_kd, &ind); @@ -412,8 +411,7 @@ bool confgenerator_deserialize_mcconf(const uint8_t *buffer, mc_configuration *c conf->s_pid_kd_filter = buffer_get_float32_auto(buffer, &ind); conf->s_pid_min_erpm = buffer_get_float32_auto(buffer, &ind); conf->s_pid_allow_braking = buffer[ind++]; - conf->s_pid_apply_input_ramp = buffer[ind++]; - conf->s_pid_ramp_erpms_ms = buffer_get_float32_auto(buffer, &ind); + conf->s_pid_ramp_erpms_s = buffer_get_float32_auto(buffer, &ind); conf->p_pid_kp = buffer_get_float32_auto(buffer, &ind); conf->p_pid_ki = buffer_get_float32_auto(buffer, &ind); conf->p_pid_kd = buffer_get_float32_auto(buffer, &ind); @@ -702,8 +700,7 @@ void confgenerator_set_defaults_mcconf(mc_configuration *conf) { conf->s_pid_kd_filter = MCCONF_S_PID_KD_FILTER; conf->s_pid_min_erpm = MCCONF_S_PID_MIN_RPM; conf->s_pid_allow_braking = MCCONF_S_PID_ALLOW_BRAKING; - conf->s_pid_apply_input_ramp = MCCONF_S_PID_APPLY_INPUT_RAMP; - conf->s_pid_ramp_erpms_ms = MCCONF_S_PID_RAMP_ERPMS_MS; + conf->s_pid_ramp_erpms_s = MCCONF_S_PID_RAMP_ERPMS_S; conf->p_pid_kp = MCCONF_P_PID_KP; conf->p_pid_ki = MCCONF_P_PID_KI; conf->p_pid_kd = MCCONF_P_PID_KD; diff --git a/confgenerator.h b/confgenerator.h index 49785c4e..ab31fdbe 100644 --- a/confgenerator.h +++ b/confgenerator.h @@ -8,7 +8,7 @@ #include // Constants -#define MCCONF_SIGNATURE 2111869092 +#define MCCONF_SIGNATURE 1775793947 #define APPCONF_SIGNATURE 2460147246 // Functions diff --git a/datatypes.h b/datatypes.h index f8fa5d52..ab1119a7 100644 --- a/datatypes.h +++ b/datatypes.h @@ -321,8 +321,7 @@ typedef struct { float s_pid_kd_filter; float s_pid_min_erpm; bool s_pid_allow_braking; - bool s_pid_apply_input_ramp; - float s_pid_ramp_erpms_ms; + float s_pid_ramp_erpms_s; // Pos PID float p_pid_kp; float p_pid_ki; diff --git a/mcconf/mcconf_default.h b/mcconf/mcconf_default.h index 8f1b6543..9c7acff6 100644 --- a/mcconf/mcconf_default.h +++ b/mcconf/mcconf_default.h @@ -136,11 +136,8 @@ #ifndef MCCONF_S_PID_ALLOW_BRAKING #define MCCONF_S_PID_ALLOW_BRAKING true // Allow braking in speed control mode #endif -#ifndef MCCONF_S_PID_APPLY_INPUT_RAMP -#define MCCONF_S_PID_APPLY_INPUT_RAMP false // Apply Input Ramp in speed control mode -#endif -#ifndef MCCONF_S_PID_RAMP_ERPMS_MS -#define MCCONF_S_PID_RAMP_ERPMS_MS 1.0 // Default Speed Input Ramp +#ifndef MCCONF_S_PID_RAMP_ERPMS_S +#define MCCONF_S_PID_RAMP_ERPMS_S -1.0 // Default Speed Input Ramp #endif // Position PID parameters diff --git a/mcpwm_foc.c b/mcpwm_foc.c index 6510e78e..62c6c67f 100644 --- a/mcpwm_foc.c +++ b/mcpwm_foc.c @@ -800,7 +800,7 @@ void mcpwm_foc_set_duty_noramp(float dutyCycle) { * The electrical RPM goal value to use. */ void mcpwm_foc_set_pid_speed(float rpm) { - if( motor_now()->m_conf->s_pid_apply_input_ramp ){ + if( motor_now()->m_conf->s_pid_ramp_erpms_s > 0.0 ){ if( motor_now()->m_control_mode != CONTROL_MODE_SPEED || motor_now()->m_state != MC_STATE_RUNNING ){ motor_now()->m_speed_pid_set_rpm = mcpwm_foc_get_rpm(); @@ -3643,8 +3643,8 @@ static void run_pid_control_speed(float dt, volatile motor_all_state_t *motor) { return; } - if(conf_now->s_pid_apply_input_ramp){ - utils_step_towards((float*)&motor->m_speed_pid_set_rpm, motor->m_speed_command_rpm, conf_now->s_pid_ramp_erpms_ms); + if(conf_now->s_pid_ramp_erpms_s > 0.0){ + utils_step_towards((float*)&motor->m_speed_pid_set_rpm, motor->m_speed_command_rpm, conf_now->s_pid_ramp_erpms_s * dt); } const float rpm = mcpwm_foc_get_rpm(); From e58c87103cf8a2d8a9ce01626a67e4e95a53203f Mon Sep 17 00:00:00 2001 From: Benjamin Vedder Date: Tue, 12 May 2020 15:30:34 +0200 Subject: [PATCH 3/4] Revert "Add ramp input to the speed PID loop." --- confgenerator.c | 3 --- confgenerator.h | 2 +- datatypes.h | 1 - mcconf/mcconf_default.h | 3 --- mcpwm_foc.c | 17 +---------------- 5 files changed, 2 insertions(+), 24 deletions(-) diff --git a/confgenerator.c b/confgenerator.c index 7be5d02d..032dbd6f 100644 --- a/confgenerator.c +++ b/confgenerator.c @@ -118,7 +118,6 @@ int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration * buffer_append_float32_auto(buffer, conf->s_pid_kd_filter, &ind); buffer_append_float32_auto(buffer, conf->s_pid_min_erpm, &ind); buffer[ind++] = conf->s_pid_allow_braking; - buffer_append_float32_auto(buffer, conf->s_pid_ramp_erpms_s, &ind); buffer_append_float32_auto(buffer, conf->p_pid_kp, &ind); buffer_append_float32_auto(buffer, conf->p_pid_ki, &ind); buffer_append_float32_auto(buffer, conf->p_pid_kd, &ind); @@ -411,7 +410,6 @@ bool confgenerator_deserialize_mcconf(const uint8_t *buffer, mc_configuration *c conf->s_pid_kd_filter = buffer_get_float32_auto(buffer, &ind); conf->s_pid_min_erpm = buffer_get_float32_auto(buffer, &ind); conf->s_pid_allow_braking = buffer[ind++]; - conf->s_pid_ramp_erpms_s = buffer_get_float32_auto(buffer, &ind); conf->p_pid_kp = buffer_get_float32_auto(buffer, &ind); conf->p_pid_ki = buffer_get_float32_auto(buffer, &ind); conf->p_pid_kd = buffer_get_float32_auto(buffer, &ind); @@ -700,7 +698,6 @@ void confgenerator_set_defaults_mcconf(mc_configuration *conf) { conf->s_pid_kd_filter = MCCONF_S_PID_KD_FILTER; conf->s_pid_min_erpm = MCCONF_S_PID_MIN_RPM; conf->s_pid_allow_braking = MCCONF_S_PID_ALLOW_BRAKING; - conf->s_pid_ramp_erpms_s = MCCONF_S_PID_RAMP_ERPMS_S; conf->p_pid_kp = MCCONF_P_PID_KP; conf->p_pid_ki = MCCONF_P_PID_KI; conf->p_pid_kd = MCCONF_P_PID_KD; diff --git a/confgenerator.h b/confgenerator.h index ab31fdbe..0385c4fd 100644 --- a/confgenerator.h +++ b/confgenerator.h @@ -8,7 +8,7 @@ #include // Constants -#define MCCONF_SIGNATURE 1775793947 +#define MCCONF_SIGNATURE 3698540221 #define APPCONF_SIGNATURE 2460147246 // Functions diff --git a/datatypes.h b/datatypes.h index ab1119a7..a170f056 100644 --- a/datatypes.h +++ b/datatypes.h @@ -321,7 +321,6 @@ typedef struct { float s_pid_kd_filter; float s_pid_min_erpm; bool s_pid_allow_braking; - float s_pid_ramp_erpms_s; // Pos PID float p_pid_kp; float p_pid_ki; diff --git a/mcconf/mcconf_default.h b/mcconf/mcconf_default.h index 9c7acff6..fbd4337c 100644 --- a/mcconf/mcconf_default.h +++ b/mcconf/mcconf_default.h @@ -136,9 +136,6 @@ #ifndef MCCONF_S_PID_ALLOW_BRAKING #define MCCONF_S_PID_ALLOW_BRAKING true // Allow braking in speed control mode #endif -#ifndef MCCONF_S_PID_RAMP_ERPMS_S -#define MCCONF_S_PID_RAMP_ERPMS_S -1.0 // Default Speed Input Ramp -#endif // Position PID parameters #ifndef MCCONF_P_PID_KP diff --git a/mcpwm_foc.c b/mcpwm_foc.c index 62c6c67f..c7154bfd 100644 --- a/mcpwm_foc.c +++ b/mcpwm_foc.c @@ -111,7 +111,6 @@ typedef struct { bool m_output_on; float m_pos_pid_set; float m_speed_pid_set_rpm; - float m_speed_command_rpm; float m_phase_now_observer; float m_phase_now_observer_override; bool m_phase_observer_override; @@ -166,7 +165,6 @@ typedef struct { float m_hall_dt_diff_now; } motor_all_state_t; - // Private variables static volatile bool m_dccal_done = false; static volatile float m_last_adc_isr_duration; @@ -800,17 +798,8 @@ void mcpwm_foc_set_duty_noramp(float dutyCycle) { * The electrical RPM goal value to use. */ void mcpwm_foc_set_pid_speed(float rpm) { - if( motor_now()->m_conf->s_pid_ramp_erpms_s > 0.0 ){ - if( motor_now()->m_control_mode != CONTROL_MODE_SPEED || - motor_now()->m_state != MC_STATE_RUNNING ){ - motor_now()->m_speed_pid_set_rpm = mcpwm_foc_get_rpm(); - } - motor_now()->m_speed_command_rpm = rpm; - }else{ - motor_now()->m_speed_pid_set_rpm = rpm; - } - motor_now()->m_control_mode = CONTROL_MODE_SPEED; + motor_now()->m_speed_pid_set_rpm = rpm; if (motor_now()->m_state != MC_STATE_RUNNING) { motor_now()->m_state = MC_STATE_RUNNING; @@ -3643,10 +3632,6 @@ static void run_pid_control_speed(float dt, volatile motor_all_state_t *motor) { return; } - if(conf_now->s_pid_ramp_erpms_s > 0.0){ - utils_step_towards((float*)&motor->m_speed_pid_set_rpm, motor->m_speed_command_rpm, conf_now->s_pid_ramp_erpms_s * dt); - } - const float rpm = mcpwm_foc_get_rpm(); float error = motor->m_speed_pid_set_rpm - rpm; From f6621e810d1434649f869acbc7777c6acdb3645e Mon Sep 17 00:00:00 2001 From: Esben Rasmussen Date: Mon, 29 Jun 2020 16:26:39 +0200 Subject: [PATCH 4/4] 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_ */