diff --git a/apps/estimator/sensors/data_validator/SConscript b/apps/estimator/sensors/data_validator/SConscript index c916b0ba62..f8be687c68 100644 --- a/apps/estimator/sensors/data_validator/SConscript +++ b/apps/estimator/sensors/data_validator/SConscript @@ -7,6 +7,6 @@ cwd = GetCurrentDir() inc = [] src = Glob("*.cpp", exclude="*_test.cpp") -objs = DefineGroup("ins/sensors", src, depend=["INS_USING_SENSOR"]) +objs = DefineGroup("ins/sensors", src, depend=["INS_USING_SENSORS"]) Return("objs") diff --git a/apps/estimator/sensors/sensors.cpp b/apps/estimator/sensors/sensors.cpp index f48de2f22c..f6fe372589 100644 --- a/apps/estimator/sensors/sensors.cpp +++ b/apps/estimator/sensors/sensors.cpp @@ -18,6 +18,8 @@ * @author Beat Küng */ +#define LOG_TAG "sensors" + #include "sensors.hpp" Sensors::Sensors(bool hil_enabled) : @@ -28,11 +30,11 @@ Sensors::Sensors(bool hil_enabled) : _voted_sensors_update(hil_enabled, _vehicle_imu_sub) { _sensor_pub.advertise(); -#if defined(CONFIG_SENSORS_VEHICLE_ACCELERATION) +#if defined(SENSORS_VEHICLE_ACCELERATION) _vehicle_acceleration.Start(); -#endif // CONFIG_SENSORS_VEHICLE_ACCELERATION +#endif // SENSORS_VEHICLE_ACCELERATION -#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) +#if defined(SENSORS_VEHICLE_AIRSPEED) /* Differential pressure offset */ _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); # ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL @@ -45,11 +47,11 @@ Sensors::Sensors(bool hil_enabled) : _airspeed_validator.set_timeout(300000); _airspeed_validator.set_equal_value_threshold(100); -#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED +#endif // SENSORS_VEHICLE_AIRSPEED -#if defined(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY) +#if defined(SENSORS_VEHICLE_ANGULAR_VELOCITY) _vehicle_angular_velocity.Start(); -#endif // CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY +#endif // SENSORS_VEHICLE_ANGULAR_VELOCITY param_find("SYS_FAC_CAL_MODE"); @@ -71,49 +73,49 @@ Sensors::~Sensors() { sub.unregisterCallback(); } -#if defined(CONFIG_SENSORS_VEHICLE_ACCELERATION) +#if defined(SENSORS_VEHICLE_ACCELERATION) _vehicle_acceleration.Stop(); -#endif // CONFIG_SENSORS_VEHICLE_ACCELERATION +#endif // SENSORS_VEHICLE_ACCELERATION -#if defined(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY) +#if defined(SENSORS_VEHICLE_ANGULAR_VELOCITY) _vehicle_angular_velocity.Stop(); -#endif // CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY +#endif // SENSORS_VEHICLE_ANGULAR_VELOCITY -#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) +#if defined(SENSORS_VEHICLE_AIR_DATA) if (_vehicle_air_data) { _vehicle_air_data->Stop(); delete _vehicle_air_data; } -#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA +#endif // SENSORS_VEHICLE_AIR_DATA -#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) +#if defined(SENSORS_VEHICLE_GPS_POSITION) if (_vehicle_gps_position) { _vehicle_gps_position->Stop(); delete _vehicle_gps_position; } -#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION +#endif // SENSORS_VEHICLE_GPS_POSITION -#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) +#if defined(SENSORS_VEHICLE_MAGNETOMETER) if (_vehicle_magnetometer) { _vehicle_magnetometer->Stop(); delete _vehicle_magnetometer; } -#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER +#endif // SENSORS_VEHICLE_MAGNETOMETER -#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) +#if defined(SENSORS_VEHICLE_OPTICAL_FLOW) if (_vehicle_optical_flow) { _vehicle_optical_flow->Stop(); delete _vehicle_optical_flow; } -#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW +#endif // SENSORS_VEHICLE_OPTICAL_FLOW for (auto &vehicle_imu : _vehicle_imu_list) { if (vehicle_imu) { @@ -136,7 +138,7 @@ int Sensors::parameters_update() { return 0; } -#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) +#if defined(SENSORS_VEHICLE_AIRSPEED) /* Airspeed offset */ param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); # ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL @@ -146,7 +148,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 +#endif // SENSORS_VEHICLE_AIRSPEED _voted_sensors_update.parametersUpdate(); @@ -189,7 +191,7 @@ int Sensors::parameters_update() { } } -#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) +#if defined(SENSORS_VEHICLE_MAGNETOMETER) // sensor_mag { uint32_t device_id_mag = calibration::GetCalibrationParamInt32("MAG", "ID", i); @@ -206,29 +208,29 @@ int Sensors::parameters_update() { cal.ParametersLoad(); } } -#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER +#endif // SENSORS_VEHICLE_MAGNETOMETER } -#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) +#if defined(SENSORS_VEHICLE_AIR_DATA) InitializeVehicleAirData(); -#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA +#endif // SENSORS_VEHICLE_AIR_DATA -#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) +#if defined(SENSORS_VEHICLE_GPS_POSITION) InitializeVehicleGPSPosition(); -#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION +#endif // SENSORS_VEHICLE_GPS_POSITION -#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) +#if defined(SENSORS_VEHICLE_MAGNETOMETER) InitializeVehicleMagnetometer(); -#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER +#endif // SENSORS_VEHICLE_MAGNETOMETER -#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) +#if defined(SENSORS_VEHICLE_OPTICAL_FLOW) InitializeVehicleOpticalFlow(); -#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW +#endif // SENSORS_VEHICLE_OPTICAL_FLOW return PX4_OK; } -#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) +#if defined(SENSORS_VEHICLE_AIRSPEED) void Sensors::diff_pres_poll() { differential_pressure_s diff_pres{}; @@ -376,9 +378,9 @@ void Sensors::adc_poll() { # endif // ADC_AIRSPEED_VOLTAGE_CHANNEL } -#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED) +#endif // SENSORS_VEHICLE_AIRSPEED) -#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) +#if defined(SENSORS_VEHICLE_AIR_DATA) void Sensors::InitializeVehicleAirData() { if (_param_sys_has_baro.get()) { if (_vehicle_air_data == nullptr) { @@ -390,9 +392,9 @@ void Sensors::InitializeVehicleAirData() { } } } -#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA +#endif // SENSORS_VEHICLE_AIR_DATA -#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) +#if defined(SENSORS_VEHICLE_GPS_POSITION) void Sensors::InitializeVehicleGPSPosition() { if (_param_sys_has_gps.get()) { if (_vehicle_gps_position == nullptr) { @@ -404,7 +406,7 @@ void Sensors::InitializeVehicleGPSPosition() { } } } -#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION +#endif // SENSORS_VEHICLE_GPS_POSITION void Sensors::InitializeVehicleIMU() { // create a VehicleIMU instance for each accel/gyro pair @@ -439,7 +441,7 @@ void Sensors::InitializeVehicleIMU() { } } -#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) +#if defined(SENSORS_VEHICLE_MAGNETOMETER) void Sensors::InitializeVehicleMagnetometer() { if (_param_sys_has_mag.get()) { if (_vehicle_magnetometer == nullptr) { @@ -451,9 +453,9 @@ void Sensors::InitializeVehicleMagnetometer() { } } } -#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER +#endif // SENSORS_VEHICLE_MAGNETOMETER -#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) +#if defined(SENSORS_VEHICLE_OPTICAL_FLOW) void Sensors::InitializeVehicleOpticalFlow() { if (_vehicle_optical_flow == nullptr) { uORB::Subscription sensor_optical_flow_sub{ORB_ID(sensor_optical_flow)}; @@ -467,7 +469,7 @@ void Sensors::InitializeVehicleOpticalFlow() { } } } -#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW +#endif // SENSORS_VEHICLE_OPTICAL_FLOW void Sensors::Run() { if (should_exit()) { @@ -496,7 +498,7 @@ void Sensors::Run() { if ((!_armed && hrt_elapsed_time(&_last_config_update) > 500_ms) || (_last_config_update == 0)) { bool updated = false; -#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) +#if defined(SENSORS_VEHICLE_AIR_DATA) const int n_baro = orb_group_count(ORB_ID(sensor_baro)); if (n_baro != _n_baro) { @@ -504,9 +506,9 @@ void Sensors::Run() { updated = true; } -#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA +#endif // SENSORS_VEHICLE_AIR_DATA -#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) +#if defined(SENSORS_VEHICLE_GPS_POSITION) const int n_gps = orb_group_count(ORB_ID(sensor_gps)); if (n_gps != _n_gps) { @@ -514,9 +516,9 @@ void Sensors::Run() { updated = true; } -#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION +#endif // SENSORS_VEHICLE_GPS_POSITION -#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) +#if defined(SENSORS_VEHICLE_MAGNETOMETER) const int n_mag = orb_group_count(ORB_ID(sensor_mag)); if (n_mag != _n_mag) { @@ -524,9 +526,9 @@ void Sensors::Run() { updated = true; } -#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER +#endif // SENSORS_VEHICLE_MAGNETOMETER -#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) +#if defined(SENSORS_VEHICLE_OPTICAL_FLOW) const int n_optical_flow = orb_group_count(ORB_ID(sensor_optical_flow)); if (n_optical_flow != _n_optical_flow) { @@ -534,7 +536,7 @@ void Sensors::Run() { updated = true; } -#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW +#endif // SENSORS_VEHICLE_OPTICAL_FLOW const int n_accel = orb_group_count(ORB_ID(sensor_accel)); const int n_gyro = orb_group_count(ORB_ID(sensor_gyro)); @@ -573,11 +575,11 @@ void Sensors::Run() { _sensor_combined_prev_timestamp = _sensor_combined.timestamp; } -#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) +#if defined(SENSORS_VEHICLE_AIRSPEED) // check analog airspeed adc_poll(); diff_pres_poll(); -#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED +#endif // SENSORS_VEHICLE_AIRSPEED // backup schedule as a watchdog timeout ScheduleDelayed(10_ms); @@ -639,57 +641,57 @@ Sensors *Sensors::instantiate(int argc, char *argv[]) { int Sensors::print_status() { _voted_sensors_update.printStatus(); -#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) +#if defined(SENSORS_VEHICLE_MAGNETOMETER) if (_vehicle_magnetometer) { PX4_INFO_RAW("\n"); _vehicle_magnetometer->PrintStatus(); } -#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER +#endif // SENSORS_VEHICLE_MAGNETOMETER -#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) +#if defined(SENSORS_VEHICLE_AIR_DATA) if (_vehicle_air_data) { PX4_INFO_RAW("\n"); _vehicle_air_data->PrintStatus(); } -#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA +#endif // SENSORS_VEHICLE_AIR_DATA -#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) +#if defined(SENSORS_VEHICLE_AIRSPEED) PX4_INFO_RAW("\n"); PX4_INFO_RAW("Airspeed status:\n"); _airspeed_validator.print(); -#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED +#endif // SENSORS_VEHICLE_AIRSPEED -#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) +#if defined(SENSORS_VEHICLE_OPTICAL_FLOW) if (_vehicle_optical_flow) { PX4_INFO_RAW("\n"); _vehicle_optical_flow->PrintStatus(); } -#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW +#endif // SENSORS_VEHICLE_OPTICAL_FLOW -#if defined(CONFIG_SENSORS_VEHICLE_ACCELERATION) +#if defined(SENSORS_VEHICLE_ACCELERATION) PX4_INFO_RAW("\n"); _vehicle_acceleration.PrintStatus(); -#endif // CONFIG_SENSORS_VEHICLE_ACCELERATION +#endif // SENSORS_VEHICLE_ACCELERATION -#if defined(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY) +#if defined(SENSORS_VEHICLE_ANGULAR_VELOCITY) PX4_INFO_RAW("\n"); _vehicle_angular_velocity.PrintStatus(); -#endif // CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY +#endif // SENSORS_VEHICLE_ANGULAR_VELOCITY -#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) +#if defined(SENSORS_VEHICLE_GPS_POSITION) if (_vehicle_gps_position) { PX4_INFO_RAW("\n"); _vehicle_gps_position->PrintStatus(); } -#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION +#endif // SENSORS_VEHICLE_GPS_POSITION PX4_INFO_RAW("\n"); @@ -744,3 +746,13 @@ It runs in its own thread and polls on the currently selected gyro topic. extern "C" __EXPORT int sensors_main(int argc, char *argv[]) { return Sensors::main(argc, argv); } + +MSH_CMD_EXPORT_ALIAS(sensors_main, sensors, sensors); + +int sensors_start() { + const char *argv[] = {"sensors", "start"}; + int argc = sizeof(argv) / sizeof(argv[0]); + return sensors_main(argc, (char **)argv); +} + +INIT_APP_EXPORT(sensors_start); diff --git a/apps/estimator/sensors/sensors.hpp b/apps/estimator/sensors/sensors.hpp index 8e751605f5..ee9b947e4f 100644 --- a/apps/estimator/sensors/sensors.hpp +++ b/apps/estimator/sensors/sensors.hpp @@ -27,41 +27,41 @@ #include "voted_sensors_update.h" #include "vehicle_imu/VehicleIMU.hpp" -#if defined(CONFIG_SENSORS_VEHICLE_ACCELERATION) +#if defined(SENSORS_VEHICLE_ACCELERATION) # include "vehicle_acceleration/VehicleAcceleration.hpp" -#endif // CONFIG_SENSORS_VEHICLE_ACCELERATION +#endif // SENSORS_VEHICLE_ACCELERATION -#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) +#if defined(SENSORS_VEHICLE_AIRSPEED) # include # include # include # include # include # include -#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED +#endif // SENSORS_VEHICLE_AIRSPEED -#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) +#if defined(SENSORS_VEHICLE_AIR_DATA) # include # include "vehicle_air_data/VehicleAirData.hpp" -#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA +#endif // SENSORS_VEHICLE_AIR_DATA -#if defined(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY) +#if defined(SENSORS_VEHICLE_ANGULAR_VELOCITY) # include "vehicle_angular_velocity/VehicleAngularVelocity.hpp" -#endif // CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY +#endif // SENSORS_VEHICLE_ANGULAR_VELOCITY -#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) +#if defined(SENSORS_VEHICLE_GPS_POSITION) # include "vehicle_gps_position/VehicleGPSPosition.hpp" -#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION +#endif // SENSORS_VEHICLE_GPS_POSITION -#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) +#if defined(SENSORS_VEHICLE_MAGNETOMETER) # include "vehicle_magnetometer/VehicleMagnetometer.hpp" # include # include -#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER +#endif // SENSORS_VEHICLE_MAGNETOMETER -#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) +#if defined(SENSORS_VEHICLE_OPTICAL_FLOW) # include "vehicle_optical_flow/VehicleOpticalFlow.hpp" -#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW +#endif // SENSORS_VEHICLE_OPTICAL_FLOW using namespace sensors; using namespace time_literals; @@ -135,7 +135,7 @@ class Sensors : public ModuleCommand, public ModuleParams, public WorkI uORB::Publication _sensor_pub{ORB_ID(sensor_combined)}; -#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) +#if defined(SENSORS_VEHICLE_AIRSPEED) /** * Poll the differential pressure sensor for updated data. * @@ -194,45 +194,45 @@ class Sensors : public ModuleCommand, public ModuleParams, public WorkI param_t air_tube_length; param_t air_tube_diameter_mm; } _parameter_handles{}; /**< handles for interesting parameters */ -#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED +#endif // SENSORS_VEHICLE_AIRSPEED -#if defined(CONFIG_SENSORS_VEHICLE_ACCELERATION) +#if defined(SENSORS_VEHICLE_ACCELERATION) VehicleAcceleration _vehicle_acceleration; -#endif // CONFIG_SENSORS_VEHICLE_ACCELERATION +#endif // SENSORS_VEHICLE_ACCELERATION -#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) +#if defined(SENSORS_VEHICLE_AIR_DATA) VehicleAirData *_vehicle_air_data{nullptr}; uint8_t _n_baro{0}; -#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA +#endif // SENSORS_VEHICLE_AIR_DATA -#if defined(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY) +#if defined(SENSORS_VEHICLE_ANGULAR_VELOCITY) VehicleAngularVelocity _vehicle_angular_velocity; -#endif // CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY +#endif // SENSORS_VEHICLE_ANGULAR_VELOCITY -#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) +#if defined(SENSORS_VEHICLE_MAGNETOMETER) VehicleMagnetometer *_vehicle_magnetometer{nullptr}; uint8_t _n_mag{0}; -#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER +#endif // SENSORS_VEHICLE_MAGNETOMETER -#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) +#if defined(SENSORS_VEHICLE_GPS_POSITION) VehicleGPSPosition *_vehicle_gps_position{nullptr}; uint8_t _n_gps{0}; -#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION +#endif // SENSORS_VEHICLE_GPS_POSITION -#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) +#if defined(SENSORS_VEHICLE_OPTICAL_FLOW) VehicleOpticalFlow *_vehicle_optical_flow{nullptr}; uint8_t _n_optical_flow{0}; -#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW +#endif // SENSORS_VEHICLE_OPTICAL_FLOW DEFINE_PARAMETERS( -#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) +#if defined(SENSORS_VEHICLE_AIR_DATA) (ParamBool)_param_sys_has_baro, -#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA -#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) +#endif // SENSORS_VEHICLE_AIR_DATA +#if defined(SENSORS_VEHICLE_GPS_POSITION) (ParamBool)_param_sys_has_gps, -#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION -#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) +#endif // SENSORS_VEHICLE_GPS_POSITION +#if defined(SENSORS_VEHICLE_MAGNETOMETER) (ParamInt)_param_sys_has_mag, -#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER +#endif // SENSORS_VEHICLE_MAGNETOMETER (ParamBool)_param_sens_imu_mode) }; diff --git a/apps/estimator/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/apps/estimator/sensors/vehicle_acceleration/VehicleAcceleration.cpp index 4a789011ef..72335ca777 100644 --- a/apps/estimator/sensors/vehicle_acceleration/VehicleAcceleration.cpp +++ b/apps/estimator/sensors/vehicle_acceleration/VehicleAcceleration.cpp @@ -8,8 +8,9 @@ * Copyright All Reserved © 2015-2024 NextPilot Development Team ******************************************************************/ -#include "VehicleAcceleration.hpp" +#define LOG_TAG "sensors.accel" +#include "VehicleAcceleration.hpp" #include #include @@ -19,7 +20,7 @@ namespace sensors { VehicleAcceleration::VehicleAcceleration() : ModuleParams(nullptr), - WorkItemScheduled(MODULE_NAME, wq_configurations::nav_and_controllers) { + WorkItemScheduled(LOG_TAG, wq_configurations::nav_and_controllers) { _vehicle_acceleration_pub.advertise(); CheckAndUpdateFilters(); diff --git a/apps/estimator/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/apps/estimator/sensors/vehicle_acceleration/VehicleAcceleration.hpp index 3a518c925e..a12ff83da1 100644 --- a/apps/estimator/sensors/vehicle_acceleration/VehicleAcceleration.hpp +++ b/apps/estimator/sensors/vehicle_acceleration/VehicleAcceleration.hpp @@ -27,6 +27,8 @@ #include using namespace time_literals; +using namespace nextpilot; +using namespace nextpilot::global_params; namespace sensors { diff --git a/apps/estimator/sensors/vehicle_air_data/VehicleAirData.cpp b/apps/estimator/sensors/vehicle_air_data/VehicleAirData.cpp index f8ce657ad7..260e44f3c8 100644 --- a/apps/estimator/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/apps/estimator/sensors/vehicle_air_data/VehicleAirData.cpp @@ -8,8 +8,10 @@ * Copyright All Reserved © 2015-2024 NextPilot Development Team ******************************************************************/ -#include "VehicleAirData.hpp" +#define LOG_TAG "sensors.baro" +#define MODULE_NAME LOG_TAG +#include "VehicleAirData.hpp" #include #include #include diff --git a/apps/estimator/sensors/vehicle_air_data/VehicleAirData.hpp b/apps/estimator/sensors/vehicle_air_data/VehicleAirData.hpp index 1afe151f72..70ae225107 100644 --- a/apps/estimator/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/apps/estimator/sensors/vehicle_air_data/VehicleAirData.hpp @@ -11,7 +11,6 @@ #pragma once #include "data_validator/DataValidatorGroup.hpp" - #include #include #include @@ -29,6 +28,8 @@ #include using namespace time_literals; +using namespace nextpilot; +using namespace nextpilot::global_params; namespace sensors { class VehicleAirData : public ModuleParams, public WorkItemScheduled { diff --git a/apps/estimator/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/apps/estimator/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index ed765f90cc..17b23b4705 100644 --- a/apps/estimator/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/apps/estimator/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -8,6 +8,9 @@ * Copyright All Reserved © 2015-2024 NextPilot Development Team ******************************************************************/ +#define LOG_TAG "sensors.gyro" +#define MODULE_NAME LOG_TAG + #include "VehicleAngularVelocity.hpp" #include @@ -615,7 +618,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(const hrt_abstime &time_now_u sensor_gyro_fft_s sensor_gyro_fft; if (_sensor_gyro_fft_sub.copy(&sensor_gyro_fft) && (sensor_gyro_fft.device_id == _selected_sensor_device_id) && (time_now_us < sensor_gyro_fft.timestamp + DYNAMIC_NOTCH_FITLER_TIMEOUT) && ((fabsf(sensor_gyro_fft.sensor_sample_rate_hz - _filter_sample_rate_hz) / _filter_sample_rate_hz) < 0.02f)) { - static constexpr float peak_freq_min = 10.f; // lower bound TODO: configurable? + static constexpr float peak_freq_min = 10.f; // lower bound TODO: configurable? const float bandwidth = math::constrain(sensor_gyro_fft.resolution_hz, 8.f, 30.f); // TODO: base on numerical limits? @@ -798,8 +801,7 @@ void VehicleAngularVelocity::Run() { _timestamp_sample_last = sensor_data.timestamp_sample - 1e6f / _filter_sample_rate_hz; } - const float inverse_dt_s = 1.f / math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_last) * 1e-6f), - 0.00002f, 0.02f); + const float inverse_dt_s = 1.f / math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_last) * 1e-6f), 0.00002f, 0.02f); _timestamp_sample_last = sensor_data.timestamp_sample; Vector3f angular_velocity_uncalibrated; diff --git a/apps/estimator/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/apps/estimator/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 7b583f3dea..5df645be37 100644 --- a/apps/estimator/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/apps/estimator/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -33,6 +34,8 @@ #include using namespace time_literals; +using namespace nextpilot; +using namespace nextpilot::global_params; namespace sensors { diff --git a/apps/estimator/sensors/vehicle_gps_position/VehicleGPSPosition.cpp b/apps/estimator/sensors/vehicle_gps_position/VehicleGPSPosition.cpp index 01279dc386..1a76ac8366 100644 --- a/apps/estimator/sensors/vehicle_gps_position/VehicleGPSPosition.cpp +++ b/apps/estimator/sensors/vehicle_gps_position/VehicleGPSPosition.cpp @@ -8,8 +8,10 @@ * Copyright All Reserved © 2015-2024 NextPilot Development Team ******************************************************************/ -#include "VehicleGPSPosition.hpp" +#define LOG_TAG "sensors.gps" +#define MODULE_NAME LOG_TAG +#include "VehicleGPSPosition.hpp" #include #include #include diff --git a/apps/estimator/sensors/vehicle_gps_position/VehicleGPSPosition.hpp b/apps/estimator/sensors/vehicle_gps_position/VehicleGPSPosition.hpp index 1ea4e151a8..4b68649bc3 100644 --- a/apps/estimator/sensors/vehicle_gps_position/VehicleGPSPosition.hpp +++ b/apps/estimator/sensors/vehicle_gps_position/VehicleGPSPosition.hpp @@ -24,6 +24,8 @@ #include "gps_blending.hpp" using namespace time_literals; +using namespace nextpilot; +using namespace nextpilot::global_params; namespace sensors { class VehicleGPSPosition : public ModuleParams, public WorkItemScheduled { diff --git a/apps/estimator/sensors/vehicle_imu/SConscript b/apps/estimator/sensors/vehicle_imu/SConscript index c916b0ba62..f8be687c68 100644 --- a/apps/estimator/sensors/vehicle_imu/SConscript +++ b/apps/estimator/sensors/vehicle_imu/SConscript @@ -7,6 +7,6 @@ cwd = GetCurrentDir() inc = [] src = Glob("*.cpp", exclude="*_test.cpp") -objs = DefineGroup("ins/sensors", src, depend=["INS_USING_SENSOR"]) +objs = DefineGroup("ins/sensors", src, depend=["INS_USING_SENSORS"]) Return("objs") diff --git a/apps/estimator/sensors/vehicle_imu/VehicleIMU.cpp b/apps/estimator/sensors/vehicle_imu/VehicleIMU.cpp index 7566435e39..699d163416 100644 --- a/apps/estimator/sensors/vehicle_imu/VehicleIMU.cpp +++ b/apps/estimator/sensors/vehicle_imu/VehicleIMU.cpp @@ -8,8 +8,10 @@ * Copyright All Reserved © 2015-2024 NextPilot Development Team ******************************************************************/ -#include "VehicleIMU.hpp" +#define LOG_TAG "sensors.imu" +#define MODULE_NAME LOG_TAG +#include "VehicleIMU.hpp" #include #include #include diff --git a/apps/estimator/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/apps/estimator/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 645030ed06..dd7a0e8594 100644 --- a/apps/estimator/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/apps/estimator/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -8,8 +8,10 @@ * Copyright All Reserved © 2015-2024 NextPilot Development Team ******************************************************************/ -#include "VehicleMagnetometer.hpp" +#define LOG_TAG "sensors.mag" +#define MODULE_NAME LOG_TAG +#include "VehicleMagnetometer.hpp" #include #include #include diff --git a/apps/estimator/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp b/apps/estimator/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp index 366db747df..12ac07aadd 100644 --- a/apps/estimator/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp +++ b/apps/estimator/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp @@ -37,6 +37,8 @@ #include using namespace time_literals; +using namespace nextpilot; +using namespace nextpilot::global_params; namespace sensors { class VehicleMagnetometer : public ModuleParams, public WorkItemScheduled { diff --git a/apps/estimator/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp b/apps/estimator/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp index 452568eb20..2e9ea9af08 100644 --- a/apps/estimator/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp +++ b/apps/estimator/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp @@ -8,9 +8,12 @@ * Copyright All Reserved © 2015-2024 NextPilot Development Team ******************************************************************/ -#include "VehicleOpticalFlow.hpp" +#define LOG_TAG "sensors.opticalflow" +#define MODULE_NAME LOG_TAG +#include "VehicleOpticalFlow.hpp" #include + namespace sensors { using namespace matrix; @@ -93,8 +96,8 @@ void VehicleOpticalFlow::Run() { // - from sensor_optical_flow if available, otherwise use synchronized sensor_gyro if available if (sensor_optical_flow.delta_angle_available && Vector3f(sensor_optical_flow.delta_angle).isAllFinite()) { // passthrough integrated gyro if available - _delta_angle += _flow_rotation * Vector3f{sensor_optical_flow.delta_angle}; - _delta_angle_available = true; + _delta_angle += _flow_rotation * Vector3f{sensor_optical_flow.delta_angle}; + _delta_angle_available = true; } else { _delta_angle_available = false; @@ -133,7 +136,7 @@ void VehicleOpticalFlow::Run() { _distance_sum_count = 1; } else { - _distance_sum += sensor_optical_flow.distance_m; + _distance_sum += sensor_optical_flow.distance_m; _distance_sum_count += 1; } @@ -147,15 +150,15 @@ void VehicleOpticalFlow::Run() { _distance_sum_count = 1; } else { - _distance_sum += range_sample.data; + _distance_sum += range_sample.data; _distance_sum_count += 1; } } } - _flow_timestamp_sample_last = sensor_optical_flow.timestamp_sample; - _flow_integral(0) += sensor_optical_flow.pixel_flow[0]; - _flow_integral(1) += sensor_optical_flow.pixel_flow[1]; + _flow_timestamp_sample_last = sensor_optical_flow.timestamp_sample; + _flow_integral(0) += sensor_optical_flow.pixel_flow[0]; + _flow_integral(1) += sensor_optical_flow.pixel_flow[1]; _integration_timespan_us += sensor_optical_flow.integration_timespan_us; diff --git a/apps/estimator/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp b/apps/estimator/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp index a0de238677..3e3c34c5b5 100644 --- a/apps/estimator/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp +++ b/apps/estimator/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp @@ -33,6 +33,9 @@ #include #include +using namespace nextpilot; +using namespace nextpilot::global_params; + namespace sensors { class VehicleOpticalFlow : public ModuleParams, public WorkItemScheduled { diff --git a/apps/libraries/peripheral/drivers/drv_adc.h b/apps/libraries/peripheral/drivers/drv_adc.h index 345c8ed0b9..2ec55c54ba 100644 --- a/apps/libraries/peripheral/drivers/drv_adc.h +++ b/apps/libraries/peripheral/drivers/drv_adc.h @@ -19,7 +19,6 @@ #include #include -#include #include /* Define the PX4 low level format ADC and the maximum diff --git a/apps/libraries/peripheral/drivers/drv_neopixel.h b/apps/libraries/peripheral/drivers/drv_neopixel.h index 596aadb4f3..a7f54b980c 100644 --- a/apps/libraries/peripheral/drivers/drv_neopixel.h +++ b/apps/libraries/peripheral/drivers/drv_neopixel.h @@ -19,7 +19,6 @@ #include #include -#include namespace neopixel { diff --git a/apps/telemetry/mavlink/mavlink_receiver.cpp b/apps/telemetry/mavlink/mavlink_receiver.cpp index 4ddb9c86af..41a808bc2f 100644 --- a/apps/telemetry/mavlink/mavlink_receiver.cpp +++ b/apps/telemetry/mavlink/mavlink_receiver.cpp @@ -19,8 +19,6 @@ #include #include -#include - #include #include diff --git a/bsps/sitl/qemu/config/default.config b/bsps/sitl/qemu/config/default.config index fe949c2599..34422661af 100644 --- a/bsps/sitl/qemu/config/default.config +++ b/bsps/sitl/qemu/config/default.config @@ -69,6 +69,13 @@ CONFIG_INS_USING_GYRO_CALIBRATION=y CONFIG_INS_USING_LOCAL_POSITION_ESTIMATOR=y CONFIG_INS_USING_MAG_BIAS_ESTIMATOR=y CONFIG_INS_USING_SENSORS=y +CONFIG_SENSORS_VEHICLE_AIRSPEED=y +CONFIG_SENSORS_VEHICLE_AIR_DATA=y +CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY=y +CONFIG_SENSORS_VEHICLE_ACCELERATION=y +CONFIG_SENSORS_VEHICLE_GPS_POSITION=y +CONFIG_SENSORS_VEHICLE_MAGNETOMETER=y +CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW=y CONFIG_INS_USING_TEMPERATURE_COMPENSATION=y # end of Nextpilot Estimator Config diff --git a/bsps/sitl/qemu/rtconfig.h b/bsps/sitl/qemu/rtconfig.h index 4a991d274f..8f39c2473b 100644 --- a/bsps/sitl/qemu/rtconfig.h +++ b/bsps/sitl/qemu/rtconfig.h @@ -51,6 +51,13 @@ #define INS_USING_LOCAL_POSITION_ESTIMATOR #define INS_USING_MAG_BIAS_ESTIMATOR #define INS_USING_SENSORS +#define SENSORS_VEHICLE_AIRSPEED +#define SENSORS_VEHICLE_AIR_DATA +#define SENSORS_VEHICLE_ANGULAR_VELOCITY +#define SENSORS_VEHICLE_ACCELERATION +#define SENSORS_VEHICLE_GPS_POSITION +#define SENSORS_VEHICLE_MAGNETOMETER +#define SENSORS_VEHICLE_OPTICAL_FLOW #define INS_USING_TEMPERATURE_COMPENSATION /* end of Nextpilot Estimator Config */