dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

attitude_estimator_q: enable lockstep without LPE 

When lockstep is enabled and LPE is not running, the
attitude_estimator_q needs to publish ekf2_timestamps in order to signal
to the simulator module that it has fully run.

This essentially fixes flying in lockstep when SYS_MC_EST_GROUP is 3.
This commit is contained in:
Julian Oes 2020-03-03 09:00:32 +01:00 committed by Beat Küng
parent ee220a5086
commit 581b92dd3f
1 changed files with 26 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
View File

@ -54,6 +54,7 @@
#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>
@ -115,6 +116,10 @@ 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
float _mag_decl{0.0f};
float _bias_max{0.0f};
@ -151,6 +156,9 @@ 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
)
};
@ -345,6 +353,24 @@ 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
}
}
}