From 71dcf8d619fc560b26091c3d43a2bbf97b20c356 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 18 Jun 2020 11:43:22 +0200 Subject: [PATCH] sitl: use lockstep components API - avoids the need for ekf2_timestamp publications by q and lpe - adds logger to the lockstep cycle and makes it poll on ekf2_timestamps or vehicle_attitude. This avoids dropped samples (required for replay). --- ROMFS/px4fmu_common/init.d-posix/rcS | 9 ++- ROMFS/px4fmu_common/init.d/rc.logging | 6 +- .../attitude_estimator_q_main.cpp | 36 +++------ src/modules/ekf2/ekf2_main.cpp | 8 ++ .../BlockLocalPositionEstimator.cpp | 30 +++---- .../BlockLocalPositionEstimator.hpp | 6 +- src/modules/logger/logger.cpp | 77 +++++++++--------- src/modules/logger/logger.h | 6 +- src/modules/simulator/simulator.h | 8 -- src/modules/simulator/simulator_mavlink.cpp | 81 ++++--------------- 10 files changed, 102 insertions(+), 165 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index b4c5c718e4..dc8e5a31ee 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -32,7 +32,8 @@ set PWM_OUT none set SDCARD_MIXERS_PATH etc/mixers set USE_IO no set VEHICLE_TYPE none -set LOGGER_BUF 1000 +set LOGGER_ARGS "" +set LOGGER_BUF 1000 set RUN_MINIMAL_SHELL no @@ -287,6 +288,12 @@ mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $ud [ -e "$autostart_file".post ] && sh "$autostart_file".post # Run script to start logging +if param compare SYS_MC_EST_GROUP 2 +then + set LOGGER_ARGS "-p ekf2_timestamps" +else + set LOGGER_ARGS "-p vehicle_attitude" +fi sh etc/init.d/rc.logging mavlink boot_complete diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index 11584dfc62..404ea6a138 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -16,17 +16,17 @@ fi if param compare SDLOG_MODE 1 then - set LOGGER_ARGS "-e" + set LOGGER_ARGS "${LOGGER_ARGS} -e" fi if param compare SDLOG_MODE 2 then - set LOGGER_ARGS "-f" + set LOGGER_ARGS "${LOGGER_ARGS} -f" fi if param compare SDLOG_MODE 3 then - set LOGGER_ARGS "-x" + set LOGGER_ARGS "${LOGGER_ARGS} -x" fi if ! param compare SDLOG_MODE -1 diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index af4366bb21..886a55ffe5 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -54,7 +54,6 @@ #include #include #include -#include #include #include #include @@ -76,7 +75,7 @@ class AttitudeEstimatorQ : public ModuleBase, public ModuleP public: AttitudeEstimatorQ(); - ~AttitudeEstimatorQ() override = default; + ~AttitudeEstimatorQ() override; /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -118,9 +117,7 @@ private: uORB::Publication _att_pub{ORB_ID(vehicle_attitude)}; -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - uORB::Publication _ekf2_timestamps_pub {ORB_ID(ekf2_timestamps)}; -#endif + int _lockstep_component{-1}; float _mag_decl{0.0f}; float _bias_max{0.0f}; @@ -158,9 +155,6 @@ private: (ParamInt) _param_att_acc_comp, (ParamFloat) _param_att_bias_mas, (ParamInt) _param_sys_has_mag -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - , (ParamInt) _param_est_group -#endif ) }; @@ -183,6 +177,13 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() : _gyro_bias.zero(); update_parameters(true); + + _lockstep_component = px4_lockstep_register_component(); +} + +AttitudeEstimatorQ::~AttitudeEstimatorQ() +{ + px4_lockstep_unregister_component(_lockstep_component); } bool @@ -363,24 +364,9 @@ AttitudeEstimatorQ::Run() /* the instance count is not used here */ _att_pub.publish(att); -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - - if (_param_est_group.get() == 3) { - // In this case the estimator_q is running without LPE and needs - // to publish ekf2_timestamps because SITL lockstep requires it. - ekf2_timestamps_s ekf2_timestamps; - ekf2_timestamps.timestamp = now; - ekf2_timestamps.airspeed_timestamp_rel = 0; - ekf2_timestamps.distance_sensor_timestamp_rel = 0; - ekf2_timestamps.optical_flow_timestamp_rel = 0; - ekf2_timestamps.vehicle_air_data_timestamp_rel = 0; - ekf2_timestamps.vehicle_magnetometer_timestamp_rel = 0; - ekf2_timestamps.visual_odometry_timestamp_rel = 0; - _ekf2_timestamps_pub.publish(ekf2_timestamps); - } - -#endif } + + px4_lockstep_progress(_lockstep_component); } } diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index acb9f2dd08..bab3656fad 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -253,6 +253,7 @@ private: }; int _imu_sub_index{-1}; bool _callback_registered{false}; + int _lockstep_component{-1}; // because we can have several distance sensor instances with different orientations static constexpr int MAX_RNG_SENSOR_COUNT = 4; @@ -658,6 +659,7 @@ Ekf2::Ekf2(bool replay_mode): Ekf2::~Ekf2() { + px4_lockstep_unregister_component(_lockstep_component); perf_free(_ekf_update_perf); } @@ -1733,6 +1735,12 @@ void Ekf2::Run() // publish ekf2_timestamps _ekf2_timestamps_pub.publish(ekf2_timestamps); + + if (_lockstep_component == -1) { + _lockstep_component = px4_lockstep_register_component(); + } + + px4_lockstep_progress(_lockstep_component); } } diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 0c589016f2..8065c9487f 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -112,11 +112,9 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _ref_alt(0.0) { #if defined(ENABLE_LOCKSTEP_SCHEDULER) - // For lockstep 250 Hz are needed because ekf2_timestamps need to be - // published at 250 Hz. - _sensors_sub.set_interval_ms(4); + _lockstep_component = px4_lockstep_register_component(); #else - _sensors_sub.set_interval_ms(10); // main prediction loop, 100 hz + _sensors_sub.set_interval_ms(10); // main prediction loop, 100 hz (lockstep requires to run at full rate) #endif // assign distance subs to array @@ -147,6 +145,11 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : (_param_lpe_fusion.get() & FUSE_BARO) != 0); } +BlockLocalPositionEstimator::~BlockLocalPositionEstimator() +{ + px4_lockstep_unregister_component(_lockstep_component); +} + bool BlockLocalPositionEstimator::init() { @@ -504,7 +507,6 @@ void BlockLocalPositionEstimator::Run() if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _param_lpe_fake_origin.get())) { publishGlobalPos(); - publishEk2fTimestamps(); } } @@ -519,6 +521,8 @@ void BlockLocalPositionEstimator::Run() _xDelay.update(_x); _time_last_hist = _timeStamp; } + + px4_lockstep_progress(_lockstep_component); } void BlockLocalPositionEstimator::checkTimeouts() @@ -755,22 +759,6 @@ void BlockLocalPositionEstimator::publishEstimatorStatus() _pub_est_status.update(); } -void BlockLocalPositionEstimator::publishEk2fTimestamps() -{ - _pub_ekf2_timestamps.get().timestamp = _timeStamp; - - // We only really publish this as a dummy because lockstep simulation - // requires it to be published. - _pub_ekf2_timestamps.get().airspeed_timestamp_rel = 0; - _pub_ekf2_timestamps.get().distance_sensor_timestamp_rel = 0; - _pub_ekf2_timestamps.get().optical_flow_timestamp_rel = 0; - _pub_ekf2_timestamps.get().vehicle_air_data_timestamp_rel = 0; - _pub_ekf2_timestamps.get().vehicle_magnetometer_timestamp_rel = 0; - _pub_ekf2_timestamps.get().visual_odometry_timestamp_rel = 0; - - _pub_ekf2_timestamps.update(); -} - void BlockLocalPositionEstimator::publishGlobalPos() { // publish global position diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 9418b8059d..5c4f6a07ad 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -35,7 +35,6 @@ #include #include #include -#include using namespace matrix; using namespace control; @@ -112,7 +111,7 @@ class BlockLocalPositionEstimator : public ModuleBase _pub_gpos{ORB_ID(vehicle_global_position)}; uORB::PublicationData _pub_odom{ORB_ID(vehicle_odometry)}; uORB::PublicationData _pub_est_status{ORB_ID(estimator_status)}; - uORB::PublicationData _pub_ekf2_timestamps{ORB_ID(ekf2_timestamps)}; uORB::PublicationData _pub_innov{ORB_ID(estimator_innovations)}; uORB::PublicationData _pub_innov_var{ORB_ID(estimator_innovation_variances)}; @@ -338,6 +336,8 @@ private: uint64_t _time_last_land; uint64_t _time_last_target; + int _lockstep_component{-1}; + // reference altitudes float _altOrigin; bool _altOriginInitialized; diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 48ce1ae9df..a7d9b874e6 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -644,6 +644,10 @@ void Logger::run() hrt_abstime next_subscribe_check = 0; int next_subscribe_topic_index = -1; // this is used to distribute the checks over time + if (polling_topic_sub >= 0) { + _lockstep_component = px4_lockstep_register_component(); + } + while (!should_exit()) { // Start/stop logging (depending on logging mode, by default when arming/disarming) const bool logging_started = start_stop_logging(); @@ -790,42 +794,7 @@ void Logger::run() } } - -#ifndef ORB_USE_PUBLISHER_RULES - - // publish logger status - // - this is disabled in replay builds to ensure all data in ekf2 replay logs only contain - // the same time range, otherwise the plots can be unreadable using common tools - if (hrt_elapsed_time(&_logger_status_last) >= 1_s) { - for (int i = 0; i < (int)LogType::Count; ++i) { - - const LogType log_type = static_cast(i); - - if (_writer.is_started(log_type)) { - - const size_t buffer_fill_count_file = _writer.get_buffer_fill_count_file(log_type); - - const float kb_written = _writer.get_total_written_file(log_type) / 1024.0f; - const float seconds = hrt_elapsed_time(&_statistics[i].start_time_file) * 1e-6f; - - logger_status_s status; - status.type = i; - status.backend = _writer.backend(); - status.total_written_kb = kb_written; - status.write_rate_kb_s = kb_written / seconds; - status.dropouts = _statistics[i].write_dropouts; - status.buffer_used_bytes = buffer_fill_count_file; - status.buffer_size_bytes = _writer.get_buffer_size_file(log_type); - status.num_messages = _num_subscriptions; - status.timestamp = hrt_absolute_time(); - _logger_status_pub[i].publish(status); - } - } - - _logger_status_last = hrt_absolute_time(); - } - -#endif // !ORB_USE_PUBLISHER_RULES + publish_logger_status(); /* release the log buffer */ _writer.unlock(); @@ -870,6 +839,8 @@ void Logger::run() // wait for next loop iteration... if (polling_topic_sub >= 0) { + px4_lockstep_progress(_lockstep_component); + px4_pollfd_struct_t fds[1]; fds[0].fd = polling_topic_sub; fds[0].events = POLLIN; @@ -897,6 +868,8 @@ void Logger::run() } } + px4_lockstep_unregister_component(_lockstep_component); + stop_log_file(LogType::Full); stop_log_file(LogType::Mission); @@ -939,6 +912,38 @@ void Logger::debug_print_buffer(uint32_t &total_bytes, hrt_abstime &timer_start) #endif /* DBGPRINT */ } +void Logger::publish_logger_status() +{ + if (hrt_elapsed_time(&_logger_status_last) >= 1_s) { + for (int i = 0; i < (int)LogType::Count; ++i) { + + const LogType log_type = static_cast(i); + + if (_writer.is_started(log_type)) { + + const size_t buffer_fill_count_file = _writer.get_buffer_fill_count_file(log_type); + + const float kb_written = _writer.get_total_written_file(log_type) / 1024.0f; + const float seconds = hrt_elapsed_time(&_statistics[i].start_time_file) * 1e-6f; + + logger_status_s status; + status.type = i; + status.backend = _writer.backend(); + status.total_written_kb = kb_written; + status.write_rate_kb_s = kb_written / seconds; + status.dropouts = _statistics[i].write_dropouts; + status.buffer_used_bytes = buffer_fill_count_file; + status.buffer_size_bytes = _writer.get_buffer_size_file(log_type); + status.num_messages = _num_subscriptions; + status.timestamp = hrt_absolute_time(); + _logger_status_pub[i].publish(status); + } + } + + _logger_status_last = hrt_absolute_time(); + } +} + bool Logger::get_disable_boot_logging() { if (_param_sdlog_boot_bat.get()) { diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index af0fd10227..5d006875db 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -309,6 +309,7 @@ private: */ inline void debug_print_buffer(uint32_t &total_bytes, hrt_abstime &timer_start); + void publish_logger_status(); uint8_t *_msg_buffer{nullptr}; int _msg_buffer_len{0}; @@ -341,11 +342,10 @@ private: hrt_abstime _next_load_print{0}; ///< timestamp when to print the process load PrintLoadReason _print_load_reason {PrintLoadReason::Preflight}; - uORB::PublicationMulti _logger_status_pub[2] { ORB_ID(logger_status), ORB_ID(logger_status) }; + uORB::PublicationMulti _logger_status_pub[(int)LogType::Count] { ORB_ID(logger_status), ORB_ID(logger_status) }; -#ifndef ORB_USE_PUBLISHER_RULES // don't publish logger_status when building for replay hrt_abstime _logger_status_last {0}; -#endif + int _lockstep_component{-1}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index ab0cce5ba0..161e5d9f00 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -273,14 +273,6 @@ private: #if defined(ENABLE_LOCKSTEP_SCHEDULER) px4::atomic _has_initialized {false}; - - int _ekf2_timestamps_sub{-1}; - - enum class State { - WaitingForFirstEkf2Timestamp = 0, - WaitingForActuatorControls = 1, - WaitingForEkf2Timestamp = 2, - }; #endif DEFINE_PARAMETERS( diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index ce2e9cefa0..a9cb457242 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -604,71 +604,29 @@ void Simulator::send() fds_actuator_outputs[0].fd = _actuator_outputs_sub; fds_actuator_outputs[0].events = POLLIN; -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - px4_pollfd_struct_t fds_ekf2_timestamps[1] = {}; - fds_ekf2_timestamps[0].fd = _ekf2_timestamps_sub; - fds_ekf2_timestamps[0].events = POLLIN; - - State state = State::WaitingForFirstEkf2Timestamp; -#endif - while (true) { -#if defined(ENABLE_LOCKSTEP_SCHEDULER) + // Wait for up to 100ms for data. + int pret = px4_poll(&fds_actuator_outputs[0], 1, 100); - if (state == State::WaitingForActuatorControls) { -#endif - // Wait for up to 100ms for data. - int pret = px4_poll(&fds_actuator_outputs[0], 1, 100); - - if (pret == 0) { - // Timed out, try again. - continue; - } - - if (pret < 0) { - PX4_ERR("poll error %s", strerror(errno)); - continue; - } - - if (fds_actuator_outputs[0].revents & POLLIN) { - // Got new data to read, update all topics. - parameters_update(false); - _vehicle_status_sub.update(&_vehicle_status); - send_controls(); -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - state = State::WaitingForEkf2Timestamp; -#endif - } - -#if defined(ENABLE_LOCKSTEP_SCHEDULER) + if (pret == 0) { + // Timed out, try again. + continue; } -#endif - -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - - if (state == State::WaitingForFirstEkf2Timestamp || state == State::WaitingForEkf2Timestamp) { - int pret = px4_poll(&fds_ekf2_timestamps[0], 1, 100); - - if (pret == 0) { - // Timed out, try again. - continue; - } - - if (pret < 0) { - PX4_ERR("poll error %s", strerror(errno)); - continue; - } - - if (fds_ekf2_timestamps[0].revents & POLLIN) { - ekf2_timestamps_s timestamps; - orb_copy(ORB_ID(ekf2_timestamps), _ekf2_timestamps_sub, ×tamps); - state = State::WaitingForActuatorControls; - } + if (pret < 0) { + PX4_ERR("poll error %s", strerror(errno)); + continue; } -#endif + if (fds_actuator_outputs[0].revents & POLLIN) { + // Got new data to read, update all topics. + parameters_update(false); + _vehicle_status_sub.update(&_vehicle_status); + send_controls(); + // Wait for other modules, such as logger or ekf2 + px4_lockstep_wait_for_components(); + } } } @@ -812,10 +770,6 @@ void Simulator::run() // Only subscribe to the first actuator_outputs to fill a single HIL_ACTUATOR_CONTROLS. _actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0); -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - _ekf2_timestamps_sub = orb_subscribe(ORB_ID(ekf2_timestamps)); -#endif - // got data from simulator, now activate the sending thread pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, nullptr); pthread_attr_destroy(&sender_thread_attr); @@ -878,9 +832,6 @@ void Simulator::run() } orb_unsubscribe(_actuator_outputs_sub); -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - orb_unsubscribe(_ekf2_timestamps_sub); -#endif } #ifdef ENABLE_UART_RC_INPUT