fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 11:47:35 +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/logger/logger.cpp
+41 -36
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()) {
src/modules/logger/logger.h
+3 -3
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)};