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/>.
|
|
|
|
*/
|
|
|
|
|
2015-08-11 03:28:41 -03:00
|
|
|
#include <AP_Param/AP_Param.h>
|
2020-03-09 00:28:23 -03:00
|
|
|
#include <AP_HAL_Linux/Scheduler.h>
|
2017-07-02 19:50:38 -03:00
|
|
|
#include <GCS_MAVLink/GCS_Dummy.h>
|
2015-06-29 02:59:53 -03:00
|
|
|
#include "Parameters.h"
|
|
|
|
#include "VehicleType.h"
|
|
|
|
#include "MsgHandler.h"
|
2013-12-29 07:56:30 -04:00
|
|
|
|
2014-05-13 06:10:24 -03:00
|
|
|
#ifndef INT16_MIN
|
2014-05-01 04:54:31 -03:00
|
|
|
#define INT16_MIN -32768
|
|
|
|
#define INT16_MAX 32767
|
2014-05-13 06:10:24 -03:00
|
|
|
#endif
|
2014-05-01 04:54:31 -03:00
|
|
|
|
2013-12-29 07:56:30 -04:00
|
|
|
#include "LogReader.h"
|
2019-01-18 02:18:52 -04:00
|
|
|
#include "DataFlashFileReader.h"
|
2016-05-25 07:46:18 -03:00
|
|
|
#include "Replay.h"
|
2013-12-29 07:56:30 -04:00
|
|
|
|
2019-04-04 09:12:31 -03:00
|
|
|
#include <AP_Camera/AP_Camera.h>
|
|
|
|
|
2015-10-22 12:36:14 -03:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
|
|
|
#include <SITL/SITL.h>
|
|
|
|
#endif
|
|
|
|
|
2015-05-09 19:20:32 -03:00
|
|
|
#define streq(x, y) (!strcmp(x, y))
|
|
|
|
|
2015-10-16 17:22:11 -03:00
|
|
|
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
2013-12-29 07:56:30 -04:00
|
|
|
|
2020-03-09 00:28:23 -03:00
|
|
|
static ReplayVehicle replayvehicle;
|
2013-12-29 18:47:50 -04:00
|
|
|
|
2016-05-16 04:21:52 -03:00
|
|
|
struct globals globals;
|
|
|
|
|
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
|
|
|
|
2015-06-01 03:14:11 -03:00
|
|
|
// barometer ground calibration. The GND_ prefix is chosen for
|
|
|
|
// compatibility with previous releases of ArduPlane
|
|
|
|
// @Group: GND_
|
|
|
|
// @Path: ../libraries/AP_Baro/AP_Baro.cpp
|
|
|
|
GOBJECT(barometer, "GND_", AP_Baro),
|
|
|
|
|
|
|
|
// @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
|
2019-12-10 07:01:38 -04:00
|
|
|
GOBJECTN(ahrs.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
|
2019-12-10 07:01:38 -04:00
|
|
|
GOBJECTN(ahrs.EKF3, NavEKF3, "EK3_", NavEKF3),
|
2016-07-14 02:08:43 -03:00
|
|
|
|
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
|
|
|
{
|
2016-05-03 00:21:42 -03:00
|
|
|
unlink("Replay.stg");
|
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-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();
|
|
|
|
|
|
|
|
// this compass has always been at war with the new compass priority code
|
|
|
|
const uint32_t compass_dev_id = AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, 0, 0, AP_Compass_Backend::DevTypes::DEVTYPE_SITL);
|
|
|
|
// logreader.set_parameter("DEV_ID", compass_dev_id);
|
|
|
|
// logreader.set_parameter("PRIO1_ID", compass_dev_id);
|
|
|
|
AP_Param::set_default_by_name("DEV_ID", compass_dev_id);
|
|
|
|
AP_Param::set_default_by_name("PRIO1_ID", compass_dev_id);
|
|
|
|
|
|
|
|
const struct param_default {
|
|
|
|
const char *name;
|
|
|
|
float value;
|
|
|
|
} param_defaults[] {
|
|
|
|
{ "EK2_ENABLE", 1 },
|
|
|
|
{ "EK2_IMU_MASK", 1 },
|
|
|
|
{ "EK3_ENABLE", 1 },
|
|
|
|
{ "EK3_IMU_MASK", 1 },
|
|
|
|
{ "LOG_REPLAY", 1 },
|
|
|
|
{ "AHRS_EKF_TYPE", 2 },
|
|
|
|
{ "LOG_FILE_BUFSIZE", 60 },
|
|
|
|
{ "COMPASS_DEV_ID", (float)compass_dev_id },
|
|
|
|
{ "COMPASS_PRIO1_ID", (float)compass_dev_id },
|
|
|
|
};
|
|
|
|
for (auto some_default : param_defaults) {
|
|
|
|
if (!AP_Param::set_default_by_name(some_default.name, some_default.value)) {
|
|
|
|
::fprintf(stderr, "set default failed\n");
|
|
|
|
abort();
|
|
|
|
}
|
|
|
|
}
|
2015-06-01 03:14:11 -03:00
|
|
|
}
|
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);
|
2015-06-20 09:30:00 -03:00
|
|
|
|
|
|
|
ahrs.set_compass(&compass);
|
|
|
|
ahrs.set_fly_forward(true);
|
|
|
|
ahrs.set_wind_estimation(true);
|
|
|
|
ahrs.set_correct_centrifugal(true);
|
|
|
|
ahrs.set_ekf_use(true);
|
2016-05-03 19:26:02 -03:00
|
|
|
|
2019-12-10 07:01:38 -04:00
|
|
|
ahrs.EKF2.set_enable(true);
|
|
|
|
ahrs.EKF3.set_enable(true);
|
2016-07-14 02:08:43 -03:00
|
|
|
|
2015-06-20 09:30:00 -03:00
|
|
|
printf("Starting disarmed\n");
|
|
|
|
hal.util->set_soft_armed(false);
|
|
|
|
|
|
|
|
barometer.init();
|
|
|
|
barometer.setHIL(0);
|
|
|
|
barometer.update();
|
|
|
|
compass.init();
|
|
|
|
ins.set_hil_mode();
|
|
|
|
}
|
|
|
|
|
|
|
|
Replay replay(replayvehicle);
|
2020-03-09 00:28:23 -03:00
|
|
|
AP_Vehicle& vehicle = replayvehicle;
|
2015-06-20 09:30:00 -03:00
|
|
|
|
2015-06-01 03:14:11 -03:00
|
|
|
void Replay::usage(void)
|
2014-03-01 00:15:46 -04:00
|
|
|
{
|
|
|
|
::printf("Options:\n");
|
2015-06-09 21:09:56 -03:00
|
|
|
::printf("\t--parm NAME=VALUE set parameter NAME to VALUE\n");
|
|
|
|
::printf("\t--accel-mask MASK set accel mask (1=accel1 only, 2=accel2 only, 3=both)\n");
|
|
|
|
::printf("\t--gyro-mask MASK set gyro mask (1=gyro1 only, 2=gyro2 only, 3=both)\n");
|
|
|
|
::printf("\t--arm-time time arm at time (milliseconds)\n");
|
2015-06-16 01:26:07 -03:00
|
|
|
::printf("\t--no-imt don't use IMT data\n");
|
2015-06-25 01:17:00 -03:00
|
|
|
::printf("\t--check-generate generate CHEK messages in output\n");
|
2015-06-26 02:12:17 -03:00
|
|
|
::printf("\t--check check solution against CHEK messages\n");
|
|
|
|
::printf("\t--tolerance-euler tolerance for euler angles in degrees\n");
|
|
|
|
::printf("\t--tolerance-pos tolerance for position in meters\n");
|
|
|
|
::printf("\t--tolerance-vel tolerance for velocity in meters/second\n");
|
2015-07-07 02:13:51 -03:00
|
|
|
::printf("\t--nottypes list of msg types not to output, comma separated\n");
|
2015-07-07 02:55:41 -03:00
|
|
|
::printf("\t--downsample downsampling rate for output\n");
|
2016-05-05 01:08:52 -03:00
|
|
|
::printf("\t--logmatch match logging rate to source\n");
|
2016-05-16 04:21:52 -03:00
|
|
|
::printf("\t--no-params don't use parameters from the log\n");
|
2016-03-10 18:54:00 -04:00
|
|
|
::printf("\t--no-fpe do not generate floating point exceptions\n");
|
2015-06-29 23:48:49 -03:00
|
|
|
::printf("\t--packet-counts print packet counts at end of processing\n");
|
2014-03-01 00:15:46 -04:00
|
|
|
}
|
|
|
|
|
2015-06-25 01:17:00 -03:00
|
|
|
|
|
|
|
enum {
|
2015-06-26 02:12:17 -03:00
|
|
|
OPT_CHECK = 128,
|
|
|
|
OPT_CHECK_GENERATE,
|
|
|
|
OPT_TOLERANCE_EULER,
|
|
|
|
OPT_TOLERANCE_POS,
|
2015-07-07 02:13:51 -03:00
|
|
|
OPT_TOLERANCE_VEL,
|
2015-07-07 02:55:41 -03:00
|
|
|
OPT_NOTTYPES,
|
2016-05-05 01:08:52 -03:00
|
|
|
OPT_DOWNSAMPLE,
|
2016-05-16 04:21:52 -03:00
|
|
|
OPT_LOGMATCH,
|
|
|
|
OPT_NOPARAMS,
|
2016-03-10 18:54:00 -04:00
|
|
|
OPT_PARAM_FILE,
|
|
|
|
OPT_NO_FPE,
|
2015-06-29 23:48:49 -03:00
|
|
|
OPT_PACKET_COUNTS,
|
2015-06-25 01:17:00 -03:00
|
|
|
};
|
|
|
|
|
2019-03-27 18:44:23 -03:00
|
|
|
void Replay::flush_logger(void) {
|
|
|
|
_vehicle.logger.flush();
|
2015-06-18 22:57:19 -03:00
|
|
|
}
|
|
|
|
|
2015-07-07 02:13:51 -03:00
|
|
|
/*
|
|
|
|
create a list from a comma separated string
|
|
|
|
*/
|
|
|
|
const char **Replay::parse_list_from_string(const char *str_in)
|
|
|
|
{
|
|
|
|
uint16_t comma_count=0;
|
|
|
|
const char *p;
|
|
|
|
for (p=str_in; *p; p++) {
|
|
|
|
if (*p == ',') comma_count++;
|
|
|
|
}
|
|
|
|
|
|
|
|
char *str = strdup(str_in);
|
|
|
|
if (str == NULL) {
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
const char **ret = (const char **)calloc(comma_count+2, sizeof(char *));
|
|
|
|
if (ret == NULL) {
|
|
|
|
free(str);
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
char *saveptr = NULL;
|
|
|
|
uint16_t idx = 0;
|
|
|
|
for (p=strtok_r(str, ",", &saveptr); p; p=strtok_r(NULL, ",", &saveptr)) {
|
|
|
|
ret[idx++] = p;
|
|
|
|
}
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
|
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
|
2015-06-09 21:09:56 -03:00
|
|
|
{"parm", true, 0, 'p'},
|
|
|
|
{"param", true, 0, 'p'},
|
2016-05-25 07:46:18 -03:00
|
|
|
{"param-file", true, 0, OPT_PARAM_FILE},
|
2015-06-09 21:09:56 -03:00
|
|
|
{"help", false, 0, 'h'},
|
|
|
|
{"accel-mask", true, 0, 'a'},
|
|
|
|
{"gyro-mask", true, 0, 'g'},
|
|
|
|
{"arm-time", true, 0, 'A'},
|
2015-06-16 01:26:07 -03:00
|
|
|
{"no-imt", false, 0, 'n'},
|
2015-06-26 02:12:17 -03:00
|
|
|
{"check-generate", false, 0, OPT_CHECK_GENERATE},
|
|
|
|
{"check", false, 0, OPT_CHECK},
|
|
|
|
{"tolerance-euler", true, 0, OPT_TOLERANCE_EULER},
|
|
|
|
{"tolerance-pos", true, 0, OPT_TOLERANCE_POS},
|
|
|
|
{"tolerance-vel", true, 0, OPT_TOLERANCE_VEL},
|
2015-07-07 02:13:51 -03:00
|
|
|
{"nottypes", true, 0, OPT_NOTTYPES},
|
2015-07-07 02:55:41 -03:00
|
|
|
{"downsample", true, 0, OPT_DOWNSAMPLE},
|
2016-05-05 01:08:52 -03:00
|
|
|
{"logmatch", false, 0, OPT_LOGMATCH},
|
2016-05-16 04:21:52 -03:00
|
|
|
{"no-params", false, 0, OPT_NOPARAMS},
|
2016-03-10 18:54:00 -04:00
|
|
|
{"no-fpe", false, 0, OPT_NO_FPE},
|
2015-06-29 23:48:49 -03:00
|
|
|
{"packet-counts", false, 0, OPT_PACKET_COUNTS},
|
2015-06-09 21:09:56 -03:00
|
|
|
{0, false, 0, 0}
|
|
|
|
};
|
|
|
|
|
2018-04-21 03:56:25 -03:00
|
|
|
GetOptLong gopt(argc, argv, "r:p:ha:g:A:n", 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) {
|
2014-02-26 04:34:01 -04:00
|
|
|
switch (opt) {
|
2014-03-01 17:00:13 -04:00
|
|
|
case 'g':
|
2015-06-09 21:09:56 -03:00
|
|
|
logreader.set_gyro_mask(strtol(gopt.optarg, NULL, 0));
|
2014-03-01 17:00:13 -04:00
|
|
|
break;
|
|
|
|
|
|
|
|
case 'a':
|
2015-06-09 21:09:56 -03:00
|
|
|
logreader.set_accel_mask(strtol(gopt.optarg, NULL, 0));
|
2014-03-01 17:00:13 -04:00
|
|
|
break;
|
|
|
|
|
2014-04-21 00:00:59 -03:00
|
|
|
case 'A':
|
2015-06-09 21:09:56 -03:00
|
|
|
arm_time_ms = strtol(gopt.optarg, NULL, 0);
|
2014-04-21 00:00:59 -03:00
|
|
|
break;
|
|
|
|
|
2015-06-16 01:26:07 -03:00
|
|
|
case 'n':
|
|
|
|
use_imt = false;
|
|
|
|
logreader.set_use_imt(use_imt);
|
|
|
|
break;
|
|
|
|
|
2015-06-25 01:17:00 -03:00
|
|
|
case 'p': {
|
2015-06-09 21:09:56 -03:00
|
|
|
const char *eq = strchr(gopt.optarg, '=');
|
2014-03-01 00:15:46 -04:00
|
|
|
if (eq == NULL) {
|
|
|
|
::printf("Usage: -p NAME=VALUE\n");
|
|
|
|
exit(1);
|
|
|
|
}
|
2016-05-25 07:46:18 -03:00
|
|
|
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;
|
2014-03-01 00:15:46 -04:00
|
|
|
break;
|
2014-02-26 04:34:01 -04:00
|
|
|
}
|
2015-06-25 01:17:00 -03:00
|
|
|
|
2015-06-26 02:12:17 -03:00
|
|
|
case OPT_CHECK_GENERATE:
|
|
|
|
check_generate = true;
|
|
|
|
break;
|
|
|
|
|
|
|
|
case OPT_CHECK:
|
|
|
|
check_solution = true;
|
|
|
|
break;
|
|
|
|
|
|
|
|
case OPT_TOLERANCE_EULER:
|
|
|
|
tolerance_euler = atof(gopt.optarg);
|
|
|
|
break;
|
|
|
|
|
|
|
|
case OPT_TOLERANCE_POS:
|
|
|
|
tolerance_pos = atof(gopt.optarg);
|
|
|
|
break;
|
|
|
|
|
|
|
|
case OPT_TOLERANCE_VEL:
|
|
|
|
tolerance_vel = atof(gopt.optarg);
|
2015-06-25 01:17:00 -03:00
|
|
|
break;
|
|
|
|
|
2015-07-07 02:13:51 -03:00
|
|
|
case OPT_NOTTYPES:
|
|
|
|
nottypes = parse_list_from_string(gopt.optarg);
|
|
|
|
break;
|
|
|
|
|
2015-07-07 02:55:41 -03:00
|
|
|
case OPT_DOWNSAMPLE:
|
|
|
|
downsample = atoi(gopt.optarg);
|
|
|
|
break;
|
|
|
|
|
2016-05-05 01:08:52 -03:00
|
|
|
case OPT_LOGMATCH:
|
|
|
|
logmatch = true;
|
|
|
|
break;
|
2016-05-16 04:21:52 -03:00
|
|
|
|
|
|
|
case OPT_NOPARAMS:
|
|
|
|
globals.no_params = true;
|
|
|
|
break;
|
2016-05-25 07:46:18 -03:00
|
|
|
|
|
|
|
case OPT_PARAM_FILE:
|
|
|
|
load_param_file(gopt.optarg);
|
|
|
|
break;
|
2016-05-05 01:08:52 -03:00
|
|
|
|
2016-03-10 18:54:00 -04:00
|
|
|
case OPT_NO_FPE:
|
|
|
|
generate_fpe = false;
|
|
|
|
break;
|
|
|
|
|
2015-06-29 23:48:49 -03:00
|
|
|
case OPT_PACKET_COUNTS:
|
|
|
|
packet_counts = true;
|
|
|
|
break;
|
|
|
|
|
2015-06-25 01:17:00 -03:00
|
|
|
case 'h':
|
|
|
|
default:
|
|
|
|
usage();
|
|
|
|
exit(0);
|
|
|
|
}
|
2014-02-26 04:34:01 -04:00
|
|
|
}
|
|
|
|
|
2015-06-09 21:09:56 -03: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
|
|
|
}
|
|
|
|
|
2019-01-18 00:23:42 -04:00
|
|
|
class IMUCounter : public AP_LoggerFileReader {
|
2015-06-12 21:23:12 -03:00
|
|
|
public:
|
2015-07-04 02:14:14 -03:00
|
|
|
IMUCounter() {}
|
2019-09-03 07:03:59 -03:00
|
|
|
bool handle_log_format_msg(const struct log_Format &f) override;
|
2020-03-09 00:28:23 -03:00
|
|
|
bool handle_msg(const struct log_Format &f, uint8_t *msg, uint8_t &core) override;
|
2015-06-12 21:23:12 -03:00
|
|
|
|
2016-05-24 21:34:41 -03:00
|
|
|
uint64_t last_clock_timestamp = 0;
|
|
|
|
float last_parm_value = 0;
|
|
|
|
char last_parm_name[17] {};
|
2015-06-12 21:23:12 -03:00
|
|
|
private:
|
2016-05-24 21:34:41 -03:00
|
|
|
MsgHandler *handler = nullptr;
|
|
|
|
MsgHandler *parm_handler = nullptr;
|
2015-06-12 21:23:12 -03:00
|
|
|
};
|
2015-07-04 02:14:14 -03:00
|
|
|
|
|
|
|
bool IMUCounter::handle_log_format_msg(const struct log_Format &f) {
|
2015-09-09 22:26:12 -03:00
|
|
|
if (!strncmp(f.name,"IMU",4) ||
|
|
|
|
!strncmp(f.name,"IMT",4)) {
|
2016-05-24 21:34:41 -03:00
|
|
|
// an IMU or IMT message message format
|
2015-06-12 21:23:12 -03:00
|
|
|
handler = new MsgHandler(f);
|
|
|
|
}
|
2016-05-24 21:34:41 -03:00
|
|
|
if (strncmp(f.name,"PARM",4) == 0) {
|
|
|
|
// PARM message message format
|
|
|
|
parm_handler = new MsgHandler(f);
|
|
|
|
}
|
2015-06-12 21:23:12 -03:00
|
|
|
|
|
|
|
return true;
|
|
|
|
};
|
2015-07-04 02:14:14 -03:00
|
|
|
|
2020-03-09 00:28:23 -03:00
|
|
|
bool IMUCounter::handle_msg(const struct log_Format &f, uint8_t *msg, uint8_t &core) {
|
2016-05-24 21:34:41 -03:00
|
|
|
if (strncmp(f.name,"PARM",4) == 0) {
|
|
|
|
// gather parameter values to check for SCHED_LOOP_RATE
|
|
|
|
parm_handler->field_value(msg, "Name", last_parm_name, sizeof(last_parm_name));
|
|
|
|
parm_handler->field_value(msg, "Value", last_parm_value);
|
|
|
|
return true;
|
|
|
|
}
|
2015-09-09 22:26:12 -03:00
|
|
|
if (strncmp(f.name,"IMU",4) &&
|
|
|
|
strncmp(f.name,"IMT",4)) {
|
2015-07-04 02:14:14 -03:00
|
|
|
// not an IMU message
|
2015-06-12 21:23:12 -03:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2015-09-09 22:26:12 -03:00
|
|
|
if (handler->field_value(msg, "TimeUS", last_clock_timestamp)) {
|
|
|
|
} else if (handler->field_value(msg, "TimeMS", last_clock_timestamp)) {
|
|
|
|
last_clock_timestamp *= 1000;
|
2015-06-12 21:23:12 -03:00
|
|
|
} else {
|
2015-09-09 22:26:12 -03:00
|
|
|
::printf("Unable to find timestamp in message");
|
2015-06-12 21:23:12 -03:00
|
|
|
}
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2015-07-04 02:14:14 -03:00
|
|
|
/*
|
|
|
|
find information about the log
|
|
|
|
*/
|
|
|
|
bool Replay::find_log_info(struct log_information &info)
|
|
|
|
{
|
|
|
|
IMUCounter reader;
|
2015-06-12 21:23:12 -03:00
|
|
|
if (!reader.open_log(filename)) {
|
|
|
|
perror(filename);
|
|
|
|
exit(1);
|
|
|
|
}
|
2015-09-09 22:26:12 -03:00
|
|
|
char clock_source[5] = { };
|
2015-06-12 21:23:12 -03:00
|
|
|
int samplecount = 0;
|
|
|
|
uint64_t prev = 0;
|
2015-07-04 02:14:14 -03:00
|
|
|
uint64_t smallest_delta = 0;
|
2016-05-10 02:40:30 -03:00
|
|
|
uint64_t total_delta = 0;
|
2015-06-12 21:23:12 -03:00
|
|
|
prev = 0;
|
2015-09-09 22:26:12 -03:00
|
|
|
const uint16_t samples_required = 1000;
|
|
|
|
while (samplecount < samples_required) {
|
2015-06-12 21:23:12 -03:00
|
|
|
char type[5];
|
2020-03-09 00:28:23 -03:00
|
|
|
uint8_t core; // unused
|
|
|
|
if (!reader.update(type, core)) {
|
2015-06-12 21:23:12 -03:00
|
|
|
break;
|
|
|
|
}
|
2015-09-09 22:26:12 -03:00
|
|
|
|
2016-05-24 21:34:41 -03:00
|
|
|
if (streq(type, "PARM") && streq(reader.last_parm_name, "SCHED_LOOP_RATE")) {
|
|
|
|
// get rate directly from parameters
|
|
|
|
info.update_rate = reader.last_parm_value;
|
|
|
|
}
|
2015-09-09 22:26:12 -03:00
|
|
|
if (strlen(clock_source) == 0) {
|
2015-11-30 19:49:43 -04:00
|
|
|
// If you want to add a clock source, also add it to
|
|
|
|
// handle_msg and handle_log_format_msg, above. Note that
|
|
|
|
// ordering is important here. For example, when we log
|
|
|
|
// IMT we may reduce the logging speed of IMU, so then
|
|
|
|
// using IMU as your clock source will lead to incorrect
|
|
|
|
// behaviour.
|
|
|
|
if (streq(type, "IMT")) {
|
2016-05-03 00:21:42 -03:00
|
|
|
strcpy(clock_source, "IMT");
|
2015-11-30 19:49:43 -04:00
|
|
|
} else if (streq(type, "IMU")) {
|
2016-05-03 00:21:42 -03:00
|
|
|
strcpy(clock_source, "IMU");
|
2015-09-09 22:26:12 -03:00
|
|
|
} else {
|
|
|
|
continue;
|
|
|
|
}
|
2016-05-03 00:21:42 -03:00
|
|
|
hal.console->printf("Using clock source %s\n", clock_source);
|
|
|
|
}
|
|
|
|
// IMT if available always overrides
|
|
|
|
if (streq(type, "IMT") && strcmp(clock_source, "IMT") != 0) {
|
|
|
|
strcpy(clock_source, "IMT");
|
|
|
|
hal.console->printf("Changing clock source to %s\n", clock_source);
|
2016-05-04 02:25:14 -03:00
|
|
|
samplecount = 0;
|
|
|
|
prev = 0;
|
|
|
|
smallest_delta = 0;
|
2016-05-10 02:40:30 -03:00
|
|
|
total_delta = 0;
|
2015-09-09 22:26:12 -03:00
|
|
|
}
|
|
|
|
if (streq(type, clock_source)) {
|
2015-06-12 21:23:12 -03:00
|
|
|
if (prev == 0) {
|
2015-09-09 22:26:12 -03:00
|
|
|
prev = reader.last_clock_timestamp;
|
2015-06-12 21:23:12 -03:00
|
|
|
} else {
|
2015-09-09 22:26:12 -03:00
|
|
|
uint64_t delta = reader.last_clock_timestamp - prev;
|
2016-05-10 02:40:30 -03:00
|
|
|
if (delta < 40000 && delta > 1000) {
|
|
|
|
if (smallest_delta == 0 || delta < smallest_delta) {
|
|
|
|
smallest_delta = delta;
|
|
|
|
}
|
|
|
|
samplecount++;
|
|
|
|
total_delta += delta;
|
2015-07-04 02:14:14 -03:00
|
|
|
}
|
2015-06-12 21:23:12 -03:00
|
|
|
}
|
2016-05-10 02:40:30 -03:00
|
|
|
prev = reader.last_clock_timestamp;
|
2015-06-12 21:23:12 -03:00
|
|
|
}
|
2015-09-09 22:26:12 -03:00
|
|
|
|
2016-05-03 19:26:02 -03:00
|
|
|
if (streq(type, "IMU2")) {
|
2015-07-04 02:14:14 -03:00
|
|
|
info.have_imu2 = true;
|
|
|
|
}
|
2016-05-03 19:26:02 -03:00
|
|
|
if (streq(type, "IMT")) {
|
|
|
|
info.have_imt = true;
|
|
|
|
}
|
|
|
|
if (streq(type, "IMT2")) {
|
|
|
|
info.have_imt2 = true;
|
|
|
|
}
|
2015-06-12 21:23:12 -03:00
|
|
|
}
|
2015-07-04 02:14:14 -03:00
|
|
|
if (smallest_delta == 0) {
|
2015-09-09 22:26:12 -03:00
|
|
|
::printf("Unable to determine log rate - insufficient IMU/IMT messages? (need=%d got=%d)", samples_required, samplecount);
|
2015-07-04 02:14:14 -03:00
|
|
|
return false;
|
2015-06-12 21:23:12 -03:00
|
|
|
}
|
|
|
|
|
2016-05-10 02:40:30 -03:00
|
|
|
float average_delta = total_delta / samplecount;
|
|
|
|
float rate = 1.0e6f/average_delta;
|
|
|
|
printf("average_delta=%.2f smallest_delta=%lu samplecount=%lu\n",
|
|
|
|
average_delta, (unsigned long)smallest_delta, (unsigned long)samplecount);
|
2015-07-04 02:14:14 -03:00
|
|
|
if (rate < 100) {
|
|
|
|
info.update_rate = 50;
|
|
|
|
} else {
|
|
|
|
info.update_rate = 400;
|
2015-06-12 21:23:12 -03:00
|
|
|
}
|
2015-07-04 02:14:14 -03:00
|
|
|
return true;
|
2015-06-12 21:23:12 -03:00
|
|
|
}
|
|
|
|
|
2015-06-18 22:57:19 -03:00
|
|
|
// catch floating point exceptions
|
|
|
|
static void _replay_sig_fpe(int signum)
|
|
|
|
{
|
2019-03-27 18:44:23 -03:00
|
|
|
fprintf(stderr, "ERROR: Floating point exception - flushing logger...\n");
|
|
|
|
replay.flush_logger();
|
2015-06-18 22:57:19 -03:00
|
|
|
fprintf(stderr, "ERROR: ... and aborting.\n");
|
2015-07-02 01:27:10 -03:00
|
|
|
if (replay.check_solution) {
|
|
|
|
FILE *f = fopen("replay_results.txt","a");
|
|
|
|
fprintf(f, "%s\tFPE\tFPE\tFPE\tFPE\tFPE\n",
|
|
|
|
replay.log_filename);
|
|
|
|
fclose(f);
|
|
|
|
}
|
2015-06-18 22:57:19 -03:00
|
|
|
abort();
|
|
|
|
}
|
|
|
|
|
2016-11-16 20:28:11 -04:00
|
|
|
FILE *Replay::xfopen(const char *f, const char *mode)
|
2016-11-15 22:18:34 -04:00
|
|
|
{
|
2016-11-16 20:28:11 -04:00
|
|
|
FILE *ret = fopen(f, mode);
|
2016-11-15 22:18:34 -04:00
|
|
|
if (ret == nullptr) {
|
2016-11-16 20:28:11 -04:00
|
|
|
::fprintf(stderr, "Failed to open (%s): %m\n", f);
|
2016-11-15 22:18:34 -04:00
|
|
|
abort();
|
|
|
|
}
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
|
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);
|
|
|
|
|
|
|
|
_parse_command_line(argc, argv);
|
2014-02-22 17:17:01 -04:00
|
|
|
|
2015-07-08 21:20:45 -03:00
|
|
|
if (!check_generate) {
|
|
|
|
logreader.set_save_chek_messages(true);
|
|
|
|
}
|
|
|
|
|
2016-07-12 12:05:32 -03:00
|
|
|
set_signal_handlers();
|
2015-06-18 22:57:19 -03:00
|
|
|
|
2020-03-09 00:28:23 -03:00
|
|
|
// never use the system clock:
|
|
|
|
hal.scheduler->stop_clock(1);
|
|
|
|
|
2014-02-22 17:17:01 -04:00
|
|
|
hal.console->printf("Processing log %s\n", filename);
|
2015-06-12 21:23:12 -03:00
|
|
|
|
2015-07-01 23:23:48 -03:00
|
|
|
// remember filename for reporting
|
|
|
|
log_filename = filename;
|
|
|
|
|
2015-07-04 02:14:14 -03:00
|
|
|
if (!find_log_info(log_info)) {
|
|
|
|
printf("Update to get log information\n");
|
|
|
|
exit(1);
|
2015-06-12 21:23:12 -03:00
|
|
|
}
|
|
|
|
|
2015-07-04 02:14:14 -03:00
|
|
|
hal.console->printf("Using an update rate of %u Hz\n", log_info.update_rate);
|
2014-02-28 23:24:51 -04:00
|
|
|
|
2015-06-01 03:14:11 -03:00
|
|
|
if (!logreader.open_log(filename)) {
|
2014-02-28 23:24:51 -04:00
|
|
|
perror(filename);
|
|
|
|
exit(1);
|
|
|
|
}
|
2013-12-29 07:56:30 -04:00
|
|
|
|
2015-06-20 09:30:00 -03:00
|
|
|
_vehicle.setup();
|
2015-09-23 05:56:15 -03:00
|
|
|
|
2015-10-22 03:40:21 -03:00
|
|
|
inhibit_gyro_cal();
|
2018-04-22 07:19:11 -03:00
|
|
|
force_log_disarmed();
|
2016-05-03 19:26:02 -03:00
|
|
|
|
2020-03-09 00:28:23 -03:00
|
|
|
// FIXME: base this on key parameters rather than the loop rate
|
2016-05-03 19:26:02 -03:00
|
|
|
if (log_info.update_rate == 400) {
|
|
|
|
// assume copter for 400Hz
|
|
|
|
_vehicle.ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER);
|
|
|
|
_vehicle.ahrs.set_fly_forward(false);
|
|
|
|
} else if (log_info.update_rate == 50) {
|
|
|
|
// assume copter for 400Hz
|
|
|
|
_vehicle.ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING);
|
|
|
|
_vehicle.ahrs.set_fly_forward(true);
|
|
|
|
}
|
|
|
|
|
2015-07-04 02:14:14 -03:00
|
|
|
set_ins_update_rate(log_info.update_rate);
|
2014-01-04 20:39:43 -04:00
|
|
|
}
|
|
|
|
|
2015-06-25 01:17:00 -03:00
|
|
|
void Replay::set_ins_update_rate(uint16_t _update_rate) {
|
2015-12-26 00:44:44 -04:00
|
|
|
_vehicle.ins.init(_update_rate);
|
2015-06-20 09:30:00 -03:00
|
|
|
}
|
|
|
|
|
2015-10-22 03:40:21 -03:00
|
|
|
void Replay::inhibit_gyro_cal() {
|
2015-10-26 03:37:16 -03:00
|
|
|
if (!logreader.set_parameter("INS_GYR_CAL", AP_InertialSensor::GYRO_CAL_NEVER)) {
|
|
|
|
::fprintf(stderr, "Failed to set GYR_CAL parameter\n");
|
2015-10-22 03:40:21 -03:00
|
|
|
abort();
|
|
|
|
}
|
|
|
|
}
|
2018-04-22 07:19:11 -03:00
|
|
|
void Replay::force_log_disarmed() {
|
|
|
|
if (!logreader.set_parameter("LOG_DISARMED", 1)) {
|
|
|
|
::fprintf(stderr, "Failed to set LOG_DISARMED parameter\n");
|
|
|
|
abort();
|
|
|
|
}
|
|
|
|
}
|
2014-03-01 00:15:46 -04:00
|
|
|
|
|
|
|
/*
|
|
|
|
setup user -p parameters
|
|
|
|
*/
|
2015-06-01 03:14:11 -03:00
|
|
|
void Replay::set_user_parameters(void)
|
2014-03-01 00:15:46 -04:00
|
|
|
{
|
2016-05-25 07:46:18 -03:00
|
|
|
for (struct user_parameter *u=user_parameters; u; u=u->next) {
|
|
|
|
if (!logreader.set_parameter(u->name, u->value)) {
|
|
|
|
::printf("Failed to set parameter %s to %f\n", u->name, u->value);
|
2014-03-01 00:15:46 -04:00
|
|
|
exit(1);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2016-07-12 12:05:32 -03:00
|
|
|
void Replay::set_signal_handlers(void)
|
|
|
|
{
|
2016-07-13 09:55:18 -03:00
|
|
|
struct sigaction sa;
|
|
|
|
|
|
|
|
sigemptyset(&sa.sa_mask);
|
|
|
|
sa.sa_flags = 0;
|
|
|
|
|
2016-07-12 12:05:32 -03:00
|
|
|
if (generate_fpe) {
|
|
|
|
// SITL_State::_parse_command_line sets up an FPE handler. We
|
|
|
|
// can do better:
|
|
|
|
feenableexcept(FE_INVALID | FE_OVERFLOW);
|
2016-07-13 09:55:18 -03:00
|
|
|
sa.sa_handler = _replay_sig_fpe;
|
2016-07-12 12:05:32 -03:00
|
|
|
} else {
|
|
|
|
// disable floating point exception generation:
|
|
|
|
int exceptions = FE_OVERFLOW | FE_DIVBYZERO;
|
|
|
|
#ifndef __i386__
|
|
|
|
// i386 with gcc doesn't work with FE_INVALID
|
|
|
|
exceptions |= FE_INVALID;
|
|
|
|
#endif
|
|
|
|
if (feclearexcept(exceptions)) {
|
|
|
|
::fprintf(stderr, "Failed to disable floating point exceptions: %s", strerror(errno));
|
|
|
|
}
|
2016-07-13 09:55:18 -03:00
|
|
|
sa.sa_handler = SIG_IGN;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (sigaction(SIGFPE, &sa, nullptr) < 0) {
|
|
|
|
::fprintf(stderr, "Failed to set floating point exceptions' handler: %s", strerror(errno));
|
2016-07-12 12:05:32 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2016-05-05 01:08:52 -03:00
|
|
|
/*
|
|
|
|
write out EKF log messages
|
|
|
|
*/
|
|
|
|
void Replay::write_ekf_logs(void)
|
|
|
|
{
|
|
|
|
if (!LogReader::in_list("EKF", nottypes)) {
|
2019-07-03 04:12:11 -03:00
|
|
|
_vehicle.ahrs.Log_Write();
|
2016-05-05 01:08:52 -03:00
|
|
|
}
|
|
|
|
if (!LogReader::in_list("AHRS2", nottypes)) {
|
2019-10-23 21:33:44 -03:00
|
|
|
_vehicle.logger.Write_AHRS2();
|
2016-05-05 01:08:52 -03:00
|
|
|
}
|
|
|
|
if (!LogReader::in_list("POS", nottypes)) {
|
2019-10-23 21:33:44 -03:00
|
|
|
_vehicle.logger.Write_POS();
|
2016-05-05 01:08:52 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2020-03-09 00:28:23 -03:00
|
|
|
void Replay::read_sensors(const char *type, uint8_t core)
|
2014-01-04 20:39:43 -04:00
|
|
|
{
|
2018-04-22 07:19:11 -03:00
|
|
|
if (streq(type, "PARM")) {
|
2014-03-01 00:15:46 -04:00
|
|
|
set_user_parameters();
|
|
|
|
}
|
2015-04-20 02:07:13 -03:00
|
|
|
|
2015-07-04 02:14:14 -03:00
|
|
|
if (!done_home_init) {
|
|
|
|
if (streq(type, "GPS") &&
|
|
|
|
(_vehicle.gps.status() >= AP_GPS::GPS_OK_FIX_3D) && done_baro_init) {
|
|
|
|
const Location &loc = _vehicle.gps.location();
|
|
|
|
::printf("GPS Lock at %.7f %.7f %.2fm time=%.1f seconds\n",
|
|
|
|
loc.lat * 1.0e-7f,
|
|
|
|
loc.lng * 1.0e-7f,
|
|
|
|
loc.alt * 0.01f,
|
2015-11-19 23:05:11 -04:00
|
|
|
AP_HAL::millis()*0.001f);
|
2019-02-13 01:18:27 -04:00
|
|
|
if (!_vehicle.ahrs.set_home(loc)) {
|
|
|
|
::printf("Failed to set home to that location!");
|
|
|
|
}
|
2015-07-04 02:14:14 -03:00
|
|
|
done_home_init = true;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2015-05-09 19:20:32 -03:00
|
|
|
if (streq(type,"GPS")) {
|
2015-06-20 09:30:00 -03:00
|
|
|
_vehicle.gps.update();
|
|
|
|
if (_vehicle.gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
|
|
|
|
_vehicle.ahrs.estimate_wind();
|
2014-02-17 18:11:46 -04:00
|
|
|
}
|
2015-05-09 19:20:32 -03:00
|
|
|
} else if (streq(type,"MAG")) {
|
2015-06-20 09:30:00 -03:00
|
|
|
_vehicle.compass.read();
|
2015-05-09 19:20:32 -03:00
|
|
|
} else if (streq(type,"ARSP")) {
|
2015-06-20 09:30:00 -03:00
|
|
|
_vehicle.ahrs.set_airspeed(&_vehicle.airspeed);
|
2015-05-09 19:20:32 -03:00
|
|
|
} else if (streq(type,"BARO")) {
|
2015-06-20 09:30:00 -03:00
|
|
|
_vehicle.barometer.update();
|
2015-04-20 02:07:13 -03:00
|
|
|
if (!done_baro_init) {
|
|
|
|
done_baro_init = true;
|
|
|
|
::printf("Barometer initialised\n");
|
2015-06-20 09:30:00 -03:00
|
|
|
_vehicle.barometer.update_calibration();
|
2015-04-20 02:07:13 -03:00
|
|
|
}
|
2015-05-27 08:20:38 -03:00
|
|
|
}
|
|
|
|
|
2018-04-22 07:19:11 -03:00
|
|
|
static bool ekf_force_started = false;
|
|
|
|
if (!ekf_force_started) {
|
|
|
|
if (log_info.have_imt2 ||
|
|
|
|
log_info.have_imt) {
|
|
|
|
_vehicle.ahrs.force_ekf_start();
|
2020-03-09 00:28:23 -03:00
|
|
|
::fprintf(stderr, "EKF force-started at %" PRIu64 "\n", AP_HAL::micros64());
|
2018-04-22 07:19:11 -03:00
|
|
|
ekf_force_started = true;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2015-05-27 08:20:38 -03:00
|
|
|
bool run_ahrs = false;
|
2016-05-03 19:26:02 -03:00
|
|
|
if (log_info.have_imt2) {
|
|
|
|
run_ahrs = streq(type, "IMT2");
|
|
|
|
} else if (log_info.have_imt) {
|
|
|
|
run_ahrs = streq(type, "IMT");
|
|
|
|
} else if (log_info.have_imu2) {
|
|
|
|
run_ahrs = streq(type, "IMU2");
|
|
|
|
} else {
|
|
|
|
run_ahrs = streq(type, "IMU");
|
2015-05-27 08:20:38 -03:00
|
|
|
}
|
2015-06-26 02:12:17 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
always run AHRS on CHECK messages when checking the solution
|
|
|
|
*/
|
|
|
|
if (check_solution) {
|
|
|
|
run_ahrs = streq(type, "CHEK");
|
|
|
|
}
|
|
|
|
|
2015-05-27 08:20:38 -03:00
|
|
|
if (run_ahrs) {
|
2015-06-20 09:30:00 -03:00
|
|
|
_vehicle.ahrs.update();
|
2016-05-05 01:08:52 -03:00
|
|
|
if ((downsample == 0 || ++output_counter % downsample == 0) && !logmatch) {
|
|
|
|
write_ekf_logs();
|
2015-07-07 02:13:51 -03:00
|
|
|
}
|
2015-06-20 09:30:00 -03:00
|
|
|
if (_vehicle.ahrs.healthy() != ahrs_healthy) {
|
|
|
|
ahrs_healthy = _vehicle.ahrs.healthy();
|
2015-05-19 02:21:12 -03:00
|
|
|
printf("AHRS health: %u at %lu\n",
|
|
|
|
(unsigned)ahrs_healthy,
|
2015-11-19 23:05:11 -04:00
|
|
|
(unsigned long)AP_HAL::millis());
|
2014-01-05 01:37:47 -04:00
|
|
|
}
|
2015-06-26 02:12:17 -03:00
|
|
|
if (check_generate) {
|
|
|
|
log_check_generate();
|
|
|
|
} else if (check_solution) {
|
|
|
|
log_check_solution();
|
2015-06-25 01:17:00 -03:00
|
|
|
}
|
2013-12-29 08:21:09 -04:00
|
|
|
}
|
2020-03-09 00:28:23 -03:00
|
|
|
|
|
|
|
// 255 here is a special marker for "no core present in log".
|
|
|
|
// This may give us a hope of backwards-compatability.
|
|
|
|
if (logmatch &&
|
|
|
|
(streq(type, "NKF1") || streq(type, "XKF1")) &&
|
|
|
|
(core == 0 || core == 255)) {
|
2016-05-05 01:08:52 -03:00
|
|
|
write_ekf_logs();
|
|
|
|
}
|
2013-12-29 07:56:30 -04:00
|
|
|
}
|
|
|
|
|
2015-06-25 01:17:00 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
copy current data to CHEK message
|
|
|
|
*/
|
2015-06-26 02:12:17 -03:00
|
|
|
void Replay::log_check_generate(void)
|
2015-06-25 01:17:00 -03:00
|
|
|
{
|
|
|
|
Vector3f euler;
|
|
|
|
Vector3f velocity;
|
|
|
|
Location loc {};
|
|
|
|
|
2019-12-10 07:01:38 -04:00
|
|
|
_vehicle.ahrs.EKF2.getEulerAngles(-1,euler);
|
|
|
|
_vehicle.ahrs.EKF2.getVelNED(-1,velocity);
|
|
|
|
_vehicle.ahrs.EKF2.getLLH(loc);
|
2015-06-25 01:17:00 -03:00
|
|
|
|
2019-03-27 18:44:23 -03:00
|
|
|
_vehicle.logger.Write(
|
2018-04-21 03:33:40 -03:00
|
|
|
"CHEK",
|
|
|
|
"TimeUS,Roll,Pitch,Yaw,Lat,Lng,Alt,VN,VE,VD",
|
|
|
|
"sdddDUmnnn",
|
|
|
|
"FBBBGGB000",
|
|
|
|
"QccCLLffff",
|
|
|
|
AP_HAL::micros64(),
|
|
|
|
(int16_t)(100*degrees(euler.x)), // roll angle (centi-deg, displayed as deg due to format string)
|
|
|
|
(int16_t)(100*degrees(euler.y)), // pitch angle (centi-deg, displayed as deg due to format string)
|
|
|
|
(uint16_t)wrap_360_cd(100*degrees(euler.z)), // yaw angle (centi-deg, displayed as deg due to format string)
|
|
|
|
loc.lat,
|
|
|
|
loc.lng,
|
|
|
|
loc.alt*0.01f,
|
|
|
|
velocity.x,
|
|
|
|
velocity.y,
|
|
|
|
velocity.z
|
2018-04-22 07:19:11 -03:00
|
|
|
);
|
2015-06-25 01:17:00 -03:00
|
|
|
}
|
|
|
|
|
2015-06-26 02:12:17 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
check current solution against CHEK message
|
|
|
|
*/
|
|
|
|
void Replay::log_check_solution(void)
|
|
|
|
{
|
|
|
|
const LR_MsgHandler::CheckState &check_state = logreader.get_check_state();
|
|
|
|
Vector3f euler;
|
|
|
|
Vector3f velocity;
|
|
|
|
Location loc {};
|
|
|
|
|
2019-12-10 07:01:38 -04:00
|
|
|
_vehicle.ahrs.EKF2.getEulerAngles(-1,euler);
|
|
|
|
_vehicle.ahrs.EKF2.getVelNED(-1,velocity);
|
|
|
|
_vehicle.ahrs.EKF2.getLLH(loc);
|
2015-06-26 02:12:17 -03:00
|
|
|
|
|
|
|
float roll_error = degrees(fabsf(euler.x - check_state.euler.x));
|
|
|
|
float pitch_error = degrees(fabsf(euler.y - check_state.euler.y));
|
2016-04-27 06:35:18 -03:00
|
|
|
float yaw_error = wrap_180_cd(100*degrees(fabsf(euler.z - check_state.euler.z)))*0.01f;
|
2015-06-26 02:12:17 -03:00
|
|
|
float vel_error = (velocity - check_state.velocity).length();
|
2019-02-24 20:12:26 -04:00
|
|
|
float pos_error = check_state.pos.get_distance(loc);
|
2015-06-26 02:12:17 -03:00
|
|
|
|
2015-11-27 13:11:58 -04:00
|
|
|
check_result.max_roll_error = MAX(check_result.max_roll_error, roll_error);
|
|
|
|
check_result.max_pitch_error = MAX(check_result.max_pitch_error, pitch_error);
|
|
|
|
check_result.max_yaw_error = MAX(check_result.max_yaw_error, yaw_error);
|
|
|
|
check_result.max_vel_error = MAX(check_result.max_vel_error, vel_error);
|
|
|
|
check_result.max_pos_error = MAX(check_result.max_pos_error, pos_error);
|
2015-06-26 02:12:17 -03:00
|
|
|
}
|
|
|
|
|
2016-11-02 09:29:58 -03:00
|
|
|
void Replay::flush_and_exit()
|
|
|
|
{
|
2019-03-27 18:44:23 -03:00
|
|
|
flush_logger();
|
2016-11-02 09:29:58 -03:00
|
|
|
|
|
|
|
if (check_solution) {
|
|
|
|
report_checks();
|
|
|
|
}
|
|
|
|
|
2015-06-29 23:48:49 -03:00
|
|
|
if (packet_counts) {
|
|
|
|
show_packet_counts();
|
|
|
|
}
|
|
|
|
|
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.
|
|
|
|
((Linux::Scheduler*)hal.scheduler)->teardown();
|
|
|
|
|
2016-11-02 09:29:58 -03:00
|
|
|
exit(0);
|
|
|
|
}
|
2015-06-26 02:12:17 -03:00
|
|
|
|
2015-06-29 23:48:49 -03:00
|
|
|
void Replay::show_packet_counts()
|
|
|
|
{
|
|
|
|
uint64_t counts[LOGREADER_MAX_FORMATS];
|
|
|
|
logreader.get_packet_counts(counts);
|
|
|
|
char format_type[5];
|
|
|
|
uint64_t total = 0;
|
|
|
|
for(uint16_t i=0;i<LOGREADER_MAX_FORMATS;i++) {
|
|
|
|
if (counts[i] != 0) {
|
|
|
|
logreader.format_type(i, format_type);
|
2019-06-11 23:39:18 -03:00
|
|
|
printf("%10" PRIu64 " %s\n", counts[i], format_type);
|
2015-06-29 23:48:49 -03:00
|
|
|
total += counts[i];
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2019-06-11 23:39:18 -03:00
|
|
|
printf("%" PRIu64 " total\n", total);
|
2015-06-29 23:48:49 -03:00
|
|
|
}
|
|
|
|
|
2015-06-01 03:14:11 -03:00
|
|
|
void Replay::loop()
|
2013-12-29 07:56:30 -04:00
|
|
|
{
|
2016-11-02 09:29:58 -03:00
|
|
|
char type[5];
|
2020-03-09 00:28:23 -03:00
|
|
|
uint8_t core;
|
2014-04-21 00:00:59 -03:00
|
|
|
|
2016-11-02 09:29:58 -03:00
|
|
|
if (arm_time_ms >= 0 && AP_HAL::millis() > (uint32_t)arm_time_ms) {
|
|
|
|
if (!hal.util->get_soft_armed()) {
|
|
|
|
hal.util->set_soft_armed(true);
|
|
|
|
::printf("Arming at %u ms\n", (unsigned)AP_HAL::millis());
|
2014-04-21 00:00:59 -03:00
|
|
|
}
|
2016-11-02 09:29:58 -03:00
|
|
|
}
|
2014-04-21 00:00:59 -03:00
|
|
|
|
2020-03-09 00:28:23 -03:00
|
|
|
if (!logreader.update(type, core)) {
|
2016-11-02 09:29:58 -03:00
|
|
|
::printf("End of log at %.1f seconds\n", AP_HAL::millis()*0.001f);
|
|
|
|
flush_and_exit();
|
|
|
|
}
|
2016-03-10 18:13:28 -04:00
|
|
|
|
2020-03-09 00:28:23 -03:00
|
|
|
const uint64_t now64 = AP_HAL::micros64();
|
2016-11-02 09:29:58 -03:00
|
|
|
if (last_timestamp != 0) {
|
2020-03-09 00:28:23 -03:00
|
|
|
if (now64 < last_timestamp) {
|
|
|
|
::printf("time going backwards?! now=%" PRIu64 " last_timestamp=%" PRIu64 "us\n",
|
|
|
|
now64, last_timestamp);
|
|
|
|
exit(1);
|
|
|
|
} else {
|
|
|
|
const uint64_t gap = now64 - last_timestamp;
|
|
|
|
if (gap > 40000) {
|
|
|
|
::printf("Gap in log at timestamp=%" PRIu64 " of length %" PRIu64 "us\n",
|
|
|
|
last_timestamp, gap);
|
|
|
|
}
|
2013-12-29 07:56:30 -04:00
|
|
|
}
|
|
|
|
}
|
2020-03-09 00:28:23 -03:00
|
|
|
last_timestamp = now64;
|
2015-06-26 02:12:17 -03:00
|
|
|
|
2018-04-22 07:19:11 -03:00
|
|
|
if (streq(type, "FMT")) {
|
|
|
|
if (!seen_non_fmt) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
seen_non_fmt = true;
|
2015-06-26 02:12:17 -03:00
|
|
|
}
|
2018-04-22 07:19:11 -03:00
|
|
|
|
2020-03-09 00:28:23 -03:00
|
|
|
read_sensors(type, core);
|
2015-06-26 02:12:17 -03:00
|
|
|
}
|
|
|
|
|
2015-07-04 02:14:14 -03:00
|
|
|
|
|
|
|
bool Replay::show_error(const char *text, float max_error, float tolerance)
|
|
|
|
{
|
|
|
|
bool failed = max_error > tolerance;
|
|
|
|
printf("%s:\t%.2f %c %.2f\n",
|
|
|
|
text,
|
|
|
|
max_error,
|
|
|
|
failed?'>':'<',
|
|
|
|
tolerance);
|
|
|
|
return failed;
|
|
|
|
}
|
|
|
|
|
2015-06-26 02:12:17 -03:00
|
|
|
/*
|
|
|
|
report results of --check
|
|
|
|
*/
|
|
|
|
void Replay::report_checks(void)
|
|
|
|
{
|
|
|
|
bool failed = false;
|
|
|
|
if (tolerance_euler < 0.01f) {
|
|
|
|
tolerance_euler = 0.01f;
|
|
|
|
}
|
2015-07-01 23:23:48 -03:00
|
|
|
FILE *f = fopen("replay_results.txt","a");
|
|
|
|
if (f != NULL) {
|
2015-07-02 01:27:10 -03:00
|
|
|
fprintf(f, "%s\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n",
|
2015-07-01 23:23:48 -03:00
|
|
|
log_filename,
|
|
|
|
check_result.max_roll_error,
|
|
|
|
check_result.max_pitch_error,
|
|
|
|
check_result.max_yaw_error,
|
|
|
|
check_result.max_pos_error,
|
|
|
|
check_result.max_vel_error);
|
|
|
|
fclose(f);
|
|
|
|
}
|
2015-07-04 02:14:14 -03:00
|
|
|
failed |= show_error("Roll error", check_result.max_roll_error, tolerance_euler);
|
|
|
|
failed |= show_error("Pitch error", check_result.max_pitch_error, tolerance_euler);
|
|
|
|
failed |= show_error("Yaw error", check_result.max_yaw_error, tolerance_euler);
|
|
|
|
failed |= show_error("Position error", check_result.max_pos_error, tolerance_pos);
|
|
|
|
failed |= show_error("Velocity error", check_result.max_vel_error, tolerance_vel);
|
2015-06-26 02:12:17 -03:00
|
|
|
if (failed) {
|
|
|
|
printf("Checks failed\n");
|
|
|
|
exit(1);
|
|
|
|
} else {
|
|
|
|
printf("Checks passed\n");
|
|
|
|
}
|
2013-12-29 07:56:30 -04:00
|
|
|
}
|
|
|
|
|
2016-05-25 07:46:18 -03:00
|
|
|
/*
|
|
|
|
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;
|
|
|
|
strncpy(u->name, pname, sizeof(u->name));
|
|
|
|
u->value = value;
|
|
|
|
u->next = user_parameters;
|
|
|
|
user_parameters = u;
|
|
|
|
}
|
|
|
|
fclose(f);
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
see if a user parameter is set
|
|
|
|
*/
|
|
|
|
bool Replay::check_user_param(const char *name)
|
|
|
|
{
|
|
|
|
for (struct user_parameter *u=user_parameters; u; u=u->next) {
|
|
|
|
if (strcmp(name, u->name) == 0) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2019-06-21 00:55:06 -03:00
|
|
|
const struct AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
|
2017-07-02 19:50:38 -03:00
|
|
|
AP_GROUPEND
|
2017-02-23 02:28:56 -04:00
|
|
|
};
|
2017-07-02 19:50:38 -03:00
|
|
|
GCS_Dummy _gcs;
|
2017-02-23 02:28:56 -04:00
|
|
|
|
2019-11-03 20:40:59 -04:00
|
|
|
#include <AP_ADSB/AP_ADSB.h>
|
|
|
|
#include <AP_Avoidance/AP_Avoidance.h>
|
|
|
|
|
2017-07-27 16:03:41 -03:00
|
|
|
// dummy methods to avoid linking with these libraries
|
2018-03-02 00:04:01 -04:00
|
|
|
bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate, const char *reason) { return false; }
|
2019-09-15 05:08:00 -03:00
|
|
|
AP_AdvancedFailsafe *AP::advancedfailsafe() { return nullptr; }
|
2017-08-05 08:35:56 -03:00
|
|
|
|
2019-11-03 20:40:59 -04:00
|
|
|
// dummy method to avoid linking AP_Avoidance
|
|
|
|
AP_Avoidance *AP::ap_avoidance() { return nullptr; }
|
|
|
|
|
2019-09-05 02:01:46 -03:00
|
|
|
// avoid building/linking LTM:
|
|
|
|
void AP_LTM_Telem::init() {};
|
2019-03-15 01:41:58 -03:00
|
|
|
// avoid building/linking Devo:
|
|
|
|
void AP_DEVO_Telem::init() {};
|
|
|
|
|
2015-10-19 16:59:30 -03:00
|
|
|
AP_HAL_MAIN_CALLBACKS(&replay);
|