From f746141afe3499ae56e75b3db06409381ead2f4b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 31 Jan 2017 09:27:26 +0100 Subject: [PATCH] mpu6000: add & check for the device_type on driver startup Fixes the following case: a board that has several bus_options enabled, and wants to start a specific device (eg mpu6000 -T 20608 start). In that case the given device_type is never actually checked, and since the start routine iterates over all bus_options, several can match, and thus start multiple instances of the driver (note that the whoami check is performed against the value provided with -T and thus the check will pass). This happens on Pixracer (PX4_SPIDEV_MPU and PX4_SPIDEV_ICM_20608 are both set). --- src/drivers/mpu6000/mpu6000.cpp | 24 +++++++++++++++--------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 40dbe8098f..4d385f1e99 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -2253,6 +2253,7 @@ namespace mpu6000 struct mpu6000_bus_option { enum MPU6000_BUS busid; + MPU_DEVICE_TYPE device_type; const char *accelpath; const char *gyropath; MPU6000_constructor interface_constructor; @@ -2262,30 +2263,30 @@ struct mpu6000_bus_option { } bus_options[] = { #if defined (USE_I2C) # if defined(PX4_I2C_BUS_ONBOARD) - { MPU6000_BUS_I2C_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, &MPU6000_I2C_interface, PX4_I2C_BUS_ONBOARD, false, NULL }, + { MPU6000_BUS_I2C_INTERNAL, MPU_DEVICE_TYPE_MPU6000, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, &MPU6000_I2C_interface, PX4_I2C_BUS_ONBOARD, false, NULL }, # endif # if defined(PX4_I2C_BUS_EXPANSION) - { MPU6000_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, &MPU6000_I2C_interface, PX4_I2C_BUS_EXPANSION, true, NULL }, + { MPU6000_BUS_I2C_EXTERNAL, MPU_DEVICE_TYPE_MPU6000, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, &MPU6000_I2C_interface, PX4_I2C_BUS_EXPANSION, true, NULL }, # endif #endif #ifdef PX4_SPIDEV_MPU - { MPU6000_BUS_SPI_INTERNAL1, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS, false, NULL }, + { MPU6000_BUS_SPI_INTERNAL1, MPU_DEVICE_TYPE_MPU6000, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS, false, NULL }, #endif #if defined(PX4_SPI_BUS_EXT) - { MPU6000_BUS_SPI_EXTERNAL1, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, &MPU6000_SPI_interface, PX4_SPI_BUS_EXT, true, NULL }, + { MPU6000_BUS_SPI_EXTERNAL1, MPU_DEVICE_TYPE_MPU6000, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, &MPU6000_SPI_interface, PX4_SPI_BUS_EXT, true, NULL }, #endif #ifdef PX4_SPIDEV_ICM_20602 - { MPU6000_BUS_SPI_INTERNAL1, ICM20602_DEVICE_PATH_ACCEL, ICM20602_DEVICE_PATH_GYRO, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS, false, NULL }, + { MPU6000_BUS_SPI_INTERNAL1, MPU_DEVICE_TYPE_ICM20602, ICM20602_DEVICE_PATH_ACCEL, ICM20602_DEVICE_PATH_GYRO, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS, false, NULL }, #endif #ifdef PX4_SPIDEV_ICM_20608 - { MPU6000_BUS_SPI_INTERNAL1, ICM20608_DEVICE_PATH_ACCEL, ICM20608_DEVICE_PATH_GYRO, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS, false, NULL }, + { MPU6000_BUS_SPI_INTERNAL1, MPU_DEVICE_TYPE_ICM20608, ICM20608_DEVICE_PATH_ACCEL, ICM20608_DEVICE_PATH_GYRO, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS, false, NULL }, #endif #ifdef PX4_SPIDEV_ICM_20689 - { MPU6000_BUS_SPI_INTERNAL2, ICM20689_DEVICE_PATH_ACCEL, ICM20689_DEVICE_PATH_GYRO, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS, false, NULL }, + { MPU6000_BUS_SPI_INTERNAL2, MPU_DEVICE_TYPE_ICM20689, ICM20689_DEVICE_PATH_ACCEL, ICM20689_DEVICE_PATH_GYRO, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS, false, NULL }, #endif #if defined(PX4_SPI_BUS_EXTERNAL) - { MPU6000_BUS_SPI_EXTERNAL1, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, &MPU6000_SPI_interface, PX4_SPI_BUS_EXTERNAL, true, NULL }, - { MPU6000_BUS_SPI_EXTERNAL2, MPU_DEVICE_PATH_ACCEL_EXT1, MPU_DEVICE_PATH_GYRO_EXT1, &MPU6000_SPI_interface, PX4_SPI_BUS_EXTERNAL, true, NULL }, + { MPU6000_BUS_SPI_EXTERNAL1, MPU_DEVICE_TYPE_MPU6000, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, &MPU6000_SPI_interface, PX4_SPI_BUS_EXTERNAL, true, NULL }, + { MPU6000_BUS_SPI_EXTERNAL2, MPU_DEVICE_TYPE_MPU6000, MPU_DEVICE_PATH_ACCEL_EXT1, MPU_DEVICE_PATH_GYRO_EXT1, &MPU6000_SPI_interface, PX4_SPI_BUS_EXTERNAL, true, NULL }, #endif }; @@ -2413,6 +2414,11 @@ start(enum MPU6000_BUS busid, enum Rotation rotation, int range, int device_type continue; } + if (bus_options[i].device_type != device_type) { + // not the one that is asked for + continue; + } + started |= start_bus(bus_options[i], rotation, range, device_type); }