From 98cff947025d2f5c3e4c69c2f4689a6dae02a2ca Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 3 Jan 2021 21:41:08 -0500 Subject: [PATCH] px4_work_queue: directly support SITL lockstep - the purpose is to ensure that every WorkItem (and WorkItems scheduled by WorkItems) is allowed to run to completion every step - per workqueue register a lockstep component whenever a work item is added (if not already registered) - once the work queue is empty unregister component --- .../px4_work_queue/WorkQueue.hpp | 4 ++++ platforms/common/px4_work_queue/WorkQueue.cpp | 18 ++++++++++++++++++ .../attitude_estimator_q_main.cpp | 13 +------------ src/modules/ekf2/EKF2.cpp | 12 ------------ src/modules/ekf2/EKF2.hpp | 1 - src/modules/ekf2/EKF2Selector.cpp | 9 --------- src/modules/ekf2/EKF2Selector.hpp | 2 -- .../BlockLocalPositionEstimator.cpp | 12 ------------ .../BlockLocalPositionEstimator.hpp | 4 +--- src/modules/sensors/sensors.cpp | 10 ---------- 10 files changed, 24 insertions(+), 61 deletions(-) diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp index 019ee6821e..d1ad917cdd 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp @@ -100,6 +100,10 @@ private: BlockingList _work_items; px4::atomic_bool _should_exit{false}; +#if defined(ENABLE_LOCKSTEP_SCHEDULER) + int _lockstep_component {-1}; +#endif // ENABLE_LOCKSTEP_SCHEDULER + }; } // namespace px4 diff --git a/platforms/common/px4_work_queue/WorkQueue.cpp b/platforms/common/px4_work_queue/WorkQueue.cpp index 8fe345ff9a..b6a172e7b8 100644 --- a/platforms/common/px4_work_queue/WorkQueue.cpp +++ b/platforms/common/px4_work_queue/WorkQueue.cpp @@ -106,6 +106,15 @@ void WorkQueue::Detach(WorkItem *item) void WorkQueue::Add(WorkItem *item) { work_lock(); + +#if defined(ENABLE_LOCKSTEP_SCHEDULER) + + if (_lockstep_component == -1) { + _lockstep_component = px4_lockstep_register_component(); + } + +#endif // ENABLE_LOCKSTEP_SCHEDULER + _q.push(item); work_unlock(); @@ -158,6 +167,15 @@ void WorkQueue::Run() work_lock(); // re-lock } +#if defined(ENABLE_LOCKSTEP_SCHEDULER) + + if (_q.empty()) { + px4_lockstep_unregister_component(_lockstep_component); + _lockstep_component = -1; + } + +#endif // ENABLE_LOCKSTEP_SCHEDULER + work_unlock(); } 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 c4153f9dff..ce90d2a7e1 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -75,7 +75,7 @@ class AttitudeEstimatorQ : public ModuleBase, public ModuleP public: AttitudeEstimatorQ(); - ~AttitudeEstimatorQ() override; + ~AttitudeEstimatorQ() override = default; /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -117,8 +117,6 @@ private: uORB::Publication _att_pub{ORB_ID(vehicle_attitude)}; - int _lockstep_component{-1}; - float _mag_decl{0.0f}; float _bias_max{0.0f}; @@ -177,13 +175,6 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() : _gyro_bias.zero(); update_parameters(true); - - _lockstep_component = px4_lockstep_register_component(); -} - -AttitudeEstimatorQ::~AttitudeEstimatorQ() -{ - px4_lockstep_unregister_component(_lockstep_component); } bool @@ -366,8 +357,6 @@ AttitudeEstimatorQ::Run() _att_pub.publish(att); } - - px4_lockstep_progress(_lockstep_component); } } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index b183d66558..be07e16213 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -194,10 +194,6 @@ EKF2::EKF2(int instance, const px4::wq_config_t &config, int imu, int mag, bool EKF2::~EKF2() { - if (!_multi_mode) { - px4_lockstep_unregister_component(_lockstep_component); - } - perf_free(_ecl_ekf_update_perf); perf_free(_ecl_ekf_update_full_perf); } @@ -468,14 +464,6 @@ void EKF2::Run() // publish ekf2_timestamps _ekf2_timestamps_pub.publish(ekf2_timestamps); - - if (!_multi_mode) { - if (_lockstep_component == -1) { - _lockstep_component = px4_lockstep_register_component(); - } - - px4_lockstep_progress(_lockstep_component); - } } } diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 2dc1ce866c..91e405ecb8 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -224,7 +224,6 @@ private: uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)}; bool _callback_registered{false}; - int _lockstep_component{-1}; bool _distance_sensor_selected{false}; // because we can have several distance sensor instances with different orientations bool _armed{false}; diff --git a/src/modules/ekf2/EKF2Selector.cpp b/src/modules/ekf2/EKF2Selector.cpp index 1b1b2b7832..5274dc05ff 100644 --- a/src/modules/ekf2/EKF2Selector.cpp +++ b/src/modules/ekf2/EKF2Selector.cpp @@ -49,8 +49,6 @@ EKF2Selector::EKF2Selector() : EKF2Selector::~EKF2Selector() { Stop(); - - px4_lockstep_unregister_component(_lockstep_component); } bool EKF2Selector::Start() @@ -326,11 +324,6 @@ void EKF2Selector::PublishVehicleAttitude(bool reset) attitude.timestamp = hrt_absolute_time(); _vehicle_attitude_pub.publish(attitude); - - // register lockstep component on first attitude publish - if (_lockstep_component == -1) { - _lockstep_component = px4_lockstep_register_component(); - } } } @@ -618,8 +611,6 @@ void EKF2Selector::Run() } } } - - px4_lockstep_progress(_lockstep_component); } void EKF2Selector::PrintStatus() diff --git a/src/modules/ekf2/EKF2Selector.hpp b/src/modules/ekf2/EKF2Selector.hpp index d402bf2164..44edfa9f54 100644 --- a/src/modules/ekf2/EKF2Selector.hpp +++ b/src/modules/ekf2/EKF2Selector.hpp @@ -173,8 +173,6 @@ private: uint8_t _lat_lon_reset_counter{0}; uint8_t _alt_reset_counter{0}; - int _lockstep_component{-1}; - uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _sensors_status_imu{ORB_ID(sensors_status_imu)}; diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index fb4878e7ab..86b10794d4 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -111,11 +111,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _ref_lon(0.0), _ref_alt(0.0) { -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - _lockstep_component = px4_lockstep_register_component(); -#else _sensors_sub.set_interval_ms(10); // main prediction loop, 100 hz (lockstep requires to run at full rate) -#endif // assign distance subs to array _dist_subs[0] = &_sub_dist0; @@ -145,11 +141,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : (_param_lpe_fusion.get() & FUSE_BARO) != 0); } -BlockLocalPositionEstimator::~BlockLocalPositionEstimator() -{ - px4_lockstep_unregister_component(_lockstep_component); -} - bool BlockLocalPositionEstimator::init() { @@ -161,7 +152,6 @@ BlockLocalPositionEstimator::init() return true; } - Vector BlockLocalPositionEstimator::dynamics( float t, const Vector &x, @@ -525,8 +515,6 @@ void BlockLocalPositionEstimator::Run() _xDelay.update(_x); _time_last_hist = _timeStamp; } - - px4_lockstep_progress(_lockstep_component); } void BlockLocalPositionEstimator::checkTimeouts() diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 312df7420e..690fcc5a19 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -112,7 +112,7 @@ class BlockLocalPositionEstimator : public ModuleBase