Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

FMUv5x/v6x Remove unused sensors, non-inline some functions #23944

Closed
wants to merge 3 commits into from
Closed

FMUv5x/v6x Remove unused sensors, non-inline some functions #23944

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
64b0589
boards: Remove unused sensors from FMUv5x/v6x board config
niklaut Nov 14, 2024
7d9d8f7
Un-inlined rotate_3
alexcekay Nov 14, 2024
adce656
Un-inlined get_table_data
alexcekay Nov 14, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 2 additions & 7 deletions boards/px4/fmu-v5x/default.px4board
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down
24 changes: 6 additions & 18 deletions boards/px4/fmu-v5x/init/rc.board_sensors
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down
10 changes: 2 additions & 8 deletions boards/px4/fmu-v6x/default.px4board
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down
95 changes: 11 additions & 84 deletions boards/px4/fmu-v6x/init/rc.board_sensors
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -71,102 +71,29 @@ 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

# 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
Expand Down
2 changes: 0 additions & 2 deletions src/drivers/distance_sensor/Kconfig
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 0 additions & 2 deletions src/drivers/magnetometer/Kconfig
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
28 changes: 28 additions & 0 deletions src/lib/conversion/rotation.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
}
}
44 changes: 11 additions & 33 deletions src/lib/conversion/rotation.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<typename T>
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:
Expand Down Expand Up @@ -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);
}
}
}
4 changes: 2 additions & 2 deletions src/lib/world_magnetic_model/geo_mag_declination.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -66,7 +66,7 @@ static constexpr unsigned get_lookup_table_index(float *val, float min, float ma
return static_cast<unsigned>((-(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);

Expand Down
Loading