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).
This commit is contained in:
Beat Küng
2020-06-23 11:53:55 -04:00
committed by Daniel Agar
parent 5378d1468f
commit 71dcf8d619
10 changed files with 102 additions and 165 deletions
@@ -35,7 +35,6 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/estimator_innovations.h>
#include <uORB/topics/ekf2_timestamps.h>
using namespace matrix;
using namespace control;
@@ -112,7 +111,7 @@ class BlockLocalPositionEstimator : public ModuleBase<BlockLocalPositionEstimato
public:
BlockLocalPositionEstimator();
~BlockLocalPositionEstimator() override = default;
~BlockLocalPositionEstimator() override;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
@@ -287,7 +286,6 @@ private:
uORB::PublicationData<vehicle_global_position_s> _pub_gpos{ORB_ID(vehicle_global_position)};
uORB::PublicationData<vehicle_odometry_s> _pub_odom{ORB_ID(vehicle_odometry)};
uORB::PublicationData<estimator_status_s> _pub_est_status{ORB_ID(estimator_status)};
uORB::PublicationData<ekf2_timestamps_s> _pub_ekf2_timestamps{ORB_ID(ekf2_timestamps)};
uORB::PublicationData<estimator_innovations_s> _pub_innov{ORB_ID(estimator_innovations)};
uORB::PublicationData<estimator_innovations_s> _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;