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
This commit is contained in:
Daniel Agar
2021-01-03 21:41:08 -05:00
committed by Lorenz Meier
parent 8c71ecd97e
commit 98cff94702
10 changed files with 24 additions and 61 deletions
@@ -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<float, BlockLocalPositionEstimator::n_x> BlockLocalPositionEstimator::dynamics(
float t,
const Vector<float, BlockLocalPositionEstimator::n_x> &x,
@@ -525,8 +515,6 @@ void BlockLocalPositionEstimator::Run()
_xDelay.update(_x);
_time_last_hist = _timeStamp;
}
px4_lockstep_progress(_lockstep_component);
}
void BlockLocalPositionEstimator::checkTimeouts()