icm20948+mpu9250: add support to configure the high bus speed

PX4 · May 4, 2020 · bcce75e · bcce75e
1 parent 0becd29
commit bcce75e
Show file tree

Hide file tree

Showing 4 changed files with 12 additions and 7 deletions.
diff --git a/src/drivers/imu/icm20948/icm20948_main.cpp b/src/drivers/imu/icm20948/icm20948_main.cpp
@@ -103,7 +103,7 @@ icm20948_main(int argc, char *argv[])
 	int ch;
 	using ThisDriver = ICM20948;
 	BusCLIArguments cli{true, true};
-	cli.default_spi_frequency = 1000 * 1000; // low speed freq
+	cli.default_spi_frequency = 20 * 1000 * 1000;
 	cli.default_i2c_frequency = 400000;
 
 	while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {

diff --git a/src/drivers/imu/icm20948/icm20948_spi.cpp b/src/drivers/imu/icm20948/icm20948_spi.cpp
@@ -77,6 +77,8 @@ class ICM20948_SPI : public device::SPI
 
 	/* Helper to set the desired speed and isolate the register on return */
 	void set_bus_frequency(unsigned &reg_speed_reg_out);
+
+	const int _high_bus_speed;
 };
 
 device::Device *
@@ -86,15 +88,16 @@ ICM20948_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e sp
 }
 
 ICM20948_SPI::ICM20948_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) :
-	SPI(DRV_IMU_DEVTYPE_ICM20948, MODULE_NAME, bus, device, spi_mode, bus_frequency)
+	SPI(DRV_IMU_DEVTYPE_ICM20948, MODULE_NAME, bus, device, spi_mode, ICM20948_LOW_SPI_BUS_SPEED),
+	_high_bus_speed(bus_frequency)
 {
 }
 
 void
 ICM20948_SPI::set_bus_frequency(unsigned &reg_speed)
 {
 	/* Set the desired speed */
-	set_frequency(ICM20948_IS_HIGH_SPEED(reg_speed) ? ICM20948_HIGH_SPI_BUS_SPEED : ICM20948_LOW_SPI_BUS_SPEED);
+	set_frequency(ICM20948_IS_HIGH_SPEED(reg_speed) ? _high_bus_speed : ICM20948_LOW_SPI_BUS_SPEED);
 
 	/* Isoolate the register on return */
 	reg_speed = ICM20948_REG(reg_speed);

diff --git a/src/drivers/imu/mpu9250/mpu9250_main.cpp b/src/drivers/imu/mpu9250/mpu9250_main.cpp
@@ -99,7 +99,7 @@ mpu9250_main(int argc, char *argv[])
 	int ch;
 	using ThisDriver = MPU9250;
 	BusCLIArguments cli{true, true};
-	cli.default_spi_frequency = 1000 * 1000; // low speed bus frequency
+	cli.default_spi_frequency = 20 * 1000 * 1000;
 	cli.default_i2c_frequency = 400000;
 
 	while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {

diff --git a/src/drivers/imu/mpu9250/mpu9250_spi.cpp b/src/drivers/imu/mpu9250/mpu9250_spi.cpp
@@ -74,9 +74,10 @@ class MPU9250_SPI : public device::SPI
 	int probe() override;
 
 private:
-
 	/* Helper to set the desired speed and isolate the register on return */
 	void set_bus_frequency(unsigned &reg_speed_reg_out);
+
+	const int _high_bus_speed;
 };
 
 device::Device *
@@ -86,15 +87,16 @@ MPU9250_SPI_interface(int bus, uint32_t cs, int bus_frequency, spi_mode_e spi_mo
 }
 
 MPU9250_SPI::MPU9250_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) :
-	SPI(DRV_IMU_DEVTYPE_MPU9250, MODULE_NAME, bus, device, spi_mode, bus_frequency)
+	SPI(DRV_IMU_DEVTYPE_MPU9250, MODULE_NAME, bus, device, spi_mode, MPU9250_LOW_SPI_BUS_SPEED),
+	_high_bus_speed(bus_frequency)
 {
 }
 
 void
 MPU9250_SPI::set_bus_frequency(unsigned &reg_speed)
 {
 	/* Set the desired speed */
-	set_frequency(MPU9250_IS_HIGH_SPEED(reg_speed) ? MPU9250_HIGH_SPI_BUS_SPEED : MPU9250_LOW_SPI_BUS_SPEED);
+	set_frequency(MPU9250_IS_HIGH_SPEED(reg_speed) ? _high_bus_speed : MPU9250_LOW_SPI_BUS_SPEED);
 
 	/* Isolate the register on return */
 	reg_speed = MPU9250_REG(reg_speed);