fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 03:53:33 +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-23 11:53:55 -04: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/attitude_estimator_q/attitude_estimator_q_main.cpp
+11 -25
View File
@@ -54,7 +54,6 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/ekf2_timestamps.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
@@ -76,7 +75,7 @@ class AttitudeEstimatorQ : public ModuleBase<AttitudeEstimatorQ>, public ModuleP
public:
AttitudeEstimatorQ();
~AttitudeEstimatorQ() override = default;
~AttitudeEstimatorQ() override;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
@@ -118,9 +117,7 @@ private:
uORB::Publication<vehicle_attitude_s> _att_pub{ORB_ID(vehicle_attitude)};
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
uORB::Publication<ekf2_timestamps_s> _ekf2_timestamps_pub {ORB_ID(ekf2_timestamps)};
#endif
int _lockstep_component{-1};
float _mag_decl{0.0f};
float _bias_max{0.0f};
@@ -158,9 +155,6 @@ private:
(ParamInt<px4::params::ATT_ACC_COMP>) _param_att_acc_comp,
(ParamFloat<px4::params::ATT_BIAS_MAX>) _param_att_bias_mas,
(ParamInt<px4::params::SYS_HAS_MAG>) _param_sys_has_mag
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
, (ParamInt<px4::params::SYS_MC_EST_GROUP>) _param_est_group
#endif
)
};
@@ -183,6 +177,13 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
_gyro_bias.zero();
update_parameters(true);
_lockstep_component = px4_lockstep_register_component();
}
AttitudeEstimatorQ::~AttitudeEstimatorQ()
{
px4_lockstep_unregister_component(_lockstep_component);
}
bool
@@ -363,24 +364,9 @@ AttitudeEstimatorQ::Run()
/* the instance count is not used here */
_att_pub.publish(att);
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
if (_param_est_group.get() == 3) {
// In this case the estimator_q is running without LPE and needs
// to publish ekf2_timestamps because SITL lockstep requires it.
ekf2_timestamps_s ekf2_timestamps;
ekf2_timestamps.timestamp = now;
ekf2_timestamps.airspeed_timestamp_rel = 0;
ekf2_timestamps.distance_sensor_timestamp_rel = 0;
ekf2_timestamps.optical_flow_timestamp_rel = 0;
ekf2_timestamps.vehicle_air_data_timestamp_rel = 0;
ekf2_timestamps.vehicle_magnetometer_timestamp_rel = 0;
ekf2_timestamps.visual_odometry_timestamp_rel = 0;
_ekf2_timestamps_pub.publish(ekf2_timestamps);
}
#endif
}
px4_lockstep_progress(_lockstep_component);
}
}