mirror of https://github.com/rusefi/bldc.git
Added terminal command to read internal imu type
This commit is contained in:
parent
f359fc75cd
commit
3dbd21820e
19
imu/imu.c
19
imu/imu.c
|
@ -47,6 +47,7 @@ static imu_config m_settings;
|
||||||
static systime_t init_time;
|
static systime_t init_time;
|
||||||
static bool imu_ready;
|
static bool imu_ready;
|
||||||
static Biquad acc_x_biquad, acc_y_biquad, acc_z_biquad;
|
static Biquad acc_x_biquad, acc_y_biquad, acc_z_biquad;
|
||||||
|
static char *m_imu_type_internal = "Unknown";
|
||||||
|
|
||||||
// Private functions
|
// Private functions
|
||||||
static void imu_read_callback(float *accel, float *gyro, float *mag);
|
static void imu_read_callback(float *accel, float *gyro, float *mag);
|
||||||
|
@ -54,6 +55,7 @@ static int8_t user_i2c_read(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, u
|
||||||
static int8_t user_i2c_write(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len);
|
static int8_t user_i2c_write(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len);
|
||||||
static int8_t user_spi_read(uint8_t dev_id, uint8_t reg_addr, uint8_t *data, uint16_t len);
|
static int8_t user_spi_read(uint8_t dev_id, uint8_t reg_addr, uint8_t *data, uint16_t len);
|
||||||
static int8_t user_spi_write(uint8_t dev_id, uint8_t reg_addr, uint8_t *data, uint16_t len);
|
static int8_t user_spi_write(uint8_t dev_id, uint8_t reg_addr, uint8_t *data, uint16_t len);
|
||||||
|
static void terminal_imu_type_internal(int argc, const char **argv);
|
||||||
|
|
||||||
void imu_init(imu_config *set) {
|
void imu_init(imu_config *set) {
|
||||||
m_settings = *set;
|
m_settings = *set;
|
||||||
|
@ -74,21 +76,25 @@ void imu_init(imu_config *set) {
|
||||||
#ifdef MPU9X50_SDA_GPIO
|
#ifdef MPU9X50_SDA_GPIO
|
||||||
imu_init_mpu9x50(MPU9X50_SDA_GPIO, MPU9X50_SDA_PIN,
|
imu_init_mpu9x50(MPU9X50_SDA_GPIO, MPU9X50_SDA_PIN,
|
||||||
MPU9X50_SCL_GPIO, MPU9X50_SCL_PIN);
|
MPU9X50_SCL_GPIO, MPU9X50_SCL_PIN);
|
||||||
|
m_imu_type_internal = "MPU9X50";
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef ICM20948_SDA_GPIO
|
#ifdef ICM20948_SDA_GPIO
|
||||||
imu_init_icm20948(ICM20948_SDA_GPIO, ICM20948_SDA_PIN,
|
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);
|
||||||
|
m_imu_type_internal = "ICM20948";
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef BMI160_SDA_GPIO
|
#ifdef BMI160_SDA_GPIO
|
||||||
imu_init_bmi160_i2c(BMI160_SDA_GPIO, BMI160_SDA_PIN,
|
imu_init_bmi160_i2c(BMI160_SDA_GPIO, BMI160_SDA_PIN,
|
||||||
BMI160_SCL_GPIO, BMI160_SCL_PIN);
|
BMI160_SCL_GPIO, BMI160_SCL_PIN);
|
||||||
|
m_imu_type_internal = "BMI160";
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef LSM6DS3_SDA_GPIO
|
#ifdef LSM6DS3_SDA_GPIO
|
||||||
imu_init_lsm6ds3(LSM6DS3_SDA_GPIO, LSM6DS3_SDA_PIN,
|
imu_init_lsm6ds3(LSM6DS3_SDA_GPIO, LSM6DS3_SDA_PIN,
|
||||||
LSM6DS3_SCL_GPIO, LSM6DS3_SCL_PIN);
|
LSM6DS3_SCL_GPIO, LSM6DS3_SCL_PIN);
|
||||||
|
m_imu_type_internal = "LSM6DS3";
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// SPI not implemented yet, use as I2C
|
// SPI not implemented yet, use as I2C
|
||||||
|
@ -99,6 +105,7 @@ void imu_init(imu_config *set) {
|
||||||
palClearPad(LSM6DS3_MISO_GPIO, LSM6DS3_MISO_PIN);
|
palClearPad(LSM6DS3_MISO_GPIO, LSM6DS3_MISO_PIN);
|
||||||
imu_init_lsm6ds3(LSM6DS3_MOSI_GPIO, LSM6DS3_MOSI_PIN,
|
imu_init_lsm6ds3(LSM6DS3_MOSI_GPIO, LSM6DS3_MOSI_PIN,
|
||||||
LSM6DS3_SCK_GPIO, LSM6DS3_SCK_PIN);
|
LSM6DS3_SCK_GPIO, LSM6DS3_SCK_PIN);
|
||||||
|
m_imu_type_internal = "LSM6DS3";
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef BMI160_SPI_PORT_NSS
|
#ifdef BMI160_SPI_PORT_NSS
|
||||||
|
@ -107,6 +114,7 @@ void imu_init(imu_config *set) {
|
||||||
BMI160_SPI_PORT_SCK, BMI160_SPI_PIN_SCK,
|
BMI160_SPI_PORT_SCK, BMI160_SPI_PIN_SCK,
|
||||||
BMI160_SPI_PORT_MOSI, BMI160_SPI_PIN_MOSI,
|
BMI160_SPI_PORT_MOSI, BMI160_SPI_PIN_MOSI,
|
||||||
BMI160_SPI_PORT_MISO, BMI160_SPI_PIN_MISO);
|
BMI160_SPI_PORT_MISO, BMI160_SPI_PIN_MISO);
|
||||||
|
m_imu_type_internal = "BMI160_SPI";
|
||||||
#endif
|
#endif
|
||||||
} else if (set->type == IMU_TYPE_EXTERNAL_MPU9X50) {
|
} else if (set->type == IMU_TYPE_EXTERNAL_MPU9X50) {
|
||||||
imu_init_mpu9x50(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
|
imu_init_mpu9x50(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
|
||||||
|
@ -139,6 +147,12 @@ void imu_init(imu_config *set) {
|
||||||
fc = m_settings.accel_lowpass_filter_z / m_settings.sample_rate_hz;
|
fc = m_settings.accel_lowpass_filter_z / m_settings.sample_rate_hz;
|
||||||
biquad_config(&acc_z_biquad, BQ_LOWPASS, fc);
|
biquad_config(&acc_z_biquad, BQ_LOWPASS, fc);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
terminal_register_command_callback(
|
||||||
|
"imu_type_internal",
|
||||||
|
"Print internal IMU type",
|
||||||
|
0,
|
||||||
|
terminal_imu_type_internal);
|
||||||
}
|
}
|
||||||
|
|
||||||
void imu_reset_orientation(void) {
|
void imu_reset_orientation(void) {
|
||||||
|
@ -635,3 +649,8 @@ static int8_t user_spi_write(uint8_t dev_id, uint8_t reg_addr, uint8_t *data, ui
|
||||||
|
|
||||||
return rslt;
|
return rslt;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static void terminal_imu_type_internal(int argc, const char **argv) {
|
||||||
|
(void)argc;(void)argv;
|
||||||
|
commands_printf(m_imu_type_internal);
|
||||||
|
}
|
||||||
|
|
Loading…
Reference in New Issue