diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 462aba0dcc..206d2654d4 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -223,6 +223,13 @@ rc_update start manual_control start sensors start commander start + +# Configure vehicle type specific parameters. +# Note: rc.vehicle_setup is the entry point for rc.interface, +# rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps. +# +. ${R}etc/init.d/rc.vehicle_setup + navigator start # Try to start the micrortps_client with UDP transport if module exists @@ -252,12 +259,6 @@ then gyro_calibration start fi -# Configure vehicle type specific parameters. -# Note: rc.vehicle_setup is the entry point for rc.interface, -# rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps. -# -. ${R}etc/init.d/rc.vehicle_setup - #user defined mavlink streams for instances can be in PATH . px4-rc.mavlink diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f560f1fea1..8fc5b5202c 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -408,6 +408,13 @@ else commander start fi + # + # Configure vehicle type specific parameters. + # Note: rc.vehicle_setup is the entry point for rc.interface, + # rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps. + # + . ${R}etc/init.d/rc.vehicle_setup + # Pre-takeoff continuous magnetometer calibration if param compare -s MBE_ENABLE 1 then @@ -458,13 +465,6 @@ else fi fi - # - # Configure vehicle type specific parameters. - # Note: rc.vehicle_setup is the entry point for rc.interface, - # rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps. - # - . ${R}etc/init.d/rc.vehicle_setup - # # Play the startup tune (if not disabled or there is an error) # diff --git a/src/drivers/heater/heater.cpp b/src/drivers/heater/heater.cpp index 2a984eccaf..170bd859a4 100644 --- a/src/drivers/heater/heater.cpp +++ b/src/drivers/heater/heater.cpp @@ -73,6 +73,8 @@ Heater::Heater() : } #endif + + _heater_status_pub.advertise(); } Heater::~Heater() diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index dc7c0ebffd..553c8899a9 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -103,6 +103,9 @@ _param_prefix(param_prefix) } updateParams(); + + } else { + _control_allocator_status_pub.advertise(); } _outputs_pub.advertise(); diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 55d3bf90a5..a240083955 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -206,6 +206,8 @@ AirspeedModule::AirspeedModule(): _perf_elapsed = perf_alloc(PC_ELAPSED, MODULE_NAME": elapsed"); _airspeed_validated_pub.advertise(); + _wind_est_pub[0].advertise(); + _wind_est_pub[1].advertise(); } AirspeedModule::~AirspeedModule() diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 2d797a24ee..81fbc8f795 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -54,6 +54,11 @@ ControlAllocator::ControlAllocator() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { + _control_allocator_status_pub.advertise(); + _actuator_motors_pub.advertise(); + _actuator_servos_pub.advertise(); + _actuator_servos_trim_pub.advertise(); + for (int i = 0; i < MAX_NUM_MOTORS; ++i) { char buffer[17]; snprintf(buffer, sizeof(buffer), "CA_R%u_SLEW", i); diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index a9fc373ac5..e47d1f2f60 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -42,6 +42,8 @@ px4_add_module( #-DDEBUG_BUILD INCLUDES EKF + PRIORITY + "SCHED_PRIORITY_MAX - 18" # max priority below high priority WQ threads STACK_MAX 3600 SRCS diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 88fdfb3af8..fa59ca8c46 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2021 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 @@ -164,6 +164,18 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_synthetic_mag_z(_params->synthesize_mag_z), _param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default) { + // advertise expected minimal topic set immediately to ensure logging + _attitude_pub.advertise(); + _local_position_pub.advertise(); + + _estimator_event_flags_pub.advertise(); + _estimator_innovation_test_ratios_pub.advertise(); + _estimator_innovation_variances_pub.advertise(); + _estimator_innovations_pub.advertise(); + _estimator_sensor_bias_pub.advertise(); + _estimator_states_pub.advertise(); + _estimator_status_flags_pub.advertise(); + _estimator_status_pub.advertise(); } EKF2::~EKF2() @@ -183,13 +195,7 @@ EKF2::~EKF2() bool EKF2::multi_init(int imu, int mag) { - // advertise immediately to ensure consistent uORB instance numbering - _attitude_pub.advertise(); - _local_position_pub.advertise(); - _global_position_pub.advertise(); - _odometry_pub.advertise(); - _wind_pub.advertise(); - + // advertise all topics to ensure consistent uORB instance numbering _ekf2_timestamps_pub.advertise(); _estimator_baro_bias_pub.advertise(); _estimator_event_flags_pub.advertise(); @@ -205,6 +211,12 @@ bool EKF2::multi_init(int imu, int mag) _estimator_visual_odometry_aligned_pub.advertise(); _yaw_est_pub.advertise(); + _attitude_pub.advertise(); + _local_position_pub.advertise(); + _global_position_pub.advertise(); + _odometry_pub.advertise(); + _wind_pub.advertise(); + bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu) && _magnetometer_sub.ChangeInstance(mag); const int status_instance = _estimator_states_pub.get_instance(); @@ -320,8 +332,7 @@ void EKF2::Run() } if (!_callback_registered) { - PX4_WARN("%d - failed to register callback, retrying", _instance); - ScheduleDelayed(1_s); + ScheduleDelayed(10_ms); return; } } @@ -593,6 +604,9 @@ void EKF2::Run() // publish ekf2_timestamps _ekf2_timestamps_pub.publish(ekf2_timestamps); } + + // re-schedule as backup timeout + ScheduleDelayed(100_ms); } void EKF2::PublishAttitude(const hrt_abstime ×tamp) @@ -692,11 +706,19 @@ void EKF2::PublishEventFlags(const hrt_abstime ×tamp) event_flags.emergency_yaw_reset_gps_yaw_stopped = _ekf.warning_event_flags().emergency_yaw_reset_gps_yaw_stopped; event_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); - _estimator_event_flags_pub.publish(event_flags); - } + _estimator_event_flags_pub.update(event_flags); + + _last_event_flags_publish = event_flags.timestamp; + + _ekf.clear_information_events(); + _ekf.clear_warning_events(); - _ekf.clear_information_events(); - _ekf.clear_warning_events(); + } else if ((_last_event_flags_publish != 0) && (timestamp >= _last_event_flags_publish + 1_s)) { + // continue publishing periodically + _estimator_event_flags_pub.get().timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + _estimator_event_flags_pub.update(); + _last_event_flags_publish = _estimator_event_flags_pub.get().timestamp; + } } void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) @@ -1198,7 +1220,7 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) { // publish at ~ 1 Hz (or immediately if filter control status or fault status changes) - bool update = (hrt_elapsed_time(&_last_status_flag_update) >= 1_s); + bool update = (timestamp >= _last_status_flags_publish + 1_s); // filter control status if (_ekf.control_status().value != _filter_control_status) { @@ -1296,7 +1318,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_status_flags_pub.publish(status_flags); - _last_status_flag_update = status_flags.timestamp; + _last_status_flags_publish = status_flags.timestamp; } } @@ -2027,9 +2049,7 @@ int EKF2::task_spawn(int argc, char *argv[]) vehicle_mag_sub.update(); // Mag & IMU data must be valid, first mag can be ignored initially - if ((vehicle_mag_sub.get().device_id != 0 || mag == 0) - && (vehicle_imu_sub.get().accel_device_id != 0) - && (vehicle_imu_sub.get().gyro_device_id != 0)) { + if ((vehicle_mag_sub.advertised() || mag == 0) && (vehicle_imu_sub.advertised())) { if (!ekf2_instance_created[imu][mag]) { EKF2 *ekf2_inst = new EKF2(true, px4::ins_instance_to_wq(imu), false); @@ -2043,17 +2063,11 @@ int EKF2::task_spawn(int argc, char *argv[]) multi_instances_allocated++; ekf2_instance_created[imu][mag] = true; - if (actual_instance == 0) { - // force selector to run immediately if first instance started - _ekf2_selector.load()->ScheduleNow(); - } - PX4_DEBUG("starting instance %d, IMU:%" PRIu8 " (%" PRIu32 "), MAG:%" PRIu8 " (%" PRIu32 ")", actual_instance, imu, vehicle_imu_sub.get().accel_device_id, mag, vehicle_mag_sub.get().device_id); - // sleep briefly before starting more instances - px4_usleep(10000); + _ekf2_selector.load()->ScheduleNow(); } else { PX4_ERR("instance numbering problem instance: %d", actual_instance); @@ -2063,20 +2077,20 @@ int EKF2::task_spawn(int argc, char *argv[]) } else { PX4_ERR("alloc and init failed imu: %" PRIu8 " mag:%" PRIu8, imu, mag); - px4_usleep(1000000); + px4_usleep(100000); break; } } } else { - px4_usleep(50000); // give the sensors extra time to start - continue; + px4_usleep(1000); // give the sensors extra time to start + break; } } } if (multi_instances_allocated < multi_instances) { - px4_usleep(100000); + px4_usleep(10000); } } diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index e5e5de74af..b1c6c69c6c 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -267,7 +267,9 @@ private: bool _callback_registered{false}; - hrt_abstime _last_status_flag_update{0}; + hrt_abstime _last_event_flags_publish{0}; + hrt_abstime _last_status_flags_publish{0}; + hrt_abstime _last_range_sensor_update{0}; uint32_t _filter_control_status{0}; @@ -282,7 +284,7 @@ private: uORB::PublicationMulti _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; uORB::PublicationMulti _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)}; - uORB::PublicationMulti _estimator_event_flags_pub{ORB_ID(estimator_event_flags)}; + uORB::PublicationMultiData _estimator_event_flags_pub{ORB_ID(estimator_event_flags)}; uORB::PublicationMulti _estimator_gps_status_pub{ORB_ID(estimator_gps_status)}; uORB::PublicationMulti _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; uORB::PublicationMulti _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)}; diff --git a/src/modules/ekf2/EKF2Selector.cpp b/src/modules/ekf2/EKF2Selector.cpp index 4150519c94..99fb863eed 100644 --- a/src/modules/ekf2/EKF2Selector.cpp +++ b/src/modules/ekf2/EKF2Selector.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -43,6 +43,13 @@ EKF2Selector::EKF2Selector() : ModuleParams(nullptr), ScheduledWorkItem("ekf2_selector", px4::wq_configurations::nav_and_controllers) { + _estimator_selector_status_pub.advertise(); + _sensor_selection_pub.advertise(); + _vehicle_attitude_pub.advertise(); + _vehicle_global_position_pub.advertise(); + _vehicle_local_position_pub.advertise(); + _vehicle_odometry_pub.advertise(); + _wind_pub.advertise(); } EKF2Selector::~EKF2Selector() @@ -50,12 +57,6 @@ EKF2Selector::~EKF2Selector() Stop(); } -bool EKF2Selector::Start() -{ - ScheduleNow(); - return true; -} - void EKF2Selector::Stop() { for (int i = 0; i < EKF2_MAX_INSTANCES; i++) { @@ -673,9 +674,6 @@ void EKF2Selector::PublishWindEstimate() void EKF2Selector::Run() { - // re-schedule as backup timeout - ScheduleDelayed(FILTER_UPDATE_PERIOD); - // check for parameter updates if (_parameter_update_sub.updated()) { // clear update @@ -703,6 +701,7 @@ void EKF2Selector::Run() // if still invalid return early and check again on next scheduled run if (_selected_instance == INVALID_INSTANCE) { + ScheduleDelayed(100_ms); return; } } @@ -803,6 +802,9 @@ void EKF2Selector::Run() PublishVehicleGlobalPosition(); PublishVehicleOdometry(); PublishWindEstimate(); + + // re-schedule as backup timeout + ScheduleDelayed(FILTER_UPDATE_PERIOD); } void EKF2Selector::PublishEstimatorSelectorStatus() diff --git a/src/modules/ekf2/EKF2Selector.hpp b/src/modules/ekf2/EKF2Selector.hpp index a45925561a..6507233bd3 100644 --- a/src/modules/ekf2/EKF2Selector.hpp +++ b/src/modules/ekf2/EKF2Selector.hpp @@ -70,7 +70,6 @@ public: EKF2Selector(); ~EKF2Selector() override; - bool Start(); void Stop(); void PrintStatus(); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 070444f5a8..a896ebda05 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -70,6 +70,8 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) : // limit to 50 Hz _local_pos_sub.set_interval_ms(20); + _pos_ctrl_status_pub.advertise(); + _pos_ctrl_landing_status_pub.advertise(); _tecs_status_pub.advertise(); _slew_rate_airspeed.setSlewRate(ASPD_SP_SLEW_RATE); diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index cee9afbb79..23f0126224 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019, 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -120,10 +120,10 @@ void LoggedTopics::add_default_topics() // multi topics add_optional_topic_multi("actuator_outputs", 100, 3); - add_topic_multi("airspeed_wind", 1000, 4); - add_topic_multi("control_allocator_status", 200, 2); + add_optional_topic_multi("airspeed_wind", 1000, 4); + add_optional_topic_multi("control_allocator_status", 200, 2); add_optional_topic_multi("rate_ctrl_status", 200, 2); - add_topic_multi("sensor_hygrometer", 500, 4); + add_optional_topic_multi("sensor_hygrometer", 500, 4); add_optional_topic_multi("rpm", 200); add_optional_topic_multi("telemetry_status", 1000, 4); @@ -132,7 +132,7 @@ void LoggedTopics::add_default_topics() static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 1; #else static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 6; // artificially limited until PlotJuggler fixed - add_topic("estimator_selector_status"); + add_optional_topic("estimator_selector_status"); add_optional_topic_multi("estimator_attitude", 500, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_global_position", 1000, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_local_position", 500, MAX_ESTIMATOR_INSTANCES); @@ -440,7 +440,7 @@ bool LoggedTopics::add_topic(const char *name, uint16_t interval_ms, uint8_t ins if (_subscriptions.sub[j].id == static_cast(topics[i]->o_id) && _subscriptions.sub[j].instance == instance) { - PX4_DEBUG("logging topic %s(%" PRUu8 "), interval: %" PRUu16 ", already added, only setting interval", + PX4_DEBUG("logging topic %s(%" PRIu8 "), interval: %" PRIu16 ", already added, only setting interval", topics[i]->o_name, instance, interval_ms); _subscriptions.sub[j].interval_ms = interval_ms; @@ -452,7 +452,11 @@ bool LoggedTopics::add_topic(const char *name, uint16_t interval_ms, uint8_t ins if (!already_added) { success = add_topic(topics[i], interval_ms, instance, optional); - PX4_DEBUG("logging topic: %s(%" PRUu8 "), interval: %" PRUu16, topics[i]->o_name, instance, interval_ms); + + if (success) { + PX4_DEBUG("logging topic: %s(%" PRIu8 "), interval: %" PRIu16, topics[i]->o_name, instance, interval_ms); + } + break; } } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 7c49f588b7..4a9f02fa51 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -233,6 +233,11 @@ Sensors::Sensors(bool hil_enabled) : _loop_perf(perf_alloc(PC_ELAPSED, "sensors")), _voted_sensors_update(hil_enabled, _vehicle_imu_sub) { + _sensor_pub.advertise(); + + _vehicle_angular_velocity.Start(); + _vehicle_acceleration.Start(); + /* Differential pressure offset */ _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL @@ -250,11 +255,17 @@ Sensors::Sensors(bool hil_enabled) : param_find("SYS_CAL_TMAX"); param_find("SYS_CAL_TMIN"); + _sensor_combined.accelerometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID; + _airspeed_validator.set_timeout(300000); _airspeed_validator.set_equal_value_threshold(100); - _vehicle_acceleration.Start(); - _vehicle_angular_velocity.Start(); + parameters_update(); + + InitializeVehicleAirData(); + InitializeVehicleGPSPosition(); + InitializeVehicleIMU(); + InitializeVehicleMagnetometer(); } Sensors::~Sensors() @@ -369,6 +380,10 @@ int Sensors::parameters_update() } } + InitializeVehicleAirData(); + InitializeVehicleGPSPosition(); + InitializeVehicleMagnetometer(); + return PX4_OK; } @@ -569,14 +584,9 @@ void Sensors::InitializeVehicleIMU() if (_vehicle_imu_list[i] == nullptr) { uORB::Subscription accel_sub{ORB_ID(sensor_accel), i}; - sensor_accel_s accel{}; - accel_sub.copy(&accel); - uORB::Subscription gyro_sub{ORB_ID(sensor_gyro), i}; - sensor_gyro_s gyro{}; - gyro_sub.copy(&gyro); - if (accel.device_id > 0 && gyro.device_id > 0) { + if (accel_sub.advertised() && gyro_sub.advertised()) { // if the sensors module is responsible for voting (SENS_IMU_MODE 1) then run every VehicleIMU in the same WQ // otherwise each VehicleIMU runs in a corresponding INSx WQ const bool multi_mode = (_param_sens_imu_mode.get() == 0); @@ -627,21 +637,8 @@ void Sensors::Run() return; } - // run once - if (_last_config_update == 0) { - InitializeVehicleAirData(); - InitializeVehicleIMU(); - InitializeVehicleGPSPosition(); - InitializeVehicleMagnetometer(); - _voted_sensors_update.init(_sensor_combined); - parameter_update_poll(true); - } - perf_begin(_loop_perf); - // backup schedule as a watchdog timeout - ScheduleDelayed(10_ms); - // check vehicle status for changes to publication state if (_vcontrol_mode_sub.updated()) { vehicle_control_mode_s vcontrol_mode{}; @@ -651,23 +648,9 @@ void Sensors::Run() } } - _voted_sensors_update.sensorsPoll(_sensor_combined); - - // check analog airspeed - adc_poll(); - - diff_pres_poll(); - - if (_sensor_combined.timestamp != _sensor_combined_prev_timestamp) { - - _voted_sensors_update.setRelativeTimestamps(_sensor_combined); - _sensor_pub.publish(_sensor_combined); - _sensor_combined_prev_timestamp = _sensor_combined.timestamp; - } - // keep adding sensors as long as we are not armed, // when not adding sensors poll for param updates - if (!_armed && hrt_elapsed_time(&_last_config_update) > 1000_ms) { + 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)); @@ -683,10 +666,6 @@ void Sensors::Run() _n_mag = n_mag; parameters_update(); - - InitializeVehicleAirData(); - InitializeVehicleGPSPosition(); - InitializeVehicleMagnetometer(); } // sensor device id (not just orb_group_count) must be populated before IMU init can succeed @@ -700,6 +679,23 @@ void Sensors::Run() parameter_update_poll(); } + _voted_sensors_update.sensorsPoll(_sensor_combined); + + if (_sensor_combined.timestamp != _sensor_combined_prev_timestamp) { + + _voted_sensors_update.setRelativeTimestamps(_sensor_combined); + _sensor_pub.publish(_sensor_combined); + _sensor_combined_prev_timestamp = _sensor_combined.timestamp; + } + + // check analog airspeed + adc_poll(); + + diff_pres_poll(); + + // backup schedule as a watchdog timeout + ScheduleDelayed(10_ms); + perf_end(_loop_perf); } diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp index d1baa48e3d..b908d5a5b0 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019, 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -46,6 +46,8 @@ VehicleAcceleration::VehicleAcceleration() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) { + _vehicle_acceleration_pub.advertise(); + CheckAndUpdateFilters(); } diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index 8e4ab8f413..2d52112535 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -48,6 +48,8 @@ VehicleAirData::VehicleAirData() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) { + _vehicle_air_data_pub.advertise(); + _voter.set_timeout(SENSOR_TIMEOUT); } @@ -334,7 +336,7 @@ void VehicleAirData::Run() } // reschedule timeout - ScheduleDelayed(100_ms); + ScheduleDelayed(50_ms); perf_end(_cycle_perf); } diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 4f52197f6f..e0e562f020 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -46,6 +46,8 @@ VehicleAngularVelocity::VehicleAngularVelocity() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { + _vehicle_angular_acceleration_pub.advertise(); + _vehicle_angular_velocity_pub.advertise(); } VehicleAngularVelocity::~VehicleAngularVelocity() diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp index f30aef9b5a..b722b65624 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -43,6 +43,7 @@ VehicleGPSPosition::VehicleGPSPosition() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) { + _vehicle_gps_position_pub.advertise(); } VehicleGPSPosition::~VehicleGPSPosition() @@ -103,9 +104,6 @@ void VehicleGPSPosition::Run() perf_begin(_cycle_perf); ParametersUpdate(); - // GPS blending - ScheduleDelayed(500_ms); // backup schedule - // Check all GPS instance bool any_gps_updated = false; bool gps_updated = false; @@ -135,6 +133,8 @@ void VehicleGPSPosition::Run() } } + ScheduleDelayed(300_ms); // backup schedule + perf_end(_cycle_perf); } diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index c2ad29781b..99e3e75a49 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -650,8 +650,8 @@ void VehicleIMU::UpdateIntegratorConfiguration() _gyro_integrator.set_reset_interval(roundf((gyro_integral_samples - 0.5f) * _gyro_interval_us)); _gyro_integrator.set_reset_samples(gyro_integral_samples); - _backup_schedule_timeout_us = math::min(sensor_accel_s::ORB_QUEUE_LENGTH * _accel_interval_us, - sensor_gyro_s::ORB_QUEUE_LENGTH * _gyro_interval_us); + _backup_schedule_timeout_us = math::constrain((int)math::min(sensor_accel_s::ORB_QUEUE_LENGTH * _accel_interval_us, + sensor_gyro_s::ORB_QUEUE_LENGTH * _gyro_interval_us) / 2, 1000, 20000); // gyro: find largest integer multiple of gyro_integral_samples for (int n = sensor_gyro_s::ORB_QUEUE_LENGTH; n > 0; n--) { diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 545e912500..ffde91e056 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -36,6 +36,7 @@ #include #include #include +#include namespace sensors { @@ -55,6 +56,20 @@ VehicleMagnetometer::VehicleMagnetometer() : _voter.set_equal_value_threshold(1000); ParametersUpdate(true); + + _vehicle_magnetometer_pub[0].advertise(); + _sensor_preflight_mag_pub.advertise(); + + // if publishing multiple mags advertise instances immediately for existing calibrations + if (!_param_sens_mag_mode.get()) { + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + uint32_t device_id_mag = calibration::GetCalibrationParamInt32("MAG", "ID", i); + + if (device_id_mag != 0) { + _vehicle_magnetometer_pub[i].advertise(); + } + } + } } VehicleMagnetometer::~VehicleMagnetometer() @@ -547,7 +562,7 @@ void VehicleMagnetometer::Run() UpdateMagCalibration(); // reschedule timeout - ScheduleDelayed(40_ms); + ScheduleDelayed(50_ms); perf_end(_cycle_perf); } diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 5deb0106d1..7e4f4f07a4 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -54,22 +54,17 @@ VotedSensorsUpdate::VotedSensorsUpdate(bool hil_enabled, _vehicle_imu_sub(vehicle_imu_sub), _hil_enabled(hil_enabled) { + _sensor_selection_pub.advertise(); + _sensors_status_imu_pub.advertise(); + if (_hil_enabled) { // HIL has less accurate timing so increase the timeouts a bit _gyro.voter.set_timeout(500000); _accel.voter.set_timeout(500000); } -} - -int VotedSensorsUpdate::init(sensor_combined_s &raw) -{ - raw.accelerometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID; - raw.timestamp = 0; initializeSensors(); - _selection_changed = true; - - return 0; + parametersUpdate(); } void VotedSensorsUpdate::initializeSensors() diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index fab257b182..9e8d40a6e9 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -78,12 +78,6 @@ public: */ VotedSensorsUpdate(bool hil_enabled, uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[MAX_SENSOR_COUNT]); - /** - * initialize subscriptions etc. - * @return 0 on success, <0 otherwise - */ - int init(sensor_combined_s &raw); - /** * This tries to find new sensor instances. This is called from init(), then it can be called periodically. */