From c7cec4252cf2d1865da89698b7e546184a274c5e Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 17 Jun 2022 15:39:41 -0400 Subject: [PATCH] sensors: add CONFIG_SENSORS_VEHICLE_AIRSPEED for airspeed/differential pressure - disable CONFIG_SENSORS_VEHICLE_AIRSPEED on boards only used for multicopter --- boards/ark/can-flow/default.px4board | 2 + boards/ark/can-gps/default.px4board | 9 +- boards/ark/can-rtk-gps/default.px4board | 9 +- boards/bitcraze/crazyflie/default.px4board | 1 + boards/bitcraze/crazyflie21/default.px4board | 1 + boards/holybro/kakutef7/default.px4board | 1 + boards/holybro/kakuteh7/default.px4board | 9 +- boards/omnibus/f4sd/default.px4board | 1 + boards/spracing/h7extreme/default.px4board | 1 + src/modules/sensors/CMakeLists.txt | 5 +- src/modules/sensors/Kconfig | 4 + src/modules/sensors/sensors.cpp | 94 ++++++++++--------- .../sensors/vehicle_air_data/CMakeLists.txt | 1 + .../vehicle_magnetometer/CMakeLists.txt | 1 + 14 files changed, 89 insertions(+), 50 deletions(-) diff --git a/boards/ark/can-flow/default.px4board b/boards/ark/can-flow/default.px4board index 6df227e138..48b3132cc3 100644 --- a/boards/ark/can-flow/default.px4board +++ b/boards/ark/can-flow/default.px4board @@ -13,6 +13,7 @@ CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set # CONFIG_SENSORS_VEHICLE_AIR_DATA is not set # CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set # CONFIG_SENSORS_VEHICLE_MAGNETOMETER is not set @@ -21,4 +22,5 @@ CONFIG_SYSTEMCMDS_PERF=y CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/ark/can-gps/default.px4board b/boards/ark/can-gps/default.px4board index ad4ec22ad8..151aeda7bc 100644 --- a/boards/ark/can-gps/default.px4board +++ b/boards/ark/can-gps/default.px4board @@ -4,7 +4,6 @@ CONFIG_BOARD_ROMFSROOT="cannode" CONFIG_BOARD_CONSTRAINED_FLASH=y CONFIG_BOARD_NO_HELP=y CONFIG_BOARD_CONSTRAINED_MEMORY=y -CONFIG_BOARD_EXTERNAL_METADATA=y CONFIG_DRIVERS_BAROMETER_BMP388=y CONFIG_DRIVERS_BOOTLOADERS=y CONFIG_DRIVERS_GPS=y @@ -14,5 +13,13 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_TONE_ALARM=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/ark/can-rtk-gps/default.px4board b/boards/ark/can-rtk-gps/default.px4board index ad4ec22ad8..151aeda7bc 100644 --- a/boards/ark/can-rtk-gps/default.px4board +++ b/boards/ark/can-rtk-gps/default.px4board @@ -4,7 +4,6 @@ CONFIG_BOARD_ROMFSROOT="cannode" CONFIG_BOARD_CONSTRAINED_FLASH=y CONFIG_BOARD_NO_HELP=y CONFIG_BOARD_CONSTRAINED_MEMORY=y -CONFIG_BOARD_EXTERNAL_METADATA=y CONFIG_DRIVERS_BAROMETER_BMP388=y CONFIG_DRIVERS_BOOTLOADERS=y CONFIG_DRIVERS_GPS=y @@ -14,5 +13,13 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_TONE_ALARM=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/bitcraze/crazyflie/default.px4board b/boards/bitcraze/crazyflie/default.px4board index 80437d3812..41509df016 100644 --- a/boards/bitcraze/crazyflie/default.px4board +++ b/boards/bitcraze/crazyflie/default.px4board @@ -30,6 +30,7 @@ CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set CONFIG_SYSTEMCMDS_DMESG=y CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y CONFIG_SYSTEMCMDS_MFT=y diff --git a/boards/bitcraze/crazyflie21/default.px4board b/boards/bitcraze/crazyflie21/default.px4board index 61b075eb0c..5f06c0fef9 100644 --- a/boards/bitcraze/crazyflie21/default.px4board +++ b/boards/bitcraze/crazyflie21/default.px4board @@ -30,6 +30,7 @@ CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set CONFIG_SYSTEMCMDS_DMESG=y CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y CONFIG_SYSTEMCMDS_MIXER=y diff --git a/boards/holybro/kakutef7/default.px4board b/boards/holybro/kakutef7/default.px4board index f2c412b7c4..d82f8bb1da 100644 --- a/boards/holybro/kakutef7/default.px4board +++ b/boards/holybro/kakutef7/default.px4board @@ -32,6 +32,7 @@ CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set CONFIG_SYSTEMCMDS_MIXER=y CONFIG_SYSTEMCMDS_PARAM=y CONFIG_SYSTEMCMDS_PWM=y diff --git a/boards/holybro/kakuteh7/default.px4board b/boards/holybro/kakuteh7/default.px4board index 095afea53d..f124262a0b 100644 --- a/boards/holybro/kakuteh7/default.px4board +++ b/boards/holybro/kakuteh7/default.px4board @@ -15,10 +15,10 @@ CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y -CONFIG_DRIVERS_OSD=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_PWM_OUT_SIM=y @@ -28,8 +28,8 @@ CONFIG_DRIVERS_RPM=y CONFIG_DRIVERS_SMART_BATTERY_BATMON=y CONFIG_COMMON_TELEMETRY=y CONFIG_DRIVERS_TONE_ALARM=y -CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y @@ -42,6 +42,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y CONFIG_MODULES_LAND_DETECTOR=y @@ -60,12 +61,12 @@ CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set CONFIG_MODULES_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y -CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_VTOL_ATT_CONTROL=y -CONFIG_SYSTEMCMDS_BL_UPDATE=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y CONFIG_SYSTEMCMDS_DMESG=y CONFIG_SYSTEMCMDS_DUMPFILE=y CONFIG_SYSTEMCMDS_GPIO=y diff --git a/boards/omnibus/f4sd/default.px4board b/boards/omnibus/f4sd/default.px4board index 942328ce95..05506d3c2d 100644 --- a/boards/omnibus/f4sd/default.px4board +++ b/boards/omnibus/f4sd/default.px4board @@ -32,6 +32,7 @@ CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y CONFIG_SYSTEMCMDS_MIXER=y CONFIG_SYSTEMCMDS_PARAM=y diff --git a/boards/spracing/h7extreme/default.px4board b/boards/spracing/h7extreme/default.px4board index 5e50b09833..d1463c40f6 100644 --- a/boards/spracing/h7extreme/default.px4board +++ b/boards/spracing/h7extreme/default.px4board @@ -41,6 +41,7 @@ CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set CONFIG_SYSTEMCMDS_DMESG=y CONFIG_SYSTEMCMDS_DUMPFILE=y CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 428e5d8fde..120c478212 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -61,7 +61,6 @@ px4_add_module( MODULE_CONFIG module.yaml DEPENDS - airspeed conversion data_validator mathlib @@ -71,6 +70,10 @@ px4_add_module( vehicle_imu ) +if(CONFIG_SENSORS_VEHICLE_AIRSPEED) + target_link_libraries(modules__sensors PRIVATE airspeed) +endif() + if(CONFIG_SENSORS_VEHICLE_AIR_DATA) target_link_libraries(modules__sensors PRIVATE vehicle_air_data) endif() diff --git a/src/modules/sensors/Kconfig b/src/modules/sensors/Kconfig index cc697a3938..67863ec53f 100644 --- a/src/modules/sensors/Kconfig +++ b/src/modules/sensors/Kconfig @@ -12,6 +12,10 @@ menuconfig USER_SENSORS Put sensors in userspace memory if MODULES_SENSORS + config SENSORS_VEHICLE_AIRSPEED + bool "Include vehicle airspeed" + default y + config SENSORS_VEHICLE_AIR_DATA bool "Include vehicle air data" default y diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6706fa3de2..a9d1840fb4 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -41,10 +41,7 @@ * @author Beat Küng */ -#include #include -#include -#include #include #include #include @@ -62,23 +59,27 @@ #include #include #include -#include -#include #include -#include #include -#include #include #include #include "voted_sensors_update.h" #include "vehicle_acceleration/VehicleAcceleration.hpp" #include "vehicle_angular_velocity/VehicleAngularVelocity.hpp" - - #include "vehicle_imu/VehicleIMU.hpp" +#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) +# include +# include +# include +# include +# include +# include +#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED + #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) +# include # include "vehicle_air_data/VehicleAirData.hpp" #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA @@ -140,16 +141,18 @@ private: }; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - - uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)}; uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; - uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; - uORB::Publication _airspeed_pub{ORB_ID(airspeed)}; uORB::Publication _sensor_pub{ORB_ID(sensor_combined)}; perf_counter_t _loop_perf; /**< loop performance counter */ +#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) + uORB::Subscription _diff_pres_sub {ORB_ID(differential_pressure)}; + uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; + + uORB::Publication _airspeed_pub{ORB_ID(airspeed)}; + DataValidator _airspeed_validator; /**< data validator to monitor airspeed */ uint64_t _airspeed_last_publish{0}; @@ -159,11 +162,10 @@ private: float _baro_pressure_sum{0.f}; int _diff_pres_count{0}; -#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL +# ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL uORB::Subscription _adc_report_sub {ORB_ID(adc_report)}; uORB::PublicationMulti _diff_pres_pub{ORB_ID(differential_pressure)}; -#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ - +# endif // ADC_AIRSPEED_VOLTAGE_CHANNEL struct Parameters { float diff_pres_offset_pa; @@ -186,6 +188,7 @@ private: param_t air_tube_length; param_t air_tube_diameter_mm; } _parameter_handles{}; /**< handles for interesting parameters */ +#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED VotedSensorsUpdate _voted_sensors_update; @@ -217,6 +220,7 @@ private: */ int parameters_update(); +#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) /** * Poll the differential pressure sensor for updated data. * @@ -225,11 +229,6 @@ private: */ void diff_pres_poll(); - /** - * Check for changes in parameters. - */ - void parameter_update_poll(bool forced = false); - /** * Poll the ADC and update readings to suit. * @@ -237,6 +236,7 @@ private: * data should be returned. */ void adc_poll(); +#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED void InitializeVehicleAirData(); void InitializeVehicleGPSPosition(); @@ -244,9 +244,15 @@ private: void InitializeVehicleMagnetometer(); DEFINE_PARAMETERS( +#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) (ParamBool) _param_sys_has_baro, +#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA +#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) (ParamBool) _param_sys_has_gps, +#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION +#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) (ParamBool) _param_sys_has_mag, +#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER (ParamBool) _param_sens_imu_mode ) }; @@ -263,6 +269,7 @@ Sensors::Sensors(bool hil_enabled) : _vehicle_angular_velocity.Start(); _vehicle_acceleration.Start(); +#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) /* Differential pressure offset */ _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL @@ -273,6 +280,10 @@ Sensors::Sensors(bool hil_enabled) : _parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN"); _parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM"); + _airspeed_validator.set_timeout(300000); + _airspeed_validator.set_equal_value_threshold(100); +#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED + param_find("SYS_FAC_CAL_MODE"); // Parameters controlling the on-board sensor thermal calibrator @@ -282,9 +293,6 @@ Sensors::Sensors(bool hil_enabled) : _sensor_combined.accelerometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID; - _airspeed_validator.set_timeout(300000); - _airspeed_validator.set_equal_value_threshold(100); - parameters_update(); InitializeVehicleIMU(); @@ -350,6 +358,7 @@ int Sensors::parameters_update() return 0; } +#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) /* Airspeed offset */ param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL @@ -359,6 +368,7 @@ int Sensors::parameters_update() param_get(_parameter_handles.air_cmodel, &_parameters.air_cmodel); param_get(_parameter_handles.air_tube_length, &_parameters.air_tube_length); param_get(_parameter_handles.air_tube_diameter_mm, &_parameters.air_tube_diameter_mm); +#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED _voted_sensors_update.parametersUpdate(); @@ -438,6 +448,7 @@ int Sensors::parameters_update() return PX4_OK; } +#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) void Sensors::diff_pres_poll() { differential_pressure_s diff_pres{}; @@ -545,21 +556,6 @@ void Sensors::diff_pres_poll() } } -void -Sensors::parameter_update_poll(bool forced) -{ - // check for parameter updates - if (_parameter_update_sub.updated() || forced) { - // clear update - parameter_update_s pupdate; - _parameter_update_sub.copy(&pupdate); - - // update parameters from storage - parameters_update(); - updateParams(); - } -} - void Sensors::adc_poll() { /* only read if not in HIL mode */ @@ -608,8 +604,9 @@ void Sensors::adc_poll() } } -#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ +#endif // ADC_AIRSPEED_VOLTAGE_CHANNEL } +#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED) #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) void Sensors::InitializeVehicleAirData() @@ -768,8 +765,16 @@ void Sensors::Run() _last_config_update = hrt_absolute_time(); } else { - // check parameters for updates - parameter_update_poll(); + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + parameters_update(); + updateParams(); + } } _voted_sensors_update.sensorsPoll(_sensor_combined); @@ -781,10 +786,11 @@ void Sensors::Run() _sensor_combined_prev_timestamp = _sensor_combined.timestamp; } +#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) // check analog airspeed adc_poll(); - diff_pres_poll(); +#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED // backup schedule as a watchdog timeout ScheduleDelayed(10_ms); @@ -865,9 +871,11 @@ int Sensors::print_status() #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA +#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) PX4_INFO_RAW("\n"); PX4_INFO_RAW("Airspeed status:\n"); _airspeed_validator.print(); +#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED PX4_INFO_RAW("\n"); _vehicle_acceleration.PrintStatus(); diff --git a/src/modules/sensors/vehicle_air_data/CMakeLists.txt b/src/modules/sensors/vehicle_air_data/CMakeLists.txt index 55ece1fcac..c8457cc2df 100644 --- a/src/modules/sensors/vehicle_air_data/CMakeLists.txt +++ b/src/modules/sensors/vehicle_air_data/CMakeLists.txt @@ -37,6 +37,7 @@ px4_add_library(vehicle_air_data ) target_link_libraries(vehicle_air_data PRIVATE + data_validator px4_work_queue sensor_calibration ) diff --git a/src/modules/sensors/vehicle_magnetometer/CMakeLists.txt b/src/modules/sensors/vehicle_magnetometer/CMakeLists.txt index a864b955e2..0fad202f50 100644 --- a/src/modules/sensors/vehicle_magnetometer/CMakeLists.txt +++ b/src/modules/sensors/vehicle_magnetometer/CMakeLists.txt @@ -38,6 +38,7 @@ px4_add_library(vehicle_magnetometer target_link_libraries(vehicle_magnetometer PRIVATE + data_validator px4_work_queue sensor_calibration )