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
-9
View File
@@ -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()