From c46fa01195b2b3b2223767f430bf6553447a0bc2 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 17 Jun 2022 14:56:49 -0400 Subject: [PATCH] sensors: add kconfig mechanism to optionally exclude sensor types --- src/modules/sensors/CMakeLists.txt | 33 ++++- src/modules/sensors/Kconfig | 15 ++ src/modules/sensors/sensors.cpp | 215 +++++++++++++++++++++-------- 3 files changed, 195 insertions(+), 68 deletions(-) diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 0eb8a7e0a9..428e5d8fde 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-2022 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -34,12 +34,22 @@ add_subdirectory(data_validator) include_directories(${CMAKE_CURRENT_SOURCE_DIR}) + add_subdirectory(vehicle_acceleration) add_subdirectory(vehicle_angular_velocity) -add_subdirectory(vehicle_air_data) -add_subdirectory(vehicle_gps_position) add_subdirectory(vehicle_imu) -add_subdirectory(vehicle_magnetometer) + +if(CONFIG_SENSORS_VEHICLE_AIR_DATA) + add_subdirectory(vehicle_air_data) +endif() + +if(CONFIG_SENSORS_VEHICLE_GPS_POSITION) + add_subdirectory(vehicle_gps_position) +endif() + +if(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) + add_subdirectory(vehicle_magnetometer) +endif() px4_add_module( MODULE modules__sensors @@ -58,8 +68,17 @@ px4_add_module( sensor_calibration vehicle_acceleration vehicle_angular_velocity - vehicle_air_data - vehicle_gps_position vehicle_imu - vehicle_magnetometer ) + +if(CONFIG_SENSORS_VEHICLE_AIR_DATA) + target_link_libraries(modules__sensors PRIVATE vehicle_air_data) +endif() + +if(CONFIG_SENSORS_VEHICLE_GPS_POSITION) + target_link_libraries(modules__sensors PRIVATE vehicle_gps_position) +endif() + +if(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) + target_link_libraries(modules__sensors PRIVATE vehicle_magnetometer) +endif() diff --git a/src/modules/sensors/Kconfig b/src/modules/sensors/Kconfig index d7edc7cf74..cc697a3938 100644 --- a/src/modules/sensors/Kconfig +++ b/src/modules/sensors/Kconfig @@ -10,3 +10,18 @@ menuconfig USER_SENSORS depends on BOARD_PROTECTED && MODULES_SENSORS ---help--- Put sensors in userspace memory + +if MODULES_SENSORS + config SENSORS_VEHICLE_AIR_DATA + bool "Include vehicle air data" + default y + + config SENSORS_VEHICLE_GPS_POSITION + bool "Include vehicle gps position" + default y + + config SENSORS_VEHICLE_MAGNETOMETER + bool "Include vehicle magnetometer" + default y + +endif #MODULES_SENSORS diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 30ba6aeaac..6706fa3de2 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -48,6 +48,7 @@ #include #include #include + #include #include #include @@ -61,7 +62,6 @@ #include #include #include -#include #include #include #include @@ -74,10 +74,23 @@ #include "voted_sensors_update.h" #include "vehicle_acceleration/VehicleAcceleration.hpp" #include "vehicle_angular_velocity/VehicleAngularVelocity.hpp" -#include "vehicle_air_data/VehicleAirData.hpp" -#include "vehicle_gps_position/VehicleGPSPosition.hpp" + + #include "vehicle_imu/VehicleIMU.hpp" -#include "vehicle_magnetometer/VehicleMagnetometer.hpp" + +#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) +# include "vehicle_air_data/VehicleAirData.hpp" +#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA + +#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) +# include "vehicle_gps_position/VehicleGPSPosition.hpp" +#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION + +#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) +# include "vehicle_magnetometer/VehicleMagnetometer.hpp" +# include +# include +#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER using namespace sensors; using namespace time_literals; @@ -178,17 +191,26 @@ private: VehicleAcceleration _vehicle_acceleration; VehicleAngularVelocity _vehicle_angular_velocity; - VehicleAirData *_vehicle_air_data{nullptr}; - VehicleMagnetometer *_vehicle_magnetometer{nullptr}; - VehicleGPSPosition *_vehicle_gps_position{nullptr}; - VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {}; +#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) + VehicleAirData *_vehicle_air_data {nullptr}; + uint8_t _n_baro{0}; +#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA + +#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) + VehicleMagnetometer *_vehicle_magnetometer {nullptr}; + uint8_t _n_mag{0}; +#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER + +#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) + VehicleGPSPosition *_vehicle_gps_position {nullptr}; + uint8_t _n_gps{0}; +#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION + + VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {}; uint8_t _n_accel{0}; - uint8_t _n_baro{0}; - uint8_t _n_gps{0}; uint8_t _n_gyro{0}; - uint8_t _n_mag{0}; /** * Update our local parameter cache. @@ -265,10 +287,7 @@ Sensors::Sensors(bool hil_enabled) : parameters_update(); - InitializeVehicleAirData(); - InitializeVehicleGPSPosition(); InitializeVehicleIMU(); - InitializeVehicleMagnetometer(); } Sensors::~Sensors() @@ -281,21 +300,33 @@ Sensors::~Sensors() _vehicle_acceleration.Stop(); _vehicle_angular_velocity.Stop(); +#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) + if (_vehicle_air_data) { _vehicle_air_data->Stop(); delete _vehicle_air_data; } +#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA + +#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) + if (_vehicle_gps_position) { _vehicle_gps_position->Stop(); delete _vehicle_gps_position; } +#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION + +#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) + if (_vehicle_magnetometer) { _vehicle_magnetometer->Stop(); delete _vehicle_magnetometer; } +#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER + for (auto &vehicle_imu : _vehicle_imu_list) { if (vehicle_imu) { vehicle_imu->Stop(); @@ -331,61 +362,78 @@ int Sensors::parameters_update() _voted_sensors_update.parametersUpdate(); - // mark all existing sensor calibrations active even if sensor is missing - // this preserves the calibration in the event of a parameter export while the sensor is missing - for (int i = 0; i < MAX_SENSOR_COUNT; i++) { - uint32_t device_id_accel = calibration::GetCalibrationParamInt32("ACC", "ID", i); - uint32_t device_id_gyro = calibration::GetCalibrationParamInt32("GYRO", "ID", i); - uint32_t device_id_mag = calibration::GetCalibrationParamInt32("MAG", "ID", i); - - if (device_id_accel != 0) { - calibration::Accelerometer accel_cal(device_id_accel); - } - - if (device_id_gyro != 0) { - calibration::Gyroscope gyro_cal(device_id_gyro); - } - - if (device_id_mag != 0) { - calibration::Magnetometer mag_cal(device_id_mag); - } - } - - // ensure calibration slots are active for the number of sensors currently available - // this to done to eliminate differences in the active set of parameters before and after sensor calibration + // 1. mark all existing sensor calibrations active even if sensor is missing + // this preserves the calibration in the event of a parameter export while the sensor is missing + // 2. ensure calibration slots are active for the number of sensors currently available + // this to done to eliminate differences in the active set of parameters before and after sensor calibration for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { - // sensor_accel - uORB::SubscriptionData sensor_accel_sub{ORB_ID(sensor_accel), i}; + { + const uint32_t device_id_accel = calibration::GetCalibrationParamInt32("ACC", "ID", i); - if (sensor_accel_sub.advertised() && (sensor_accel_sub.get().device_id != 0)) { - calibration::Accelerometer cal; - cal.set_calibration_index(i); - cal.ParametersLoad(); + if (device_id_accel != 0) { + calibration::Accelerometer accel_cal(device_id_accel); + } + + uORB::SubscriptionData sensor_accel_sub{ORB_ID(sensor_accel), i}; + + if (sensor_accel_sub.advertised() && (sensor_accel_sub.get().device_id != 0)) { + calibration::Accelerometer cal; + cal.set_calibration_index(i); + cal.ParametersLoad(); + } } + // sensor_gyro - uORB::SubscriptionData sensor_gyro_sub{ORB_ID(sensor_gyro), i}; + { + const uint32_t device_id_gyro = calibration::GetCalibrationParamInt32("GYRO", "ID", i); - if (sensor_gyro_sub.advertised() && (sensor_gyro_sub.get().device_id != 0)) { - calibration::Gyroscope cal; - cal.set_calibration_index(i); - cal.ParametersLoad(); + if (device_id_gyro != 0) { + calibration::Gyroscope gyro_cal(device_id_gyro); + } + + uORB::SubscriptionData sensor_gyro_sub{ORB_ID(sensor_gyro), i}; + + if (sensor_gyro_sub.advertised() && (sensor_gyro_sub.get().device_id != 0)) { + calibration::Gyroscope cal; + cal.set_calibration_index(i); + cal.ParametersLoad(); + } } + +#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) // sensor_mag - uORB::SubscriptionData sensor_mag_sub{ORB_ID(sensor_mag), i}; + { + uint32_t device_id_mag = calibration::GetCalibrationParamInt32("MAG", "ID", i); - if (sensor_mag_sub.advertised() && (sensor_mag_sub.get().device_id != 0)) { - calibration::Magnetometer cal; - cal.set_calibration_index(i); - cal.ParametersLoad(); + if (device_id_mag != 0) { + calibration::Magnetometer mag_cal(device_id_mag); + } + + uORB::SubscriptionData sensor_mag_sub{ORB_ID(sensor_mag), i}; + + if (sensor_mag_sub.advertised() && (sensor_mag_sub.get().device_id != 0)) { + calibration::Magnetometer cal; + cal.set_calibration_index(i); + cal.ParametersLoad(); + } } +#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER } +#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) InitializeVehicleAirData(); +#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA + +#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) InitializeVehicleGPSPosition(); +#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION + +#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) InitializeVehicleMagnetometer(); +#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER return PX4_OK; } @@ -563,6 +611,7 @@ void Sensors::adc_poll() #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ } +#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) void Sensors::InitializeVehicleAirData() { if (_param_sys_has_baro.get()) { @@ -575,7 +624,9 @@ void Sensors::InitializeVehicleAirData() } } } +#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA +#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) void Sensors::InitializeVehicleGPSPosition() { if (_param_sys_has_gps.get()) { @@ -588,6 +639,7 @@ void Sensors::InitializeVehicleGPSPosition() } } } +#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION void Sensors::InitializeVehicleIMU() { @@ -624,6 +676,7 @@ void Sensors::InitializeVehicleIMU() } } +#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) void Sensors::InitializeVehicleMagnetometer() { if (_param_sys_has_mag.get()) { @@ -636,6 +689,7 @@ void Sensors::InitializeVehicleMagnetometer() } } } +#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER void Sensors::Run() { @@ -664,18 +718,45 @@ void Sensors::Run() // when not adding sensors poll for param updates if ((!_armed && hrt_elapsed_time(&_last_config_update) > 500_ms) || (_last_config_update == 0)) { - const int n_accel = orb_group_count(ORB_ID(sensor_accel)); - const int n_baro = orb_group_count(ORB_ID(sensor_baro)); - const int n_gps = orb_group_count(ORB_ID(sensor_gps)); - const int n_gyro = orb_group_count(ORB_ID(sensor_gyro)); - const int n_mag = orb_group_count(ORB_ID(sensor_mag)); + bool updated = false; - if ((n_accel != _n_accel) || (n_baro != _n_baro) || (n_gps != _n_gps) || (n_gyro != _n_gyro) || (n_mag != _n_mag)) { - _n_accel = n_accel; +#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) + const int n_baro = orb_group_count(ORB_ID(sensor_baro)); + + if (n_baro != _n_baro) { _n_baro = n_baro; + updated = true; + } + +#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA + +#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) + const int n_gps = orb_group_count(ORB_ID(sensor_gps)); + + if (n_gps != _n_gps) { _n_gps = n_gps; - _n_gyro = n_gyro; + updated = true; + } + +#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION + +#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) + const int n_mag = orb_group_count(ORB_ID(sensor_mag)); + + if (n_mag != _n_mag) { _n_mag = n_mag; + updated = true; + } + +#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER + + + const int n_accel = orb_group_count(ORB_ID(sensor_accel)); + const int n_gyro = orb_group_count(ORB_ID(sensor_gyro)); + + if ((n_accel != _n_accel) || (n_gyro != _n_gyro) || updated) { + _n_accel = n_accel; + _n_gyro = n_gyro; parameters_update(); } @@ -766,16 +847,24 @@ int Sensors::print_status() { _voted_sensors_update.printStatus(); +#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) + if (_vehicle_magnetometer) { PX4_INFO_RAW("\n"); _vehicle_magnetometer->PrintStatus(); } +#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER + +#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) + if (_vehicle_air_data) { PX4_INFO_RAW("\n"); _vehicle_air_data->PrintStatus(); } +#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA + PX4_INFO_RAW("\n"); PX4_INFO_RAW("Airspeed status:\n"); _airspeed_validator.print(); @@ -786,11 +875,15 @@ int Sensors::print_status() PX4_INFO_RAW("\n"); _vehicle_angular_velocity.PrintStatus(); +#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) + if (_vehicle_gps_position) { PX4_INFO_RAW("\n"); _vehicle_gps_position->PrintStatus(); } +#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION + PX4_INFO_RAW("\n"); for (auto &i : _vehicle_imu_list) {