fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ROMFS/px4fmu_common/init.d-posix/rcS
View File

@ -32,7 +32,8 @@ set PWM_OUT none
set SDCARD_MIXERS_PATH etc/mixers
set USE_IO no
set VEHICLE_TYPE none
set LOGGER_BUF 1000
set LOGGER_ARGS ""
set LOGGER_BUF 1000
set RUN_MINIMAL_SHELL no
@ -287,6 +288,12 @@ mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $ud
[ -e "$autostart_file".post ] && sh "$autostart_file".post
# Run script to start logging
if param compare SYS_MC_EST_GROUP 2
then
set LOGGER_ARGS "-p ekf2_timestamps"
else
set LOGGER_ARGS "-p vehicle_attitude"
fi
sh etc/init.d/rc.logging
mavlink boot_complete

6
ROMFS/px4fmu_common/init.d/rc.logging
View File

@ -16,17 +16,17 @@ fi
if param compare SDLOG_MODE 1
then
set LOGGER_ARGS "-e"
set LOGGER_ARGS "${LOGGER_ARGS} -e"
fi
if param compare SDLOG_MODE 2
then
set LOGGER_ARGS "-f"
set LOGGER_ARGS "${LOGGER_ARGS} -f"
fi
if param compare SDLOG_MODE 3
then
set LOGGER_ARGS "-x"
set LOGGER_ARGS "${LOGGER_ARGS} -x"
fi
if ! param compare SDLOG_MODE -1

36
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
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);
}
}

8
src/modules/ekf2/ekf2_main.cpp
View File

@ -253,6 +253,7 @@ private:
};
int _imu_sub_index{-1};
bool _callback_registered{false};
int _lockstep_component{-1};
// because we can have several distance sensor instances with different orientations
static constexpr int MAX_RNG_SENSOR_COUNT = 4;
@ -658,6 +659,7 @@ Ekf2::Ekf2(bool replay_mode):
Ekf2::~Ekf2()
{
px4_lockstep_unregister_component(_lockstep_component);
perf_free(_ekf_update_perf);
}
@ -1733,6 +1735,12 @@ void Ekf2::Run()
// publish ekf2_timestamps
_ekf2_timestamps_pub.publish(ekf2_timestamps);
if (_lockstep_component == -1) {
_lockstep_component = px4_lockstep_register_component();
}
px4_lockstep_progress(_lockstep_component);
}
}

30
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
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

6
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
View File

@ -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;

77
src/modules/logger/logger.cpp
View File

@ -644,6 +644,10 @@ void Logger::run()
hrt_abstime next_subscribe_check = 0;
int next_subscribe_topic_index = -1; // this is used to distribute the checks over time
if (polling_topic_sub >= 0) {
_lockstep_component = px4_lockstep_register_component();
}
while (!should_exit()) {
// Start/stop logging (depending on logging mode, by default when arming/disarming)
const bool logging_started = start_stop_logging();
@ -790,42 +794,7 @@ void Logger::run()
}
}
#ifndef ORB_USE_PUBLISHER_RULES
// publish logger status
// - this is disabled in replay builds to ensure all data in ekf2 replay logs only contain
// the same time range, otherwise the plots can be unreadable using common tools
if (hrt_elapsed_time(&_logger_status_last) >= 1_s) {
for (int i = 0; i < (int)LogType::Count; ++i) {
const LogType log_type = static_cast<LogType>(i);
if (_writer.is_started(log_type)) {
const size_t buffer_fill_count_file = _writer.get_buffer_fill_count_file(log_type);
const float kb_written = _writer.get_total_written_file(log_type) / 1024.0f;
const float seconds = hrt_elapsed_time(&_statistics[i].start_time_file) * 1e-6f;
logger_status_s status;
status.type = i;
status.backend = _writer.backend();
status.total_written_kb = kb_written;
status.write_rate_kb_s = kb_written / seconds;
status.dropouts = _statistics[i].write_dropouts;
status.buffer_used_bytes = buffer_fill_count_file;
status.buffer_size_bytes = _writer.get_buffer_size_file(log_type);
status.num_messages = _num_subscriptions;
status.timestamp = hrt_absolute_time();
_logger_status_pub[i].publish(status);
}
}
_logger_status_last = hrt_absolute_time();
}
#endif // !ORB_USE_PUBLISHER_RULES
publish_logger_status();
/* release the log buffer */
_writer.unlock();
@ -870,6 +839,8 @@ void Logger::run()
// wait for next loop iteration...
if (polling_topic_sub >= 0) {
px4_lockstep_progress(_lockstep_component);
px4_pollfd_struct_t fds[1];
fds[0].fd = polling_topic_sub;
fds[0].events = POLLIN;
@ -897,6 +868,8 @@ void Logger::run()
}
}
px4_lockstep_unregister_component(_lockstep_component);
stop_log_file(LogType::Full);
stop_log_file(LogType::Mission);
@ -939,6 +912,38 @@ void Logger::debug_print_buffer(uint32_t &total_bytes, hrt_abstime &timer_start)
#endif /* DBGPRINT */
}
void Logger::publish_logger_status()
{
if (hrt_elapsed_time(&_logger_status_last) >= 1_s) {
for (int i = 0; i < (int)LogType::Count; ++i) {
const LogType log_type = static_cast<LogType>(i);
if (_writer.is_started(log_type)) {
const size_t buffer_fill_count_file = _writer.get_buffer_fill_count_file(log_type);
const float kb_written = _writer.get_total_written_file(log_type) / 1024.0f;
const float seconds = hrt_elapsed_time(&_statistics[i].start_time_file) * 1e-6f;
logger_status_s status;
status.type = i;
status.backend = _writer.backend();
status.total_written_kb = kb_written;
status.write_rate_kb_s = kb_written / seconds;
status.dropouts = _statistics[i].write_dropouts;
status.buffer_used_bytes = buffer_fill_count_file;
status.buffer_size_bytes = _writer.get_buffer_size_file(log_type);
status.num_messages = _num_subscriptions;
status.timestamp = hrt_absolute_time();
_logger_status_pub[i].publish(status);
}
}
_logger_status_last = hrt_absolute_time();
}
}
bool Logger::get_disable_boot_logging()
{
if (_param_sdlog_boot_bat.get()) {

6
src/modules/logger/logger.h
View File

@ -309,6 +309,7 @@ private:
*/
inline void debug_print_buffer(uint32_t &total_bytes, hrt_abstime &timer_start);
void publish_logger_status();
uint8_t *_msg_buffer{nullptr};
int _msg_buffer_len{0};
@ -341,11 +342,10 @@ private:
hrt_abstime _next_load_print{0}; ///< timestamp when to print the process load
PrintLoadReason _print_load_reason {PrintLoadReason::Preflight};
uORB::PublicationMulti<logger_status_s> _logger_status_pub[2] { ORB_ID(logger_status), ORB_ID(logger_status) };
uORB::PublicationMulti<logger_status_s> _logger_status_pub[(int)LogType::Count] { ORB_ID(logger_status), ORB_ID(logger_status) };
#ifndef ORB_USE_PUBLISHER_RULES // don't publish logger_status when building for replay
hrt_abstime _logger_status_last {0};
#endif
int _lockstep_component{-1};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};

8
src/modules/simulator/simulator.h
View File

@ -273,14 +273,6 @@ private:
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
px4::atomic<bool> _has_initialized {false};
int _ekf2_timestamps_sub{-1};
enum class State {
WaitingForFirstEkf2Timestamp = 0,
WaitingForActuatorControls = 1,
WaitingForEkf2Timestamp = 2,
};
#endif
DEFINE_PARAMETERS(

81
src/modules/simulator/simulator_mavlink.cpp
View File

@ -604,71 +604,29 @@ void Simulator::send()
fds_actuator_outputs[0].fd = _actuator_outputs_sub;
fds_actuator_outputs[0].events = POLLIN;
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
px4_pollfd_struct_t fds_ekf2_timestamps[1] = {};
fds_ekf2_timestamps[0].fd = _ekf2_timestamps_sub;
fds_ekf2_timestamps[0].events = POLLIN;
State state = State::WaitingForFirstEkf2Timestamp;
#endif
while (true) {
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
// Wait for up to 100ms for data.
int pret = px4_poll(&fds_actuator_outputs[0], 1, 100);
if (state == State::WaitingForActuatorControls) {
#endif
// Wait for up to 100ms for data.
int pret = px4_poll(&fds_actuator_outputs[0], 1, 100);
if (pret == 0) {
// Timed out, try again.
continue;
}
if (pret < 0) {
PX4_ERR("poll error %s", strerror(errno));
continue;
}
if (fds_actuator_outputs[0].revents & POLLIN) {
// Got new data to read, update all topics.
parameters_update(false);
_vehicle_status_sub.update(&_vehicle_status);
send_controls();
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
state = State::WaitingForEkf2Timestamp;
#endif
}
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
if (pret == 0) {
// Timed out, try again.
continue;
}
#endif
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
if (state == State::WaitingForFirstEkf2Timestamp || state == State::WaitingForEkf2Timestamp) {
int pret = px4_poll(&fds_ekf2_timestamps[0], 1, 100);
if (pret == 0) {
// Timed out, try again.
continue;
}
if (pret < 0) {
PX4_ERR("poll error %s", strerror(errno));
continue;
}
if (fds_ekf2_timestamps[0].revents & POLLIN) {
ekf2_timestamps_s timestamps;
orb_copy(ORB_ID(ekf2_timestamps), _ekf2_timestamps_sub, &timestamps);
state = State::WaitingForActuatorControls;
}
if (pret < 0) {
PX4_ERR("poll error %s", strerror(errno));
continue;
}
#endif
if (fds_actuator_outputs[0].revents & POLLIN) {
// Got new data to read, update all topics.
parameters_update(false);
_vehicle_status_sub.update(&_vehicle_status);
send_controls();
// Wait for other modules, such as logger or ekf2
px4_lockstep_wait_for_components();
}
}
}
@ -812,10 +770,6 @@ void Simulator::run()
// Only subscribe to the first actuator_outputs to fill a single HIL_ACTUATOR_CONTROLS.
_actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0);
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
_ekf2_timestamps_sub = orb_subscribe(ORB_ID(ekf2_timestamps));
#endif
// got data from simulator, now activate the sending thread
pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, nullptr);
pthread_attr_destroy(&sender_thread_attr);
@ -878,9 +832,6 @@ void Simulator::run()
}
orb_unsubscribe(_actuator_outputs_sub);
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
orb_unsubscribe(_ekf2_timestamps_sub);
#endif
}
#ifdef ENABLE_UART_RC_INPUT