Added 2 IMU I2C drivers, SPI mode and bus speed option, fixed 1 I2C read bug, etc. [rebased]

[bkueng]

Rebased version of #14168. @UAV-Pilot can you test it? And let me know if I dropped something, there were many conflicts.

Tested on nxp/fmuk66.

[UAV-Pilot]

@bkueng Thanks a lot for helping with the merge! When starting with correct configuration parameters, barometer and IMU sensors worked with minor changes described below, please get these changes into imu_drivers_i2c_interface branch.

Two changes to get code compiled natively on my board:

1. add one cast (uint32_t)

git diff src/drivers/adc/ADC.cpp
diff --git a/src/drivers/adc/ADC.cpp b/src/drivers/adc/ADC.cpp
index d5befd80b3..aa5761c5c0 100644
--- a/src/drivers/adc/ADC.cpp
+++ b/src/drivers/adc/ADC.cpp
@@ -51,7 +51,7 @@ ADC::ADC(uint32_t base_address, uint32_t channels) :
        }
 
        if (_channel_count > PX4_MAX_ADC_CHANNELS) {
-               PX4_ERR("PX4_MAX_ADC_CHANNELS is too small:is %d needed:%d", PX4_MAX_ADC_CHANNELS, _channel_count);
+               PX4_ERR("PX4_MAX_ADC_CHANNELS is too small:is %d needed:%d", (uint32_t)PX4_MAX_ADC_CHANNELS, _channel_count);
        }
 
        _samples = new px4_adc_msg_t[_channel_count];

2. remove one override and change one parameter type:

 git diff src/drivers/magnetometer/bmm150/bmm150.hpp
diff --git a/src/drivers/magnetometer/bmm150/bmm150.hpp b/src/drivers/magnetometer/bmm150/bmm150.hpp
index 0de7dec46e..30fdd60ecf 100644
--- a/src/drivers/magnetometer/bmm150/bmm150.hpp
+++ b/src/drivers/magnetometer/bmm150/bmm150.hpp
@@ -228,8 +228,8 @@ public:
        static void print_usage();
 
        int             init() override;
-       ssize_t       read(struct file *filp, char *buffer, size_t buflen) override;
-       int       ioctl(struct file *filp, int cmd, unsigned long arg) override;
+       ssize_t       read(struct file *filp, char *buffer, size_t buflen) /*override*/;
+       int       ioctl(file_t *filp, int cmd, unsigned long arg) override;
 
        void            print_status() override;

mpu9250 and icm20948 need alternate between 2 SPI frequencies (high and low frequency) on the fly, the following changes are needed to take the frequency specified in command options as the high frequency:

3. applying specified high frequency for mpu9250

git diff src/drivers/imu/mpu9250/mpu9250_spi.cpp
diff --git a/src/drivers/imu/mpu9250/mpu9250_spi.cpp b/src/drivers/imu/mpu9250/mpu9250_spi.cpp
index 2b2084289d..192a185811 100644
--- a/src/drivers/imu/mpu9250/mpu9250_spi.cpp
+++ b/src/drivers/imu/mpu9250/mpu9250_spi.cpp
@@ -74,6 +74,7 @@ protected:
        int probe() override;
 
 private:
+    int32_t high_bus_speed;
 
        /* Helper to set the desired speed and isolate the register on return */
        void set_bus_frequency(unsigned ®_speed_reg_out);
@@ -86,7 +87,8 @@ 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("MPU9250", nullptr, bus, device, spi_mode, bus_frequency)
+    SPI("MPU9250", nullptr, bus, device, spi_mode, bus_frequency),
+    high_bus_speed(bus_frequency)
 {
        set_device_type(DRV_IMU_DEVTYPE_MPU9250);
 }
@@ -95,7 +97,7 @@ void
 MPU9250_SPI::set_bus_frequency(unsigned ®_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);

4. same change for icm20948

git diff src/drivers/imu/icm20948/icm20948_spi.cpp
diff --git a/src/drivers/imu/icm20948/icm20948_spi.cpp b/src/drivers/imu/icm20948/icm20948_spi.cpp
index d086fddff7..0c1ab7859c 100644
--- a/src/drivers/imu/icm20948/icm20948_spi.cpp
+++ b/src/drivers/imu/icm20948/icm20948_spi.cpp
@@ -74,6 +74,7 @@ protected:
        int probe() override;
 
 private:
+    int32_t high_bus_speed;
 
        /* Helper to set the desired speed and isolate the register on return */
        void set_bus_frequency(unsigned ®_speed_reg_out);
@@ -86,7 +87,8 @@ 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("ICM20948", nullptr, bus, device, spi_mode, bus_frequency)
+    SPI("ICM20948", nullptr, bus, device, spi_mode, bus_frequency),
+    high_bus_speed(bus_frequency)
 {
        _device_id.devid_s.devtype = DRV_IMU_DEVTYPE_ICM20948;
 }
@@ -95,7 +97,7 @@ void
 ICM20948_SPI::set_bus_frequency(unsigned ®_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);

Two observations for future consideration:

a) When redundant sensors are used, I cannot clearly get a sense which sensor behaved better when seeing/hearing error messages like "Gyro #0 fail". It would be better to change it to something like "Gyro #0 (mpu9250) fail"

b) When incorrect internal/external SPI/I2C options were given, neither error messages and nor successful messages were printed out.

[UAV-Pilot]

I tested another sensor board and found a problem with I2C device address. The problem did not exist in previous code which does not use BusCLIArguments, etc. yet. It seems that addresses of I2C FXAS21002C and FXOS8701CQ drivers were hard coded (0x20, 0x1E below):

FXAS21002C_I2C::FXAS21002C_I2C(int bus, int bus_frequency) :
	I2C("FXAS21002C", nullptr, bus, 0x20, bus_frequency)
{
	set_device_type(DRV_GYR_DEVTYPE_FXAS2100C);
}

FXOS8701CQ_I2C::FXOS8701CQ_I2C(int bus, int bus_frequency) :
	I2C("FXOS8701CQ", nullptr, bus, 0x1E, bus_frequency)
{
	set_device_type(DRV_ACC_DEVTYPE_FXOS8701C);
}

Previously the addresses were configured by the following macros:

#define PX4_I2C_FXAS21002C_ADDR 0x21
#define PX4_I2C_FXOS8701CQ_ADDR 0x1F

I have 2 sensor boards with one board uses addresses 0x20, 0x1E and another uses 0x21, 0x1F for FXAS21002C and FXOS8701CQ respectively. Handling this kind of configuration with BusCLIArguments, etc. is an improvement over previous usage of macros as it's allowed to use command option to conveniently configure addresses of I2C drivers.

[bkueng]

@UAV-Pilot I applied your changes, including configurable I2C address for the 2 drivers (didn't think anyone would want to use the non-default address).

[UAV-Pilot]

@bkueng I got the latest from imu_drivers_i2c_interface branch, and it worked as expected! Your help is highly appreciated!

[bkueng]

Good!
@PX4/testflights can you test this on the NXP autopilot?

[Junkim3DR]

Tested on NXP FMUK66 V3

Modes Tested

• Position Mode: Good.
• Altitude Mode: Good.
• Stabilized Mode: Good.
• Mission Plan Mode (Automated): Good.
• RTL (Return To Land): Good.

Procedure
Arm and Take off in position mode, after flying for approximately one minute, switched to altitude then stabilized mode proceed to switch to mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint then trigger RTL.

Notes
No issues noted, good flight in general.

Log

• https://review.px4.io/plot_app?log=236fd1e5-0b7e-4bd4-b6f3-34b524f8805c

[UAV-Pilot]

Any chance to get reviewed and merged soon? It's almost 2 months now since the original pull request #14168 was submitted.

[bkueng]

@dagar good to go? Do you want to branch out for the release first?

[UAV-Pilot]

Is there any special reason preventing this pull request from being merged?

[dagar]

Is there any special reason preventing this pull request from being merged?

@UAV-Pilot I apologize, we've been going back and forth trying to get things ready for a release, but a number of issues keep pushing it back. It looks like we're going to have a bit more time still, so let's get this in.

[dagar]

Rebased on current master.

[UAV-Pilot]

@bkueng @dagar Thanks a lot for getting these changes in! One of the reasons that I want these changes being merged sooner is that I did the work on my master branch which created difficulty for submitting new pull requests. I should have done the work from a non-master branch.

[bkueng]

You're welcome. It should not have taken that long in the first place (was because of the release).