fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-30 13:00:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

sitl: use lockstep components API

Browse Source 
- 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-18 11:43:22 +02:00
committed by Daniel Agar
parent 5378d1468f
commit 71dcf8d619
10 changed files with 102 additions and 165 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+9 -21
View File
@@ -112,11 +112,9 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_ref_alt(0.0)
{
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
// For lockstep 250 Hz are needed because ekf2_timestamps need to be
// published at 250 Hz.
_sensors_sub.set_interval_ms(4);
_lockstep_component = px4_lockstep_register_component();
#else
_sensors_sub.set_interval_ms(10); // main prediction loop, 100 hz
_sensors_sub.set_interval_ms(10); // main prediction loop, 100 hz (lockstep requires to run at full rate)
#endif
// assign distance subs to array
@@ -147,6 +145,11 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
(_param_lpe_fusion.get() & FUSE_BARO) != 0);
}
BlockLocalPositionEstimator::~BlockLocalPositionEstimator()
{
px4_lockstep_unregister_component(_lockstep_component);
}
bool
BlockLocalPositionEstimator::init()
{
@@ -504,7 +507,6 @@ void BlockLocalPositionEstimator::Run()
if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _param_lpe_fake_origin.get())) {
publishGlobalPos();
publishEk2fTimestamps();
}
}
@@ -519,6 +521,8 @@ void BlockLocalPositionEstimator::Run()
_xDelay.update(_x);
_time_last_hist = _timeStamp;
}
px4_lockstep_progress(_lockstep_component);
}
void BlockLocalPositionEstimator::checkTimeouts()
@@ -755,22 +759,6 @@ void BlockLocalPositionEstimator::publishEstimatorStatus()
_pub_est_status.update();
}
void BlockLocalPositionEstimator::publishEk2fTimestamps()
{
_pub_ekf2_timestamps.get().timestamp = _timeStamp;
// We only really publish this as a dummy because lockstep simulation
// requires it to be published.
_pub_ekf2_timestamps.get().airspeed_timestamp_rel = 0;
_pub_ekf2_timestamps.get().distance_sensor_timestamp_rel = 0;
_pub_ekf2_timestamps.get().optical_flow_timestamp_rel = 0;
_pub_ekf2_timestamps.get().vehicle_air_data_timestamp_rel = 0;
_pub_ekf2_timestamps.get().vehicle_magnetometer_timestamp_rel = 0;
_pub_ekf2_timestamps.get().visual_odometry_timestamp_rel = 0;
_pub_ekf2_timestamps.update();
}
void BlockLocalPositionEstimator::publishGlobalPos()
{
// publish global position