From 503f97c8bc6243ab9255b289ada2fd6c802bb55a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 24 Apr 2022 12:45:23 -0400 Subject: [PATCH] logger: add default ground truth logging for HITL/SITL --- src/modules/logger/logged_topics.cpp | 39 ++++++++++++++++++---------- 1 file changed, 25 insertions(+), 14 deletions(-) diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 1fe3cfd6f7..dc635653e0 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -184,6 +184,30 @@ void LoggedTopics::add_default_topics() add_topic_multi("vehicle_imu_status", 1000, 4); add_optional_topic_multi("vehicle_magnetometer", 500, 4); + // SYS_CTRL_ALLOC: additional dynamic control allocation logging when enabled + int32_t sys_ctrl_alloc = 0; + param_get(param_find("SYS_CTRL_ALLOC"), &sys_ctrl_alloc); + + if (sys_ctrl_alloc >= 1) { + add_topic("actuator_motors", 100); + add_topic("actuator_servos", 100); + add_topic("vehicle_angular_acceleration", 20); + add_topic("vehicle_angular_acceleration_setpoint", 20); + add_topic_multi("vehicle_thrust_setpoint", 20, 2); + 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) { + 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"); @@ -194,7 +218,7 @@ void LoggedTopics::add_default_topics() add_topic("vehicle_angular_velocity", 10); add_topic("vehicle_attitude_groundtruth", 10); add_topic("vehicle_global_position_groundtruth", 100); - add_topic("vehicle_local_position_groundtruth", 100); + add_topic("vehicle_local_position_groundtruth", 20); // EKF replay add_topic("estimator_baro_bias"); @@ -215,19 +239,6 @@ void LoggedTopics::add_default_topics() add_topic("wind"); add_topic("yaw_estimator_status"); #endif /* CONFIG_ARCH_BOARD_PX4_SITL */ - - - int32_t sys_ctrl_alloc = 0; - param_get(param_find("SYS_CTRL_ALLOC"), &sys_ctrl_alloc); - - if (sys_ctrl_alloc >= 1) { - add_topic("actuator_motors", 100); - add_topic("actuator_servos", 100); - add_topic("vehicle_angular_acceleration", 20); - add_topic("vehicle_angular_acceleration_setpoint", 20); - add_topic_multi("vehicle_thrust_setpoint", 20, 2); - add_topic_multi("vehicle_torque_setpoint", 20, 2); - } } void LoggedTopics::add_high_rate_topics()