2013-12-29 07:56:30 -04:00
|
|
|
/*
|
|
|
|
This program is free software: you can redistribute it and/or modify
|
|
|
|
it under the terms of the GNU General Public License as published by
|
|
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
|
|
(at your option) any later version.
|
|
|
|
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
|
GNU General Public License for more details.
|
|
|
|
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
*/
|
|
|
|
|
2020-11-05 19:25:17 -04:00
|
|
|
#include "Replay.h"
|
2014-05-01 04:54:31 -03:00
|
|
|
|
2013-12-29 07:56:30 -04:00
|
|
|
#include "LogReader.h"
|
|
|
|
|
2020-11-05 19:25:17 -04:00
|
|
|
#include <stdio.h>
|
|
|
|
#include <AP_HAL/utility/getopt_cpp.h>
|
2019-04-04 09:12:31 -03:00
|
|
|
|
2020-11-05 19:25:17 -04:00
|
|
|
#include <AP_Vehicle/AP_Vehicle.h>
|
2015-10-22 12:36:14 -03:00
|
|
|
|
2020-11-05 19:25:17 -04:00
|
|
|
#include <GCS_MAVLink/GCS_Dummy.h>
|
2020-11-08 22:06:04 -04:00
|
|
|
#include <AP_Filesystem/AP_Filesystem.h>
|
|
|
|
#include <AP_Filesystem/posix_compat.h>
|
2015-05-09 19:20:32 -03:00
|
|
|
|
2020-11-08 22:06:04 -04:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
2020-11-05 19:25:17 -04:00
|
|
|
#include <AP_HAL_Linux/Scheduler.h>
|
2020-11-08 22:06:04 -04:00
|
|
|
#endif
|
2013-12-29 07:56:30 -04:00
|
|
|
|
2020-11-05 19:25:17 -04:00
|
|
|
#define streq(x, y) (!strcmp(x, y))
|
2013-12-29 18:47:50 -04:00
|
|
|
|
2020-11-05 19:25:17 -04:00
|
|
|
static ReplayVehicle replayvehicle;
|
2016-05-16 04:21:52 -03:00
|
|
|
|
2020-11-07 05:42:21 -04:00
|
|
|
// list of user parameters
|
|
|
|
user_parameter *user_parameters;
|
2020-11-15 23:34:44 -04:00
|
|
|
bool replay_force_ekf2;
|
|
|
|
bool replay_force_ekf3;
|
2020-11-07 05:42:21 -04:00
|
|
|
|
2015-06-20 09:30:00 -03:00
|
|
|
#define GSCALAR(v, name, def) { replayvehicle.g.v.vtype, name, Parameters::k_param_ ## v, &replayvehicle.g.v, {def_value : def} }
|
|
|
|
#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &replayvehicle.v, {group_info : class::var_info} }
|
|
|
|
#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, &replayvehicle.v, {group_info : class::var_info} }
|
2014-01-04 20:39:43 -04:00
|
|
|
|
2015-10-25 14:03:46 -03:00
|
|
|
const AP_Param::Info ReplayVehicle::var_info[] = {
|
2015-06-01 03:14:11 -03:00
|
|
|
GSCALAR(dummy, "_DUMMY", 0),
|
2014-03-01 00:15:46 -04:00
|
|
|
|
2020-12-03 04:14:10 -04:00
|
|
|
// @Group: BARO
|
2015-06-01 03:14:11 -03:00
|
|
|
// @Path: ../libraries/AP_Baro/AP_Baro.cpp
|
2020-12-03 04:14:10 -04:00
|
|
|
GOBJECT(barometer, "BARO", AP_Baro),
|
2015-06-01 03:14:11 -03:00
|
|
|
|
|
|
|
// @Group: INS_
|
|
|
|
// @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp
|
|
|
|
GOBJECT(ins, "INS_", AP_InertialSensor),
|
|
|
|
|
|
|
|
// @Group: AHRS_
|
|
|
|
// @Path: ../libraries/AP_AHRS/AP_AHRS.cpp
|
|
|
|
GOBJECT(ahrs, "AHRS_", AP_AHRS),
|
|
|
|
|
|
|
|
// @Group: ARSPD_
|
|
|
|
// @Path: ../libraries/AP_Airspeed/AP_Airspeed.cpp
|
2018-04-20 21:35:06 -03:00
|
|
|
GOBJECT(airspeed, "ARSP_", AP_Airspeed),
|
2015-06-01 03:14:11 -03:00
|
|
|
|
2015-09-22 22:55:26 -03:00
|
|
|
// @Group: EK2_
|
|
|
|
// @Path: ../libraries/AP_NavEKF2/AP_NavEKF2.cpp
|
2020-11-05 19:25:17 -04:00
|
|
|
GOBJECTN(ekf2, NavEKF2, "EK2_", NavEKF2),
|
2015-09-22 22:55:26 -03:00
|
|
|
|
2015-06-01 03:14:11 -03:00
|
|
|
// @Group: COMPASS_
|
|
|
|
// @Path: ../libraries/AP_Compass/AP_Compass.cpp
|
|
|
|
GOBJECT(compass, "COMPASS_", Compass),
|
|
|
|
|
2016-05-08 23:48:30 -03:00
|
|
|
// @Group: LOG
|
2019-01-18 00:23:42 -04:00
|
|
|
// @Path: ../libraries/AP_Logger/AP_Logger.cpp
|
2019-03-27 18:44:23 -03:00
|
|
|
GOBJECT(logger, "LOG", AP_Logger),
|
2016-05-08 23:48:30 -03:00
|
|
|
|
2016-07-14 02:08:43 -03:00
|
|
|
// @Group: EK3_
|
|
|
|
// @Path: ../libraries/AP_NavEKF3/AP_NavEKF3.cpp
|
2020-11-05 19:25:17 -04:00
|
|
|
GOBJECTN(ekf3, NavEKF3, "EK3_", NavEKF3),
|
2016-07-14 02:08:43 -03:00
|
|
|
|
2020-11-05 19:25:17 -04:00
|
|
|
// @Group: GPS_
|
|
|
|
// @Path: ../libraries/AP_GPS/AP_GPS.cpp
|
|
|
|
GOBJECT(gps, "GPS_", AP_GPS),
|
|
|
|
|
2015-06-01 03:14:11 -03:00
|
|
|
AP_VAREND
|
2014-12-07 20:25:22 -04:00
|
|
|
};
|
|
|
|
|
2015-06-20 09:30:00 -03:00
|
|
|
void ReplayVehicle::load_parameters(void)
|
2015-06-01 03:14:11 -03:00
|
|
|
{
|
|
|
|
if (!AP_Param::check_var_info()) {
|
2015-11-19 23:05:11 -04:00
|
|
|
AP_HAL::panic("Bad parameter table");
|
2015-06-01 03:14:11 -03:00
|
|
|
}
|
2020-11-08 16:45:44 -04:00
|
|
|
StorageManager::erase();
|
|
|
|
AP_Param::erase_all();
|
2020-03-09 00:28:23 -03:00
|
|
|
// Load all auto-loaded EEPROM variables - also registers thread
|
|
|
|
// which saves parameters, which Compass now does in its init() routine
|
|
|
|
AP_Param::load_all();
|
2020-11-05 19:25:17 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
const struct AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
|
|
|
|
AP_GROUPEND
|
|
|
|
};
|
|
|
|
GCS_Dummy _gcs;
|
2020-03-09 00:28:23 -03:00
|
|
|
|
2020-11-05 19:25:17 -04:00
|
|
|
AP_AdvancedFailsafe *AP::advancedfailsafe() { return nullptr; }
|
|
|
|
bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate, const char *reason) { return false; }
|
2020-03-09 00:28:23 -03:00
|
|
|
|
2020-11-05 19:25:17 -04:00
|
|
|
// dummy method to avoid linking AP_Avoidance
|
|
|
|
// AP_Avoidance *AP::ap_avoidance() { return nullptr; }
|
|
|
|
|
|
|
|
// avoid building/linking LTM:
|
|
|
|
void AP_LTM_Telem::init() {};
|
|
|
|
// avoid building/linking Devo:
|
|
|
|
void AP_DEVO_Telem::init() {};
|
2014-03-01 00:15:46 -04:00
|
|
|
|
2020-01-16 08:52:26 -04:00
|
|
|
void ReplayVehicle::init_ardupilot(void)
|
2015-07-04 09:43:54 -03:00
|
|
|
{
|
2018-04-22 07:19:11 -03:00
|
|
|
// we pass an empty log structure, filling the structure in with
|
|
|
|
// either the format present in the log (if we do not emit the
|
|
|
|
// message as a product of Replay), or the format understood in
|
|
|
|
// the current code (if we do emit the message in the normal
|
|
|
|
// places in the EKF, for example)
|
2019-03-27 18:44:23 -03:00
|
|
|
logger.Init(log_structure, 0);
|
2020-11-05 19:25:17 -04:00
|
|
|
logger.set_force_log_disarmed(true);
|
2015-07-07 02:13:51 -03:00
|
|
|
}
|
|
|
|
|
2020-11-07 05:42:21 -04:00
|
|
|
void Replay::usage(void)
|
|
|
|
{
|
|
|
|
::printf("Options:\n");
|
|
|
|
::printf("\t--parm NAME=VALUE set parameter NAME to VALUE\n");
|
|
|
|
::printf("\t--param-file FILENAME load parameters from a file\n");
|
2020-11-15 23:34:44 -04:00
|
|
|
::printf("\t--force-ekf2 force enable EKF2\n");
|
|
|
|
::printf("\t--force-ekf3 force enable EKF3\n");
|
2020-11-07 05:42:21 -04:00
|
|
|
}
|
|
|
|
|
2020-11-15 23:34:44 -04:00
|
|
|
enum param_key : uint8_t {
|
|
|
|
FORCE_EKF2 = 1,
|
|
|
|
FORCE_EKF3,
|
|
|
|
};
|
|
|
|
|
2015-06-09 21:09:56 -03:00
|
|
|
void Replay::_parse_command_line(uint8_t argc, char * const argv[])
|
2013-12-29 07:56:30 -04:00
|
|
|
{
|
2015-06-09 21:09:56 -03:00
|
|
|
const struct GetOptLong::option options[] = {
|
2016-03-10 18:54:00 -04:00
|
|
|
// name has_arg flag val
|
2020-11-07 05:42:21 -04:00
|
|
|
{"parm", true, 0, 'p'},
|
|
|
|
{"param", true, 0, 'p'},
|
|
|
|
{"param-file", true, 0, 'F'},
|
2020-11-15 23:34:44 -04:00
|
|
|
{"force-ekf2", false, 0, param_key::FORCE_EKF2},
|
|
|
|
{"force-ekf3", false, 0, param_key::FORCE_EKF3},
|
2020-11-07 05:42:21 -04:00
|
|
|
{"help", false, 0, 'h'},
|
2015-06-09 21:09:56 -03:00
|
|
|
{0, false, 0, 0}
|
|
|
|
};
|
|
|
|
|
2020-11-07 05:42:21 -04:00
|
|
|
GetOptLong gopt(argc, argv, "p:F:h", options);
|
2014-01-04 20:39:43 -04:00
|
|
|
|
2014-02-26 04:34:01 -04:00
|
|
|
int opt;
|
2015-06-09 21:09:56 -03:00
|
|
|
while ((opt = gopt.getoption()) != -1) {
|
2020-11-07 05:42:21 -04:00
|
|
|
switch (opt) {
|
|
|
|
case 'p': {
|
|
|
|
const char *eq = strchr(gopt.optarg, '=');
|
|
|
|
if (eq == NULL) {
|
|
|
|
::printf("Usage: -p NAME=VALUE\n");
|
|
|
|
exit(1);
|
|
|
|
}
|
|
|
|
struct user_parameter *u = new user_parameter;
|
|
|
|
strncpy(u->name, gopt.optarg, eq-gopt.optarg);
|
|
|
|
u->value = atof(eq+1);
|
|
|
|
u->next = user_parameters;
|
|
|
|
user_parameters = u;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
case 'F':
|
|
|
|
load_param_file(gopt.optarg);
|
|
|
|
break;
|
|
|
|
|
2020-11-15 23:34:44 -04:00
|
|
|
case param_key::FORCE_EKF2:
|
|
|
|
replay_force_ekf2 = true;
|
|
|
|
break;
|
|
|
|
|
|
|
|
case param_key::FORCE_EKF3:
|
|
|
|
replay_force_ekf3 = true;
|
|
|
|
break;
|
|
|
|
|
2020-11-07 05:42:21 -04:00
|
|
|
case 'h':
|
|
|
|
default:
|
|
|
|
usage();
|
|
|
|
exit(0);
|
|
|
|
}
|
2014-02-26 04:34:01 -04:00
|
|
|
}
|
2020-11-07 05:42:21 -04:00
|
|
|
|
2020-11-15 23:34:44 -04:00
|
|
|
argv += gopt.optind;
|
|
|
|
argc -= gopt.optind;
|
2014-02-26 04:34:01 -04:00
|
|
|
|
|
|
|
if (argc > 0) {
|
|
|
|
filename = argv[0];
|
2014-02-22 17:17:01 -04:00
|
|
|
}
|
2015-06-09 21:09:56 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void Replay::setup()
|
|
|
|
{
|
|
|
|
::printf("Starting\n");
|
|
|
|
|
|
|
|
uint8_t argc;
|
|
|
|
char * const *argv;
|
|
|
|
|
|
|
|
hal.util->commandline_arguments(argc, argv);
|
|
|
|
|
2020-11-08 16:45:44 -04:00
|
|
|
if (argc > 0) {
|
|
|
|
_parse_command_line(argc, argv);
|
|
|
|
}
|
2014-02-22 17:17:01 -04:00
|
|
|
|
2015-06-20 09:30:00 -03:00
|
|
|
_vehicle.setup();
|
2020-11-07 05:42:21 -04:00
|
|
|
|
|
|
|
set_user_parameters();
|
2014-01-04 20:39:43 -04:00
|
|
|
|
2020-11-15 23:34:44 -04:00
|
|
|
if (replay_force_ekf2) {
|
|
|
|
reader.set_parameter("EK2_ENABLE", 1, true);
|
|
|
|
}
|
|
|
|
if (replay_force_ekf3) {
|
|
|
|
reader.set_parameter("EK3_ENABLE", 1, true);
|
|
|
|
}
|
|
|
|
|
|
|
|
if (replay_force_ekf2 && replay_force_ekf3) {
|
|
|
|
::printf("Cannot force both EKF types\n");
|
|
|
|
exit(1);
|
|
|
|
}
|
|
|
|
|
2020-11-08 16:45:44 -04:00
|
|
|
if (filename == nullptr) {
|
2020-11-08 22:06:04 -04:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
|
|
|
|
// allow replay on stm32
|
|
|
|
filename = "APM/replayin.bin";
|
|
|
|
#else
|
2020-11-08 16:45:44 -04:00
|
|
|
::printf("You must supply a log filename\n");
|
|
|
|
exit(1);
|
2020-11-08 22:06:04 -04:00
|
|
|
#endif
|
2020-11-08 16:45:44 -04:00
|
|
|
}
|
2020-11-05 19:25:17 -04:00
|
|
|
// LogReader reader = LogReader(log_structure);
|
|
|
|
if (!reader.open_log(filename)) {
|
2020-11-08 22:06:04 -04:00
|
|
|
::printf("open(%s): %m\n", filename);
|
2020-11-05 19:25:17 -04:00
|
|
|
exit(1);
|
2016-11-02 09:29:58 -03:00
|
|
|
}
|
2020-11-08 16:45:44 -04:00
|
|
|
}
|
2015-06-29 23:48:49 -03:00
|
|
|
|
2020-11-08 16:45:44 -04:00
|
|
|
void Replay::loop()
|
|
|
|
{
|
|
|
|
if (!reader.update()) {
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
2020-03-09 00:28:23 -03:00
|
|
|
// If we don't tear down the threads then they continue to access
|
|
|
|
// global state during object destruction.
|
2020-11-08 16:45:44 -04:00
|
|
|
((Linux::Scheduler*)hal.scheduler)->teardown();
|
|
|
|
#endif
|
|
|
|
exit(0);
|
|
|
|
}
|
2016-11-02 09:29:58 -03:00
|
|
|
}
|
2015-06-26 02:12:17 -03:00
|
|
|
|
2020-11-07 05:42:21 -04:00
|
|
|
/*
|
|
|
|
setup user -p parameters
|
|
|
|
*/
|
|
|
|
void Replay::set_user_parameters(void)
|
|
|
|
{
|
|
|
|
for (struct user_parameter *u=user_parameters; u; u=u->next) {
|
|
|
|
if (!reader.set_parameter(u->name, u->value, true)) {
|
|
|
|
::printf("Failed to set parameter %s to %f\n", u->name, u->value);
|
|
|
|
exit(1);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
parse a parameter file line
|
|
|
|
*/
|
|
|
|
bool Replay::parse_param_line(char *line, char **vname, float &value)
|
|
|
|
{
|
|
|
|
if (line[0] == '#') {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
char *saveptr = NULL;
|
|
|
|
char *pname = strtok_r(line, ", =\t", &saveptr);
|
|
|
|
if (pname == NULL) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
if (strlen(pname) > AP_MAX_NAME_SIZE) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
const char *value_s = strtok_r(NULL, ", =\t", &saveptr);
|
|
|
|
if (value_s == NULL) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
value = atof(value_s);
|
|
|
|
*vname = pname;
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
load a default set of parameters from a file
|
|
|
|
*/
|
|
|
|
void Replay::load_param_file(const char *pfilename)
|
|
|
|
{
|
|
|
|
FILE *f = fopen(pfilename, "r");
|
|
|
|
if (f == NULL) {
|
|
|
|
printf("Failed to open parameter file: %s\n", pfilename);
|
|
|
|
exit(1);
|
|
|
|
}
|
|
|
|
char line[100];
|
|
|
|
|
|
|
|
while (fgets(line, sizeof(line)-1, f)) {
|
|
|
|
char *pname;
|
|
|
|
float value;
|
|
|
|
if (!parse_param_line(line, &pname, value)) {
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
struct user_parameter *u = new user_parameter;
|
2020-11-08 06:28:40 -04:00
|
|
|
strncpy_noterm(u->name, pname, sizeof(u->name));
|
2020-11-07 05:42:21 -04:00
|
|
|
u->value = value;
|
|
|
|
u->next = user_parameters;
|
|
|
|
user_parameters = u;
|
|
|
|
}
|
|
|
|
fclose(f);
|
|
|
|
}
|
|
|
|
|
2020-11-05 19:25:17 -04:00
|
|
|
Replay replay(replayvehicle);
|
|
|
|
AP_Vehicle& vehicle = replayvehicle;
|
2019-11-03 20:40:59 -04:00
|
|
|
|
2020-11-05 19:25:17 -04:00
|
|
|
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
2019-03-15 01:41:58 -03:00
|
|
|
|
2015-10-19 16:59:30 -03:00
|
|
|
AP_HAL_MAIN_CALLBACKS(&replay);
|