From 64b058937010db5c53de9ecc8e721bdd2aa9b725 Mon Sep 17 00:00:00 2001 From: Niklas Hauser Date: Thu, 14 Nov 2024 13:53:25 +0100 Subject: [PATCH 1/3] boards: Remove unused sensors from FMUv5x/v6x board config --- boards/px4/fmu-v5x/default.px4board | 9 +-- boards/px4/fmu-v5x/init/rc.board_sensors | 24 ++---- boards/px4/fmu-v6x/default.px4board | 10 +-- boards/px4/fmu-v6x/init/rc.board_sensors | 95 +++--------------------- src/drivers/distance_sensor/Kconfig | 2 - src/drivers/magnetometer/Kconfig | 2 - 6 files changed, 21 insertions(+), 121 deletions(-) diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index 8ba41726c086..fd61e50080de 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -10,7 +10,6 @@ CONFIG_BOARD_SERIAL_EXT2="/dev/ttyS3" CONFIG_DRIVERS_ADC_ADS1115=y CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_BMP388=y -CONFIG_DRIVERS_BAROMETER_MS5611=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y @@ -21,18 +20,14 @@ CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPIO_MCP23009=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_HEATER=y -CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y -CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16507=y CONFIG_DRIVERS_IMU_BOSCH_BMI088=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y -CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y -CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y -CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y CONFIG_DRIVERS_IRLOCK=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y -CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OPTICAL_FLOW_PAW3902=y +CONFIG_DRIVERS_OPTICAL_FLOW_PMW3901=y CONFIG_DRIVERS_OSD_MSP_OSD=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_INA228=y diff --git a/boards/px4/fmu-v5x/init/rc.board_sensors b/boards/px4/fmu-v5x/init/rc.board_sensors index 1a0f426db6f4..b340e297eea0 100644 --- a/boards/px4/fmu-v5x/init/rc.board_sensors +++ b/boards/px4/fmu-v5x/init/rc.board_sensors @@ -53,15 +53,9 @@ if ver hwbasecmp 008 009 00a 010 011 then #SKYNODE base fmu board orientation - if ver hwtypecmp V5X000 V5X001 - then - # Internal SPI BMI088 - bmi088 -A -R 2 -s start - bmi088 -G -R 2 -s start - else - # Internal SPI bus ICM20649 - icm20649 -s -R 4 start - fi + # Internal SPI BMI088 + bmi088 -A -R 2 -s start + bmi088 -G -R 2 -s start # Internal SPI bus ICM42688p icm42688p -R 4 -s start @@ -78,15 +72,9 @@ then else #FMUv5Xbase board orientation - if ver hwtypecmp V5X000 V5X001 - then - # Internal SPI BMI088 - bmi088 -A -R 4 -s start - bmi088 -G -R 4 -s start - else - # Internal SPI bus ICM20649 - icm20649 -s -R 6 start - fi + # Internal SPI BMI088 + bmi088 -A -R 4 -s start + bmi088 -G -R 4 -s start # Internal SPI bus ICM42688p icm42688p -R 6 -s start diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index f7bbda7c4948..c9b8d830b706 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -10,8 +10,6 @@ CONFIG_BOARD_SERIAL_EXT2="/dev/ttyS3" CONFIG_DRIVERS_ADC_ADS1115=y CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_BMP388=y -CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y -CONFIG_DRIVERS_BAROMETER_MS5611=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y CONFIG_DRIVERS_CDCACM_AUTOSTART=y @@ -22,15 +20,11 @@ CONFIG_DRIVERS_GPIO_MCP23009=y CONFIG_DRIVERS_GNSS_SEPTENTRIO=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_HEATER=y -CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y CONFIG_DRIVERS_IMU_BOSCH_BMI088=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y -CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y -CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y -CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y -CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=y -CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y +CONFIG_DRIVERS_OPTICAL_FLOW_PAW3902=y +CONFIG_DRIVERS_OPTICAL_FLOW_PMW3901=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_DRIVERS_OSD_MSP_OSD=y diff --git a/boards/px4/fmu-v6x/init/rc.board_sensors b/boards/px4/fmu-v6x/init/rc.board_sensors index 15168a6e8f51..82b4662fadaf 100644 --- a/boards/px4/fmu-v6x/init/rc.board_sensors +++ b/boards/px4/fmu-v6x/init/rc.board_sensors @@ -71,80 +71,17 @@ else fi fi -# Keep nesting shallow -if ver hwtypecmp V6X006 V6X008 -then - if ver hwtypecmp V6X006 - then - # Internal SPI bus ICM45686 - adis16470 -s -R 0 start - iim42652 -s -R 6 start - icm45686 -s -R 10 start - else - # Internal SPI bus 3x ICM45686 - icm45686 -b 3 -s -R 0 start - icm45686 -b 2 -s -R 0 start - icm45686 -b 1 -s -R 10 start - fi -else - if ver hwtypecmp V6X004 - then - # Internal SPI bus ICM20649 - icm20649 -s -R 6 start - else - # Internal SPI BMI088 - if ver hwbasecmp 009 010 011 - then - bmi088 -A -R 6 -s start - bmi088 -G -R 6 -s start - else - if ver hwtypecmp V6X010 - then - bmi088 -A -R 0 -s start - bmi088 -G -R 0 -s start - else - bmi088 -A -R 4 -s start - bmi088 -G -R 4 -s start - fi - fi - fi - - # Internal SPI bus ICM42688p - if ver hwbasecmp 009 010 011 - then - icm42688p -R 12 -s start - else - if ver hwtypecmp V6X010 - then - icm42688p -R 14 -s start - else - icm42688p -R 6 -s start - fi - fi +# Internal SPI BMI088 +bmi088 -A -R 6 -s start +bmi088 -G -R 6 -s start - if ver hwtypecmp V6X003 V6X004 - then - # Internal SPI bus ICM-42670-P (hard-mounted) - icm42670p -R 10 -s start - else - if ver hwbasecmp 009 010 011 - then - icm20602 -R 6 -s start - else - # Internal SPI bus ICM-20649 (hard-mounted) - icm20649 -R 14 -s start - fi - fi -fi +# Internal SPI bus ICM42688p +icm42688p -R 12 -s start +# Internal SPI bus ICM20602 +icm20602 -R 6 -s start -# Internal magnetometer on I2c -if ver hwtypecmp V6X001 -then - rm3100 -I -b 4 start -else - # Internal magnetometer on I2C - bmm150 -I -R 0 start -fi +# Internal magnetometer on I2C +bmm150 -I -R 0 start # External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer) ist8310 -X -b 1 -R 10 start @@ -152,21 +89,11 @@ ist8310 -X -b 1 -R 10 start # Possible internal Baro if param compare SENS_INT_BARO_EN 1 then - if ver hwtypecmp V6X001 V6X006 V6X008 - then - icp201xx -I -a 0x64 start - else - bmp388 -I -a 0x77 start - fi + bmp388 -I -a 0x77 start fi #external baro -if ver hwtypecmp V6X001 -then - icp201xx -X start -else - bmp388 -X start -fi +bmp388 -X start # Don't try to start external baro on I2C3 as it can conflict with the MS5525DSO airspeed sensor. #ms5611 -X start diff --git a/src/drivers/distance_sensor/Kconfig b/src/drivers/distance_sensor/Kconfig index f45abe877c50..22f5d0c8777f 100644 --- a/src/drivers/distance_sensor/Kconfig +++ b/src/drivers/distance_sensor/Kconfig @@ -6,9 +6,7 @@ menu "Distance sensors" select DRIVERS_DISTANCE_SENSOR_GY_US42 select DRIVERS_DISTANCE_SENSOR_LEDDAR_ONE select DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C - select DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL select DRIVERS_DISTANCE_SENSOR_LL40LS - select DRIVERS_DISTANCE_SENSOR_SRF02 select DRIVERS_DISTANCE_SENSOR_TERARANGER select DRIVERS_DISTANCE_SENSOR_TF02PRO select DRIVERS_DISTANCE_SENSOR_TFMINI diff --git a/src/drivers/magnetometer/Kconfig b/src/drivers/magnetometer/Kconfig index 018b89502b26..eb99d0dcf154 100644 --- a/src/drivers/magnetometer/Kconfig +++ b/src/drivers/magnetometer/Kconfig @@ -7,11 +7,9 @@ menu "Magnetometer" select DRIVERS_MAGNETOMETER_BOSCH_BMM150 select DRIVERS_MAGNETOMETER_HMC5883 select DRIVERS_MAGNETOMETER_QMC5883L - select DRIVERS_MAGNETOMETER_ISENTEK_IST8308 select DRIVERS_MAGNETOMETER_ISENTEK_IST8310 select DRIVERS_MAGNETOMETER_LIS3MDL select DRIVERS_MAGNETOMETER_LSM303AGR - select DRIVERS_MAGNETOMETER_RM3100 select DRIVERS_MAGNETOMETER_VTRANTECH_VCM1193L select DRIVERS_MAGNETOMETER_MEMSIC_MMC5983MA select DRIVERS_MAGNETOMETER_ST_IIS2MDC From 7d9d8f754a9eef23eb7fe50efc65e97e585ca3c3 Mon Sep 17 00:00:00 2001 From: Alexander Lerach Date: Thu, 14 Nov 2024 15:11:42 +0100 Subject: [PATCH 2/3] Un-inlined rotate_3 --- src/lib/conversion/rotation.cpp | 28 +++++++++++++++++++++ src/lib/conversion/rotation.h | 44 +++++++++------------------------ 2 files changed, 39 insertions(+), 33 deletions(-) diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index 19f99678808a..3d00bf7db2c0 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -56,3 +56,31 @@ get_rot_quaternion(enum Rotation rot) math::radians((float)rot_lookup[rot].pitch), math::radians((float)rot_lookup[rot].yaw)}}; } + +__EXPORT void +rotate_3i(enum Rotation rot, int16_t &x, int16_t &y, int16_t &z) +{ + if (!rotate_3(rot, x, y, z)) { + // otherwise use full rotation matrix for valid rotations + if (rot < ROTATION_MAX) { + const matrix::Vector3f r{get_rot_matrix(rot) *matrix::Vector3f{(float)x, (float)y, (float)z}}; + x = math::constrain(roundf(r(0)), (float)INT16_MIN, (float)INT16_MAX); + y = math::constrain(roundf(r(1)), (float)INT16_MIN, (float)INT16_MAX); + z = math::constrain(roundf(r(2)), (float)INT16_MIN, (float)INT16_MAX); + } + } +} + +__EXPORT void +rotate_3f(enum Rotation rot, float &x, float &y, float &z) +{ + if (!rotate_3(rot, x, y, z)) { + // otherwise use full rotation matrix for valid rotations + if (rot < ROTATION_MAX) { + const matrix::Vector3f r{get_rot_matrix(rot) *matrix::Vector3f{x, y, z}}; + x = r(0); + y = r(1); + z = r(2); + } + } +} diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index b33e4a8383f0..350404723e01 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -157,8 +157,18 @@ __EXPORT matrix::Dcmf get_rot_matrix(enum Rotation rot); */ __EXPORT matrix::Quatf get_rot_quaternion(enum Rotation rot); +/** + * rotate a 3 element int16_t vector in-place + */ +__EXPORT void rotate_3i(enum Rotation rot, int16_t &x, int16_t &y, int16_t &z); + +/** + * rotate a 3 element float vector in-place + */ +__EXPORT void rotate_3f(enum Rotation rot, float &x, float &y, float &z); + template -static constexpr bool rotate_3(enum Rotation rot, T &x, T &y, T &z) +static bool rotate_3(enum Rotation rot, T &x, T &y, T &z) { switch (rot) { case ROTATION_NONE: @@ -371,35 +381,3 @@ static constexpr bool rotate_3(enum Rotation rot, T &x, T &y, T &z) return false; } - -/** - * rotate a 3 element int16_t vector in-place - */ -__EXPORT inline void rotate_3i(enum Rotation rot, int16_t &x, int16_t &y, int16_t &z) -{ - if (!rotate_3(rot, x, y, z)) { - // otherwise use full rotation matrix for valid rotations - if (rot < ROTATION_MAX) { - const matrix::Vector3f r{get_rot_matrix(rot) *matrix::Vector3f{(float)x, (float)y, (float)z}}; - x = math::constrain(roundf(r(0)), (float)INT16_MIN, (float)INT16_MAX); - y = math::constrain(roundf(r(1)), (float)INT16_MIN, (float)INT16_MAX); - z = math::constrain(roundf(r(2)), (float)INT16_MIN, (float)INT16_MAX); - } - } -} - -/** - * rotate a 3 element float vector in-place - */ -__EXPORT inline void rotate_3f(enum Rotation rot, float &x, float &y, float &z) -{ - if (!rotate_3(rot, x, y, z)) { - // otherwise use full rotation matrix for valid rotations - if (rot < ROTATION_MAX) { - const matrix::Vector3f r{get_rot_matrix(rot) *matrix::Vector3f{x, y, z}}; - x = r(0); - y = r(1); - z = r(2); - } - } -} From adce6566aee962f08bf7067dc17f498f1caccdfd Mon Sep 17 00:00:00 2001 From: Alexander Lerach Date: Thu, 14 Nov 2024 15:20:47 +0100 Subject: [PATCH 3/3] Un-inlined get_table_data --- src/lib/world_magnetic_model/geo_mag_declination.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/world_magnetic_model/geo_mag_declination.cpp b/src/lib/world_magnetic_model/geo_mag_declination.cpp index 64738426f501..46e4cb6ec020 100644 --- a/src/lib/world_magnetic_model/geo_mag_declination.cpp +++ b/src/lib/world_magnetic_model/geo_mag_declination.cpp @@ -53,7 +53,7 @@ using math::constrain; -static constexpr unsigned get_lookup_table_index(float *val, float min, float max) +static unsigned get_lookup_table_index(float *val, float min, float max) { /* for the rare case of hitting the bounds exactly * the rounding logic wouldn't fit, so enforce it. @@ -66,7 +66,7 @@ static constexpr unsigned get_lookup_table_index(float *val, float min, float ma return static_cast((-(min) + *val) / SAMPLING_RES); } -static constexpr float get_table_data(float latitude_deg, float longitude_deg, const int16_t table[LAT_DIM][LON_DIM]) +static float get_table_data(float latitude_deg, float longitude_deg, const int16_t table[LAT_DIM][LON_DIM]) { latitude_deg = math::constrain(latitude_deg, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT);