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-04 09:38:05 +01:00
committed by Lorenz Meier
parent 8c71ecd97e
commit 98cff94702
10 changed files with 24 additions and 61 deletions
@@ -75,7 +75,7 @@ class AttitudeEstimatorQ : public ModuleBase<AttitudeEstimatorQ>, 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<vehicle_attitude_s> _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);
}
}