Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Replay: added --force-ekf2 and --force-ekf3 #15820

Merged
merged 7 commits into from Nov 17, 2020
Merged

Replay: added --force-ekf2 and --force-ekf3 #15820

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
966b060
AP_Logger: allow for replay with both EKF enabled
tridge Nov 16, 2020
4d31ae6
HAL_SITL: avoid sitl command line processing for replay
tridge Nov 16, 2020
0d7cb59
AP_DAL: use common events and structures for EKF2 and EKF3
tridge Nov 16, 2020
f44f90d
AP_NavEKF2: use common structure names
tridge Nov 16, 2020
b4b05c2
AP_NavEKF3: use common structure names
tridge Nov 16, 2020
0545f43
Replay: use common events and structures
tridge Nov 16, 2020
df4b829
AP_Filesystem: allow for replay on any file in local filesystem
tridge Nov 16, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
11 changes: 6 additions & 5 deletions libraries/AP_HAL_SITL/SITL_cmdline.cpp
View file
Open in desktop
Expand Up @@ -209,6 +209,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
static struct timeval first_tv;
gettimeofday(&first_tv, nullptr);
time_t start_time_UTC = first_tv.tv_sec;
const bool is_replay = APM_BUILD_TYPE(APM_BUILD_Replay);

enum long_options {
CMDLINE_GIMBAL = 1,
Expand Down Expand Up @@ -272,10 +273,10 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
{0, false, 0, 0}
};

#if APM_BUILD_TYPE(APM_BUILD_Replay)
model_str = "quad";
HALSITL::UARTDriver::_console = true;
#endif
if (is_replay) {
model_str = "quad";
HALSITL::UARTDriver::_console = true;
}

if (asprintf(&autotest_dir, SKETCHBOOK "/Tools/autotest") <= 0) {
AP_HAL::panic("out of memory");
Expand All @@ -287,7 +288,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])

GetOptLong gopt(argc, argv, "hwus:r:CI:P:SO:M:F:c:",
options);
while ((opt = gopt.getoption()) != -1) {
while (!is_replay && (opt = gopt.getoption()) != -1) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Maybe just nitpicking, but can't we just move the if (is_replay) here and put the while loop into the the else cause ?
That would also have the advantage to allow us to define another parser for the SITL replay configuration

switch (opt) {
case 'w':
AP_Param::erase_all();
Expand Down
38 changes: 27 additions & 11 deletions libraries/AP_Logger/AP_Logger.cpp
View file
Open in desktop
Expand Up @@ -911,7 +911,9 @@ void AP_Logger::WriteV(const char *name, const char *labels, const char *units,
if (f == nullptr) {
// unable to map name to a messagetype; could be out of
// msgtypes, could be out of slots, ...
#if !APM_BUILD_TYPE(APM_BUILD_Replay)
INTERNAL_ERROR(AP_InternalError::error_t::logger_mapfailure);
#endif
return;
}

Expand Down Expand Up @@ -950,7 +952,7 @@ bool AP_Logger::allow_start_ekf() const
}

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
void AP_Logger::assert_same_fmt_for_name(const AP_Logger::log_write_fmt *f,
bool AP_Logger::assert_same_fmt_for_name(const AP_Logger::log_write_fmt *f,
const char *name,
const char *labels,
const char *units,
Expand All @@ -967,6 +969,13 @@ void AP_Logger::assert_same_fmt_for_name(const AP_Logger::log_write_fmt *f,
Debug("format labels differ (%s) vs (%s)", f->labels, labels);
passed = false;
}
if (!streq(f->fmt, fmt)) {
Debug("format fmt differ (%s) vs (%s)",
(f->fmt ? f->fmt : "nullptr"),
(fmt ? fmt : "nullptr"));
passed = false;
}
#if !APM_BUILD_TYPE(APM_BUILD_Replay)
if ((f->units != nullptr && units == nullptr) ||
(f->units == nullptr && units != nullptr) ||
(units !=nullptr && !streq(f->units, units))) {
Expand All @@ -983,15 +992,11 @@ void AP_Logger::assert_same_fmt_for_name(const AP_Logger::log_write_fmt *f,
(mults ? mults : "nullptr"));
passed = false;
}
if (!streq(f->fmt, fmt)) {
Debug("format fmt differ (%s) vs (%s)",
(f->fmt ? f->fmt : "nullptr"),
(fmt ? fmt : "nullptr"));
passed = false;
}
if (!passed) {
AP_BoardConfig::config_error("See console: Format definition must be consistent for every call of Write");
}
#endif
return passed;
}
#endif

Expand All @@ -1003,22 +1008,33 @@ AP_Logger::log_write_fmt *AP_Logger::msg_fmt_for_name(const char *name, const ch
if (!direct_comp) {
if (f->name == name) { // ptr comparison
// already have an ID for this name:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !APM_BUILD_TYPE(APM_BUILD_Replay)
assert_same_fmt_for_name(f, name, labels, units, mults, fmt);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (!assert_same_fmt_for_name(f, name, labels, units, mults, fmt)) {
return nullptr;
}
#endif
return f;
}
} else {
// direct comparison used from scripting where pointer is not maintained
if (strcmp(f->name,name) == 0) {
// already have an ID for this name:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !APM_BUILD_TYPE(APM_BUILD_Replay)
assert_same_fmt_for_name(f, name, labels, units, mults, fmt);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (!assert_same_fmt_for_name(f, name, labels, units, mults, fmt)) {
return nullptr;
}
#endif
return f;
}
}
}

#if APM_BUILD_TYPE(APM_BUILD_Replay)
// don't allow for new msg types during replay. We will be able to
// support these eventually, but for now they cause corruption
return nullptr;
#endif

f = (struct log_write_fmt *)calloc(1, sizeof(*f));
if (f == nullptr) {
// out of memory
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_Logger/AP_Logger.h
View file
Open in desktop
Expand Up @@ -472,7 +472,7 @@ class AP_Logger
void validate_structures(const struct LogStructure *logstructures, const uint8_t num_types);
void dump_structure_field(const struct LogStructure *logstructure, const char *label, const uint8_t fieldnum);
void dump_structures(const struct LogStructure *logstructures, const uint8_t num_types);
void assert_same_fmt_for_name(const log_write_fmt *f,
bool assert_same_fmt_for_name(const log_write_fmt *f,
const char *name,
const char *labels,
const char *units,
Expand Down