From a891179a968c5f668ef9889d0dd9bad5af1d8668 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 11 Sep 2022 12:23:42 -0400 Subject: [PATCH] cleanup logger default profile - eliminate px4_sitl special handling - try to make as many topics optional as possible - ekf2 force initial advertisement based on configuration --- src/drivers/adc/board_adc/ADC.cpp | 3 + .../AngularVelocityController.cpp | 4 + src/modules/commander/Commander.cpp | 8 + src/modules/commander/HomePosition.cpp | 1 + src/modules/ekf2/EKF2.cpp | 131 +++++-- src/modules/ekf2/EKF2.hpp | 2 + .../flight_mode_manager/FlightModeManager.cpp | 4 + .../FixedwingAttitudeControl.cpp | 9 +- .../FixedwingPositionControl.cpp | 1 + src/modules/land_detector/LandDetector.cpp | 2 + src/modules/load_mon/LoadMon.cpp | 1 + src/modules/logger/logged_topics.cpp | 336 ++++++++---------- src/modules/manual_control/ManualControl.cpp | 1 + .../MulticopterHoverThrustEstimator.cpp | 1 + .../MulticopterPositionControl.cpp | 4 + .../MulticopterRateControl.cpp | 6 + src/modules/navigator/mission.cpp | 1 + src/modules/navigator/navigator_main.cpp | 5 + src/modules/navigator/rtl.cpp | 1 + src/modules/rc_update/rc_update.cpp | 6 + 20 files changed, 308 insertions(+), 219 deletions(-) diff --git a/src/drivers/adc/board_adc/ADC.cpp b/src/drivers/adc/board_adc/ADC.cpp index d69d085c01..ef9bd444bf 100644 --- a/src/drivers/adc/board_adc/ADC.cpp +++ b/src/drivers/adc/board_adc/ADC.cpp @@ -45,6 +45,9 @@ ADC::ADC(uint32_t base_address, uint32_t channels, bool publish_adc_report) : _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")), _base_address(base_address) { + _to_adc_report.advertise(); + _to_system_power.advertise(); + /* always enable the temperature sensor */ channels |= px4_arch_adc_temp_sensor_mask(); diff --git a/src/modules/angular_velocity_controller/AngularVelocityController.cpp b/src/modules/angular_velocity_controller/AngularVelocityController.cpp index 979e22b388..eda127112d 100644 --- a/src/modules/angular_velocity_controller/AngularVelocityController.cpp +++ b/src/modules/angular_velocity_controller/AngularVelocityController.cpp @@ -50,7 +50,11 @@ AngularVelocityController::AngularVelocityController() : _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; parameters_updated(); + _rate_ctrl_status_pub.advertise(); + _vehicle_angular_acceleration_setpoint_pub.advertise(); + _vehicle_thrust_setpoint_pub.advertise(); + _vehicle_torque_setpoint_pub.advertise(); } AngularVelocityController::~AngularVelocityController() diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index f6a4d1ed68..4c8005bd9b 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -732,6 +732,14 @@ Commander::Commander() : _failure_detector(this), _health_and_arming_checks(this, _vehicle_status_flags, _vehicle_status) { + _actuator_armed_pub.advertise(); + _commander_state_pub.advertise(); + _failure_detector_status_pub.advertise(); + _vehicle_control_mode_pub.advertise(); + _vehicle_status_flags_pub.advertise(); + _vehicle_status_pub.advertise(); + _vehicle_command_ack_pub.advertise(); + _vehicle_land_detected.landed = true; _vehicle_status.system_id = 1; diff --git a/src/modules/commander/HomePosition.cpp b/src/modules/commander/HomePosition.cpp index ee23ad3994..e5bd99612e 100644 --- a/src/modules/commander/HomePosition.cpp +++ b/src/modules/commander/HomePosition.cpp @@ -40,6 +40,7 @@ HomePosition::HomePosition(const vehicle_status_flags_s &vehicle_status_flags) : _vehicle_status_flags(vehicle_status_flags) { + _home_position_pub.advertise(); } bool HomePosition::hasMovedFromCurrentHomeLocation() diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index f286c2f990..4b056da39d 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -168,18 +168,7 @@ 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(); + advertiseTopics(); } EKF2::~EKF2() @@ -197,41 +186,121 @@ EKF2::~EKF2() perf_free(_msg_missed_optical_flow_perf); } -bool EKF2::multi_init(int imu, int mag) +void EKF2::advertiseTopics() { - // advertise all topics to ensure consistent uORB instance numbering - _ekf2_timestamps_pub.advertise(); - _estimator_baro_bias_pub.advertise(); - _estimator_ev_hgt_bias_pub.advertise(); + // advertise expected minimal topic set immediately to ensure logging + _attitude_pub.advertise(); + _local_position_pub.advertise(); + _estimator_event_flags_pub.advertise(); - _estimator_gnss_hgt_bias_pub.advertise(); - _estimator_gps_status_pub.advertise(); _estimator_innovation_test_ratios_pub.advertise(); _estimator_innovation_variances_pub.advertise(); _estimator_innovations_pub.advertise(); - _estimator_optical_flow_vel_pub.advertise(); - _estimator_rng_hgt_bias_pub.advertise(); _estimator_sensor_bias_pub.advertise(); _estimator_states_pub.advertise(); - _estimator_status_flags_pub.advertise(); _estimator_status_pub.advertise(); - _estimator_visual_odometry_aligned_pub.advertise(); - _yaw_est_pub.advertise(); + _estimator_status_flags_pub.advertise(); - _attitude_pub.advertise(); - _local_position_pub.advertise(); - _global_position_pub.advertise(); - _odometry_pub.advertise(); - _wind_pub.advertise(); + if (_multi_mode) { + _global_position_pub.advertise(); + _odometry_pub.advertise(); + _wind_pub.advertise(); + _ekf2_timestamps_pub.advertise(); + } + + // baro + if (_params->baro_ctrl != 0) { + _estimator_baro_bias_pub.advertise(); + _estimator_aid_src_baro_hgt_pub.advertise(); + } + + // GNSS + if (_params->gnss_ctrl != 0) { + + _estimator_gps_status_pub.advertise(); + + if (_params->gnss_ctrl & GnssCtrl::VEL) { + _yaw_est_pub.advertise(); + _estimator_aid_src_gnss_vel_pub.advertise(); + } + + if (_params->gnss_ctrl & GnssCtrl::VPOS) { + _estimator_gnss_hgt_bias_pub.advertise(); + } + + if (_params->gnss_ctrl & GnssCtrl::HPOS) { + _global_position_pub.advertise(); + _estimator_aid_src_gnss_pos_pub.advertise(); + } + + if (_params->gnss_ctrl & GnssCtrl::YAW) { + _estimator_aid_src_gnss_yaw_pub.advertise(); + } + } + + // range finder + if (_params->rng_ctrl != 0) { + _estimator_rng_hgt_bias_pub.advertise(); + _estimator_aid_src_rng_hgt_pub.advertise(); + } + + // external vision (TODO: add and use EKF2_EV_CTRL parameter) + if ((_params->fusion_mode & SensorFusionMask::USE_EXT_VIS_POS) + || (_params->fusion_mode & SensorFusionMask::USE_EXT_VIS_YAW) + || (_params->fusion_mode & SensorFusionMask::USE_EXT_VIS_VEL) + || (_params->height_sensor_ref == HeightSensor::EV)) { + + _estimator_ev_hgt_bias_pub.advertise(); + _estimator_visual_odometry_aligned_pub.advertise(); + + if (_params->fusion_mode & SensorFusionMask::USE_EXT_VIS_POS) { + //_estimator_aid_src_ev_pos_pub.advertise(); + } + + if (_params->fusion_mode & SensorFusionMask::USE_EXT_VIS_YAW) { + _estimator_aid_src_ev_yaw_pub.advertise(); + } + + if (_params->fusion_mode & SensorFusionMask::USE_EXT_VIS_VEL) { + //_estimator_aid_src_ev_vel_pub.advertise(); + } + } + + // optical flow + if (_params->fusion_mode & SensorFusionMask::USE_OPT_FLOW) { + _estimator_optical_flow_vel_pub.advertise(); + } + + // airspeed + if (_params->arsp_thr > 0.f) { + _estimator_aid_src_airspeed_pub.advertise(); + } + + // magnetometer + if (_params->mag_fusion_type != MagFuseType::NONE) { + _estimator_aid_src_mag_heading_pub.advertise(); + _estimator_aid_src_mag_pub.advertise(); + } +} + +bool EKF2::multi_init(int imu, int mag) +{ + advertiseTopics(); bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu) && _magnetometer_sub.ChangeInstance(mag); const int status_instance = _estimator_states_pub.get_instance(); - if ((status_instance >= 0) && changed_instance + if (changed_instance && (status_instance >= 0) + && (_estimator_event_flags_pub.get_instance() == status_instance) + && (_estimator_status_flags_pub.get_instance() == status_instance) + && (_estimator_sensor_bias_pub.get_instance() == status_instance) && (_attitude_pub.get_instance() == status_instance) && (_local_position_pub.get_instance() == status_instance) - && (_global_position_pub.get_instance() == status_instance)) { + && (_global_position_pub.get_instance() == status_instance) + && (_odometry_pub.get_instance() == status_instance) + && (_wind_pub.get_instance() == status_instance) + ) { _instance = status_instance; diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 79f37f1a68..aa530f0cfa 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -133,6 +133,8 @@ private: static constexpr uint8_t MAX_NUM_IMUS = 4; static constexpr uint8_t MAX_NUM_MAGS = 4; + void advertiseTopics(); + void Run() override; void VerifyParams(); diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 101da19251..57d9e619f2 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -42,6 +42,10 @@ FlightModeManager::FlightModeManager() : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) { + _landing_gear_pub.advertise(); + _trajectory_setpoint_pub.advertise(); + _vehicle_constraints_pub.advertise(); + updateParams(); // initialize all flight-tasks diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index c395562c75..59b310a6ed 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -48,6 +48,14 @@ FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) : _attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { + _actuator_controls_0_pub.advertise(); + _actuator_controls_status_pub.advertise(); + _attitude_sp_pub.advertise(); + _rate_sp_pub.advertise(); + _rate_ctrl_status_pub.advertise(); + _vehicle_thrust_setpoint_pub.advertise(); + _vehicle_torque_setpoint_pub.advertise(); + /* fetch initial parameter values */ parameters_update(); @@ -57,7 +65,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) : _pitch_ctrl.set_max_rate_neg(radians(_param_fw_acro_y_max.get())); _yaw_ctrl.set_max_rate(radians(_param_fw_acro_z_max.get())); - _rate_ctrl_status_pub.advertise(); _spoiler_setpoint_with_slewrate.setSlewRate(kSpoilerSlewRate); _flaps_setpoint_with_slewrate.setSlewRate(kFlapSlewRate); } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 161fdca5e0..7ee98b0f5f 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -65,6 +65,7 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) : _pos_ctrl_status_pub.advertise(); _pos_ctrl_landing_status_pub.advertise(); + _npfg_status_pub.advertise(); _tecs_status_pub.advertise(); _airspeed_slew_rate_controller.setSlewRate(ASPD_SP_SLEW_RATE); diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index f16e8ed383..eb282e32d4 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -49,6 +49,8 @@ LandDetector::LandDetector() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) { + _vehicle_land_detected_pub.advertise(); + _land_detected.ground_contact = true; _land_detected.maybe_landed = true; _land_detected.landed = true; diff --git a/src/modules/load_mon/LoadMon.cpp b/src/modules/load_mon/LoadMon.cpp index 8bc818a80c..8093a38d2f 100644 --- a/src/modules/load_mon/LoadMon.cpp +++ b/src/modules/load_mon/LoadMon.cpp @@ -53,6 +53,7 @@ LoadMon::LoadMon() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) { + _cpuload_pub.advertise(); } LoadMon::~LoadMon() diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index a835ddfeba..193f9ebc02 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include @@ -45,236 +46,194 @@ using namespace px4::logger; void LoggedTopics::add_default_topics() { - add_topic("action_request"); - add_topic("actuator_armed"); - add_topic("actuator_controls_0", 50); - add_topic("actuator_controls_1", 100); - add_topic("actuator_controls_2", 100); + // default topics that may not be advertised immediately + add_topic("battery_status", 200); + add_topic("cellular_status", 200); + add_topic("differential_pressure", 1000); + add_topic("distance_sensor", 1000); + add_topic("gimbal_manager_set_attitude", 500); + add_topic("input_rc", 500); + add_topic("offboard_control_mode", 100); + add_topic("onboard_computer_status", 100); + add_topic("radio_status"); + add_topic("sensor_accel", 1000); + add_topic("sensor_baro", 1000); + add_topic("sensor_gnss_relative", 1000); + add_topic("sensor_gps", 1000); + add_topic("sensor_gyro", 1000); + add_topic("sensor_hygrometer", 1000); + add_topic("sensor_mag", 1000); + add_topic("sensor_optical_flow", 1000); + add_topic("telemetry_status", 1000); + add_topic("transponder_report", 1000); + add_topic("vehicle_imu_status", 1000); + add_topic("vehicle_optical_flow", 500); + add_topic("wind", 1000); + + // optional topics (must be advertised) + add_optional_topic("action_request"); + add_optional_topic("actuator_armed"); + add_optional_topic("actuator_controls_0", 50); + add_optional_topic("actuator_controls_1", 200); + add_optional_topic("actuator_controls_2", 200); add_optional_topic("actuator_controls_status_0", 300); - add_topic("airspeed", 1000); + add_optional_topic("actuator_controls_virtual_fw", 300); + add_optional_topic("actuator_controls_virtual_mc", 300); + add_optional_topic("actuator_motors", 100); + add_optional_topic("actuator_servos", 100); + add_optional_topic("airspeed", 1000); add_optional_topic("airspeed_validated", 200); - add_optional_topic("autotune_attitude_control_status", 100); + add_optional_topic("autotune_attitude_control_status", 200); add_optional_topic("camera_capture"); add_optional_topic("camera_trigger"); - add_topic("cellular_status", 200); - add_topic("commander_state"); - add_topic("cpuload"); + add_optional_topic("commander_state"); + add_optional_topic("cpuload"); add_optional_topic("esc_status", 250); - add_topic("failure_detector_status", 100); + add_optional_topic("estimator_selector_status"); + add_optional_topic("failure_detector_status", 100); add_optional_topic("follow_target", 500); add_optional_topic("follow_target_estimator", 200); add_optional_topic("follow_target_status", 400); - add_topic("gimbal_manager_set_attitude", 500); + add_optional_topic("fw_virtual_attitude_setpoint", 500); add_optional_topic("generator_status"); add_optional_topic("gps_dump"); add_optional_topic("gripper"); - add_optional_topic("heater_status"); - add_topic("home_position"); - add_topic("hover_thrust_estimate", 100); - add_topic("input_rc", 500); - add_optional_topic("internal_combustion_engine_status", 10); + add_optional_topic("heater_status", 200); + add_optional_topic("home_position"); + add_optional_topic("hover_thrust_estimate", 200); + add_optional_topic("internal_combustion_engine_status", 200); add_optional_topic("irlock_report", 1000); + add_optional_topic("landing_gear", 1000); add_optional_topic("landing_target_pose", 1000); add_optional_topic("magnetometer_bias_estimate", 200); - add_topic("manual_control_setpoint", 200); - add_topic("manual_control_switches"); - add_topic("mission_result"); - add_topic("navigator_mission_item"); - add_topic("npfg_status", 100); - add_topic("offboard_control_mode", 100); - add_topic("onboard_computer_status", 10); - add_topic("parameter_update"); - add_topic("position_controller_status", 500); - add_topic("position_controller_landing_status", 100); - add_topic("position_setpoint_triplet", 200); + add_optional_topic("manual_control_setpoint", 200); + add_optional_topic("manual_control_switches"); + add_optional_topic("mc_virtual_attitude_setpoint", 500); + add_optional_topic("mission_result"); + add_optional_topic("navigator_mission_item"); + add_optional_topic("npfg_status", 200); + add_optional_topic("parameter_update"); + add_optional_topic("position_controller_landing_status", 200); + add_optional_topic("position_controller_status", 500); + add_optional_topic("position_setpoint_triplet", 200); + add_optional_topic("pps_capture"); add_optional_topic("px4io_status"); - add_topic("radio_status"); - add_topic("rtl_time_estimate", 1000); - add_topic("sensor_combined"); + add_optional_topic("rate_ctrl_status", 200); + add_optional_topic("rtl_time_estimate", 1000); + add_optional_topic("sensor_combined"); add_optional_topic("sensor_correction"); add_optional_topic("sensor_gyro_fft", 50); - add_topic("sensor_selection"); - add_topic("sensors_status_imu", 200); - add_topic("system_power", 500); + add_optional_topic("sensor_selection"); + add_optional_topic("sensors_status_imu", 200); + add_optional_topic("system_power", 500); add_optional_topic("takeoff_status", 1000); add_optional_topic("tecs_status", 200); - add_topic("trajectory_setpoint", 200); - add_topic("transponder_report"); - add_topic("vehicle_acceleration", 50); - add_topic("vehicle_air_data", 200); - add_topic("vehicle_angular_velocity", 20); - add_topic("vehicle_attitude", 50); - add_topic("vehicle_attitude_setpoint", 50); - add_topic("vehicle_command"); - add_topic("vehicle_command_ack"); - add_topic("vehicle_constraints", 1000); - add_topic("vehicle_control_mode"); - add_topic("vehicle_global_position", 200); - add_topic("vehicle_gps_position", 500); - add_topic("vehicle_land_detected"); - add_topic("vehicle_local_position", 100); - add_topic("vehicle_local_position_setpoint", 100); - add_topic("vehicle_magnetometer", 200); - add_topic("vehicle_rates_setpoint", 20); - add_topic("vehicle_roi", 1000); - add_topic("vehicle_status"); - add_topic("vehicle_status_flags"); + add_optional_topic("trajectory_setpoint", 200); + add_optional_topic("vehicle_acceleration", 50); + add_optional_topic("vehicle_air_data", 200); + add_optional_topic("vehicle_angular_acceleration", 20); + add_optional_topic("vehicle_angular_velocity", 20); + add_optional_topic("vehicle_attitude", 50); + add_optional_topic("vehicle_attitude_setpoint", 50); + add_optional_topic("vehicle_command"); + add_optional_topic("vehicle_command_ack"); + add_optional_topic("vehicle_constraints", 1000); + add_optional_topic("vehicle_control_mode"); + add_optional_topic("vehicle_global_position", 200); + add_optional_topic("vehicle_gps_position", 500); + add_optional_topic("vehicle_land_detected"); + add_optional_topic("vehicle_local_position", 100); + add_optional_topic("vehicle_local_position_setpoint", 100); + add_optional_topic("vehicle_optical_flow_vel", 100); + add_optional_topic("vehicle_rates_setpoint", 20); + add_optional_topic("vehicle_roi", 1000); + add_optional_topic("vehicle_status"); + add_optional_topic("vehicle_status_flags"); add_optional_topic("vtol_vehicle_status", 200); - add_topic("wind", 1000); + add_optional_topic("yaw_estimator_status", 1000); - // multi topics - add_optional_topic_multi("actuator_outputs", 100, 3); - 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_optional_topic_multi("sensor_hygrometer", 500, 4); - add_optional_topic_multi("rpm", 200); - add_optional_topic_multi("telemetry_status", 1000, 4); + // EKF artificially limit multi-EKF logging to keep total topics a uint8_t + static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 4; - // EKF multi topics (currently max 9 estimators) -#if CONSTRAINED_MEMORY - static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 1; -#else - static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 6; // artificially limited until PlotJuggler fixed - add_optional_topic("estimator_selector_status"); + add_optional_topic_multi("actuator_outputs", 100); + add_optional_topic_multi("airspeed_wind", 500); + add_optional_topic_multi("battery_status", 300); + add_optional_topic_multi("control_allocator_status", 200); + add_optional_topic_multi("differential_pressure", 500); + add_optional_topic_multi("distance_sensor", 500); + add_optional_topic_multi("estimator_aid_src_airspeed", 300, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_aid_src_aux_vel", 300, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_aid_src_baro_hgt", 300, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_aid_src_ev_yaw", 300, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_aid_src_fake_hgt", 300, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_aid_src_fake_pos", 300, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_aid_src_gnss_pos", 300, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_aid_src_gnss_vel", 300, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_aid_src_gnss_yaw", 300, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_aid_src_mag", 300, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_aid_src_mag_heading", 300, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_aid_src_rng_hgt", 300, MAX_ESTIMATOR_INSTANCES); 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); - add_optional_topic_multi("estimator_wind", 1000, MAX_ESTIMATOR_INSTANCES); -#endif - - // always add the first instance - add_topic("estimator_baro_bias", 500); - add_topic("estimator_gnss_hgt_bias", 500); - add_topic("estimator_rng_hgt_bias", 500); - add_topic("estimator_ev_hgt_bias", 500); - add_topic("estimator_event_flags", 0); - add_topic("estimator_gps_status", 1000); - add_topic("estimator_innovation_test_ratios", 500); - add_topic("estimator_innovation_variances", 500); - add_topic("estimator_innovations", 500); - add_topic("estimator_optical_flow_vel", 200); - add_topic("estimator_sensor_bias", 0); - add_topic("estimator_states", 1000); - add_topic("estimator_status", 200); - add_topic("estimator_status_flags", 0); - add_topic("estimator_visual_odometry_aligned", 200); - add_topic("yaw_estimator_status", 1000); - add_optional_topic_multi("estimator_baro_bias", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_gnss_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_rng_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_ev_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_gps_status", 1000, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_global_position", 500, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_gnss_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_gps_status", 500, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_innovation_test_ratios", 500, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_innovation_variances", 500, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_innovations", 500, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_local_position", 200, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_odometry", 500, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_optical_flow_vel", 200, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_rng_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_sensor_bias", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_states", 1000, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_states", 500, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_status", 200, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_status_flags", 0, MAX_ESTIMATOR_INSTANCES); add_optional_topic_multi("estimator_visual_odometry_aligned", 200, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("yaw_estimator_status", 1000, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("estimator_wind", 500, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("manual_control_input", 500); + add_optional_topic_multi("rate_ctrl_status", 200); + add_optional_topic_multi("rpm", 200); + add_optional_topic_multi("sensor_accel", 500); + add_optional_topic_multi("sensor_baro", 500); + add_optional_topic_multi("sensor_gnss_relative", 500); + add_optional_topic_multi("sensor_gps", 500); + add_optional_topic_multi("sensor_gyro", 500); + add_optional_topic_multi("sensor_hygrometer", 500); + add_optional_topic_multi("sensor_mag", 500); + add_optional_topic_multi("sensor_optical_flow", 500); + add_optional_topic_multi("telemetry_status", 500); + add_optional_topic_multi("vehicle_imu", 500); + add_optional_topic_multi("vehicle_imu_status", 500); + add_optional_topic_multi("vehicle_magnetometer", 200); + add_optional_topic_multi("vehicle_optical_flow", 500); + add_optional_topic_multi("vehicle_thrust_setpoint", 100); + add_optional_topic_multi("vehicle_torque_setpoint", 100); + add_optional_topic_multi("yaw_estimator_status", 500, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_airspeed", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_baro_hgt", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_rng_hgt", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_fake_hgt", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_fake_pos", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_gnss_yaw", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_gnss_vel", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_gnss_pos", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_mag_heading", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_mag", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES); - // log all raw sensors at minimal rate (at least 1 Hz) - add_topic_multi("battery_status", 200, 2); - add_topic_multi("differential_pressure", 1000, 2); - add_topic_multi("distance_sensor", 1000, 2); - add_optional_topic_multi("sensor_accel", 1000, 4); - add_optional_topic_multi("sensor_baro", 1000, 4); - add_topic_multi("sensor_gps", 1000, 2); - add_topic_multi("sensor_gnss_relative", 1000, 1); - add_optional_topic_multi("sensor_gyro", 1000, 4); - add_optional_topic_multi("sensor_mag", 1000, 4); - add_optional_topic_multi("sensor_optical_flow", 1000, 2); + // simulation ground truth logging (SYS_HITL or PX4_SITL) +#if defined(CONFIG_ARCH_BOARD_PX4_SITL) + bool ground_truth_topics = true; +#else + bool ground_truth_topics = false; +#endif // CONFIG_ARCH_BOARD_PX4_SITL - add_topic_multi("vehicle_imu", 500, 4); - add_topic_multi("vehicle_imu_status", 1000, 4); - add_optional_topic_multi("vehicle_magnetometer", 500, 4); - add_optional_topic("vehicle_optical_flow", 500); - //add_optional_topic("vehicle_optical_flow_vel", 100); - add_optional_topic("pps_capture"); - - // additional control allocation logging - add_topic("actuator_motors", 100); - add_topic("actuator_servos", 100); - add_topic("vehicle_angular_acceleration", 20); - add_topic_multi("vehicle_thrust_setpoint", 20, 1); - add_topic_multi("vehicle_torque_setpoint", 20, 2); - - // SYS_HITL: default ground truth logging for simulation int32_t sys_hitl = 0; param_get(param_find("SYS_HITL"), &sys_hitl); if (sys_hitl >= 1) { + ground_truth_topics = true; + } + + if (ground_truth_topics) { add_topic("vehicle_angular_velocity_groundtruth", 10); add_topic("vehicle_attitude_groundtruth", 10); add_topic("vehicle_global_position_groundtruth", 100); add_topic("vehicle_local_position_groundtruth", 20); } - -#ifdef CONFIG_ARCH_BOARD_PX4_SITL - add_topic("actuator_controls_virtual_fw"); - add_topic("actuator_controls_virtual_mc"); - add_topic("fw_virtual_attitude_setpoint"); - add_topic("mc_virtual_attitude_setpoint"); - add_topic("time_offset"); - add_topic("vehicle_angular_acceleration", 10); - add_topic("vehicle_angular_velocity", 10); - add_topic("vehicle_angular_velocity_groundtruth", 10); - add_topic("vehicle_attitude_groundtruth", 10); - add_topic("vehicle_global_position_groundtruth", 100); - add_topic("vehicle_local_position_groundtruth", 20); - - // EKF replay - add_topic("estimator_baro_bias"); - add_topic("estimator_gnss_hgt_bias"); - add_topic("estimator_rng_hgt_bias"); - add_topic("estimator_ev_hgt_bias"); - add_topic("estimator_event_flags"); - add_topic("estimator_gps_status"); - add_topic("estimator_innovation_test_ratios"); - add_topic("estimator_innovation_variances"); - add_topic("estimator_innovations"); - add_topic("estimator_optical_flow_vel"); - add_topic("estimator_sensor_bias"); - add_topic("estimator_states"); - add_topic("estimator_status"); - add_topic("estimator_status_flags"); - add_topic("estimator_visual_odometry_aligned"); - add_topic("vehicle_attitude"); - add_topic("vehicle_global_position"); - add_topic("vehicle_local_position"); - add_topic("wind"); - add_topic("yaw_estimator_status"); - - add_optional_topic_multi("estimator_aid_src_airspeed", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_baro_hgt", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_rng_hgt", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_fake_hgt", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_fake_pos", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_gnss_yaw", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_gnss_vel", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_gnss_pos", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_mag_heading", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_mag", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_ev_yaw", 0, MAX_ESTIMATOR_INSTANCES); - -#endif /* CONFIG_ARCH_BOARD_PX4_SITL */ } void LoggedTopics::add_high_rate_topics() @@ -313,16 +272,19 @@ void LoggedTopics::add_estimator_replay_topics() // current EKF2 subscriptions add_topic("airspeed"); - add_topic("optical_flow"); + add_topic("airspeed_validated"); + add_topic("landing_target_pose"); add_topic("sensor_combined"); add_topic("sensor_selection"); add_topic("vehicle_air_data"); + add_topic("vehicle_command"); add_topic("vehicle_gps_position"); add_topic("vehicle_land_detected"); add_topic("vehicle_magnetometer"); + add_topic("vehicle_optical_flow"); add_topic("vehicle_status"); add_topic("vehicle_visual_odometry"); - add_topic_multi("distance_sensor"); + add_topic_multi("distance_sensor", 0, 3); } void LoggedTopics::add_thermal_calibration_topics() @@ -492,7 +454,7 @@ bool LoggedTopics::add_topic(const char *name, uint16_t interval_ms, uint8_t ins 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; + _subscriptions.sub[j].interval_ms = math::min(interval_ms, _subscriptions.sub[j].interval_ms); success = true; already_added = true; break; diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index 056f6cbe5e..8ed88d1a48 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -42,6 +42,7 @@ ManualControl::ManualControl() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) { + _action_request_pub.advertise(); } ManualControl::~ManualControl() diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp index e767eb7d32..21dd7f0275 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp @@ -47,6 +47,7 @@ MulticopterHoverThrustEstimator::MulticopterHoverThrustEstimator() : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) { + _hover_thrust_ekf_pub.advertise(); _valid_hysteresis.set_hysteresis_time_from(false, 2_s); updateParams(); reset(); diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index fa097c7cdf..024d259602 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -53,7 +53,11 @@ MulticopterPositionControl::MulticopterPositionControl(bool vtol) : parameters_update(true); _tilt_limit_slew_rate.setSlewRate(.2f); reset_setpoint_to_nan(_setpoint); + _takeoff_status_pub.advertise(); + _vehicle_attitude_setpoint_pub.advertise(); + _local_pos_sp_pub.advertise(); + } MulticopterPositionControl::~MulticopterPositionControl() diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index 02a54c9155..2f45ac21d7 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -52,7 +52,13 @@ MulticopterRateControl::MulticopterRateControl(bool vtol) : _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; parameters_updated(); + + _actuator_controls_0_pub.advertise(); + _actuator_controls_status_0_pub.advertise(); _controller_status_pub.advertise(); + _vehicle_rates_setpoint_pub.advertise(); + _vehicle_thrust_setpoint_pub.advertise(); + _vehicle_torque_setpoint_pub.advertise(); } MulticopterRateControl::~MulticopterRateControl() diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 618cd2f58b..c066cc3316 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -67,6 +67,7 @@ Mission::Mission(Navigator *navigator) : MissionBlock(navigator), ModuleParams(navigator) { + _navigator_mission_item_pub.advertise(); mission_init(); } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c03a82198d..24ab4c26d5 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -79,6 +79,11 @@ Navigator::Navigator() : _precland(this), _rtl(this) { + _geofence_result_pub.advertise(); + _pos_sp_triplet_pub.advertise(); + _mission_result_pub.advertise(); + _vehicle_roi_pub.advertise(); + /* Create a list of our possible navigation types */ _navigation_mode_array[0] = &_mission; _navigation_mode_array[1] = &_loiter; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index d2e9875ef5..d9dee0e105 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -57,6 +57,7 @@ RTL::RTL(Navigator *navigator) : MissionBlock(navigator), ModuleParams(navigator) { + _rtl_time_estimate_pub.advertise(); _param_mpc_z_v_auto_up = param_find("MPC_Z_V_AUTO_UP"); _param_mpc_z_v_auto_dn = param_find("MPC_Z_V_AUTO_DN"); _param_mpc_land_speed = param_find("MPC_LAND_SPEED"); diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp index daccde964b..c53c2d64cf 100644 --- a/src/modules/rc_update/rc_update.cpp +++ b/src/modules/rc_update/rc_update.cpp @@ -67,6 +67,12 @@ RCUpdate::RCUpdate() : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::hp_default) { + if (_param_rc_chan_cnt.get() > 0) { + _rc_channels_pub.advertise(); + _manual_control_input_pub.advertise(); + _manual_control_switches_pub.advertise(); + } + // initialize parameter handles for (unsigned i = 0; i < RC_MAX_CHAN_COUNT; i++) { char nbuf[16];