2015-08-11 03:28:43 -03:00
|
|
|
#include <AP_HAL/AP_HAL.h>
|
2015-05-04 18:08:42 -03:00
|
|
|
|
2020-09-12 16:04:38 -03:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH)
|
2015-05-04 18:08:42 -03:00
|
|
|
|
2015-08-11 03:28:43 -03:00
|
|
|
#include "AP_HAL_SITL.h"
|
2015-05-04 18:08:42 -03:00
|
|
|
#include "AP_HAL_SITL_Namespace.h"
|
|
|
|
#include "HAL_SITL_Class.h"
|
|
|
|
#include "UARTDriver.h"
|
2015-08-11 03:28:43 -03:00
|
|
|
#include <AP_HAL/utility/getopt_cpp.h>
|
2021-06-10 23:30:38 -03:00
|
|
|
#include <AP_HAL_SITL/Storage.h>
|
2020-08-23 03:24:22 -03:00
|
|
|
#include <AP_Param/AP_Param.h>
|
2015-08-11 03:28:43 -03:00
|
|
|
|
|
|
|
#include <SITL/SIM_Multicopter.h>
|
|
|
|
#include <SITL/SIM_Helicopter.h>
|
2016-05-26 03:29:17 -03:00
|
|
|
#include <SITL/SIM_SingleCopter.h>
|
2015-12-31 21:27:07 -04:00
|
|
|
#include <SITL/SIM_Plane.h>
|
2015-12-31 23:16:22 -04:00
|
|
|
#include <SITL/SIM_QuadPlane.h>
|
2015-08-11 03:28:43 -03:00
|
|
|
#include <SITL/SIM_Rover.h>
|
2018-06-18 10:24:20 -03:00
|
|
|
#include <SITL/SIM_BalanceBot.h>
|
2018-09-24 04:17:02 -03:00
|
|
|
#include <SITL/SIM_Sailboat.h>
|
2019-09-22 21:50:27 -03:00
|
|
|
#include <SITL/SIM_MotorBoat.h>
|
2015-08-11 03:28:43 -03:00
|
|
|
#include <SITL/SIM_CRRCSim.h>
|
|
|
|
#include <SITL/SIM_Gazebo.h>
|
|
|
|
#include <SITL/SIM_last_letter.h>
|
|
|
|
#include <SITL/SIM_JSBSim.h>
|
|
|
|
#include <SITL/SIM_Tracker.h>
|
|
|
|
#include <SITL/SIM_Balloon.h>
|
2015-11-19 02:11:25 -04:00
|
|
|
#include <SITL/SIM_FlightAxis.h>
|
2016-04-26 19:44:56 -03:00
|
|
|
#include <SITL/SIM_Calibration.h>
|
2016-06-17 19:17:27 -03:00
|
|
|
#include <SITL/SIM_XPlane.h>
|
2017-02-03 00:18:26 -04:00
|
|
|
#include <SITL/SIM_Submarine.h>
|
2019-06-23 05:49:48 -03:00
|
|
|
#include <SITL/SIM_SilentWings.h>
|
2018-12-02 18:36:17 -04:00
|
|
|
#include <SITL/SIM_Morse.h>
|
2019-05-20 19:51:42 -03:00
|
|
|
#include <SITL/SIM_AirSim.h>
|
2019-07-18 14:39:00 -03:00
|
|
|
#include <SITL/SIM_Scrimmage.h>
|
2019-08-14 09:40:25 -03:00
|
|
|
#include <SITL/SIM_Webots.h>
|
2023-01-21 21:16:44 -04:00
|
|
|
#include <SITL/SIM_Webots_Python.h>
|
2020-05-19 22:01:08 -03:00
|
|
|
#include <SITL/SIM_JSON.h>
|
2021-03-18 00:12:54 -03:00
|
|
|
#include <SITL/SIM_Blimp.h>
|
2020-12-30 15:56:35 -04:00
|
|
|
#include <AP_Filesystem/AP_Filesystem.h>
|
2015-05-04 18:08:42 -03:00
|
|
|
|
2022-10-27 22:38:07 -03:00
|
|
|
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
|
|
|
|
2019-06-06 22:26:25 -03:00
|
|
|
#include <signal.h>
|
|
|
|
#include <stdio.h>
|
2020-09-04 19:23:51 -03:00
|
|
|
#include <time.h>
|
|
|
|
#include <sys/time.h>
|
2019-06-06 22:26:25 -03:00
|
|
|
|
2023-10-18 22:39:13 -03:00
|
|
|
#define FORCE_VERSION_H_INCLUDE
|
|
|
|
#include "ap_version.h"
|
|
|
|
|
2021-10-05 03:29:45 -03:00
|
|
|
extern HAL_SITL& hal;
|
2015-05-04 18:08:42 -03:00
|
|
|
|
|
|
|
using namespace HALSITL;
|
2015-10-22 10:04:42 -03:00
|
|
|
using namespace SITL;
|
2015-05-04 18:08:42 -03:00
|
|
|
|
|
|
|
// catch floating point exceptions
|
|
|
|
static void _sig_fpe(int signum)
|
|
|
|
{
|
2015-05-04 21:59:07 -03:00
|
|
|
fprintf(stderr, "ERROR: Floating point exception - aborting\n");
|
2019-06-10 23:15:27 -03:00
|
|
|
AP_HAL::dump_stack_trace();
|
2021-07-06 02:32:25 -03:00
|
|
|
AP_HAL::dump_core_file();
|
2019-06-06 22:26:25 -03:00
|
|
|
abort();
|
|
|
|
}
|
|
|
|
|
|
|
|
// catch segfault
|
|
|
|
static void _sig_segv(int signum)
|
|
|
|
{
|
|
|
|
fprintf(stderr, "ERROR: segmentation fault - aborting\n");
|
2019-06-10 23:15:27 -03:00
|
|
|
AP_HAL::dump_stack_trace();
|
2021-07-06 02:32:25 -03:00
|
|
|
AP_HAL::dump_core_file();
|
2015-05-04 18:08:42 -03:00
|
|
|
abort();
|
|
|
|
}
|
|
|
|
|
|
|
|
void SITL_State::_usage(void)
|
|
|
|
{
|
2015-05-10 19:48:42 -03:00
|
|
|
printf("Options:\n"
|
2017-02-15 20:00:55 -04:00
|
|
|
"\t--help|-h display this help information\n"
|
2019-02-11 04:35:28 -04:00
|
|
|
"\t--wipe|-w wipe eeprom\n"
|
2017-02-15 20:00:55 -04:00
|
|
|
"\t--unhide-groups|-u parameter enumeration ignores AP_PARAM_FLAG_ENABLE\n"
|
|
|
|
"\t--speedup|-s SPEEDUP set simulation speedup\n"
|
|
|
|
"\t--rate|-r RATE set SITL framerate\n"
|
|
|
|
"\t--console|-C use console instead of TCP ports\n"
|
2018-05-09 20:56:38 -03:00
|
|
|
"\t--instance|-I N set instance of SITL (adds 10*instance to all port numbers)\n"
|
2017-02-15 20:00:55 -04:00
|
|
|
// "\t--param|-P NAME=VALUE set some param\n" CURRENTLY BROKEN!
|
|
|
|
"\t--synthetic-clock|-S set synthetic clock mode\n"
|
2020-12-30 15:56:35 -04:00
|
|
|
"\t--home|-O HOME set start location (lat,lng,alt,yaw) or location name\n"
|
2017-02-15 20:00:55 -04:00
|
|
|
"\t--model|-M MODEL set simulation model\n"
|
2019-07-18 14:39:00 -03:00
|
|
|
"\t--config string set additional simulation config string\n"
|
2018-06-16 22:39:41 -03:00
|
|
|
"\t--fg|-F ADDRESS set Flight Gear view address, defaults to 127.0.0.1\n"
|
2017-01-30 06:52:58 -04:00
|
|
|
"\t--disable-fgview disable Flight Gear view\n"
|
2018-06-16 22:39:41 -03:00
|
|
|
"\t--gimbal enable simulated MAVLink gimbal\n"
|
2016-09-21 17:46:12 -03:00
|
|
|
"\t--autotest-dir DIR set directory for additional files\n"
|
2017-01-30 06:52:58 -04:00
|
|
|
"\t--defaults path set path to defaults file\n"
|
2023-12-10 23:55:21 -04:00
|
|
|
"\t--uartA device set device string for SERIAL0\n"
|
|
|
|
"\t--uartB device set device string for SERIAL3\n" // ordering captures the historical use of uartB as SERIAL3
|
|
|
|
"\t--uartC device set device string for SERIAL1\n"
|
|
|
|
"\t--uartD device set device string for SERIAL2\n"
|
|
|
|
"\t--uartE device set device string for SERIAL4\n"
|
|
|
|
"\t--uartF device set device string for SERIAL5\n"
|
|
|
|
"\t--uartG device set device string for SERIAL6\n"
|
|
|
|
"\t--uartH device set device string for SERIAL7\n"
|
|
|
|
"\t--uartI device set device string for SERIAL8\n"
|
|
|
|
"\t--uartJ device set device string for SERIAL9\n"
|
2021-04-25 20:36:49 -03:00
|
|
|
"\t--serial0 device set device string for SERIAL0\n"
|
|
|
|
"\t--serial1 device set device string for SERIAL1\n"
|
|
|
|
"\t--serial2 device set device string for SERIAL2\n"
|
|
|
|
"\t--serial3 device set device string for SERIAL3\n"
|
|
|
|
"\t--serial4 device set device string for SERIAL4\n"
|
|
|
|
"\t--serial5 device set device string for SERIAL5\n"
|
|
|
|
"\t--serial6 device set device string for SERIAL6\n"
|
|
|
|
"\t--serial7 device set device string for SERIAL7\n"
|
|
|
|
"\t--serial8 device set device string for SERIAL8\n"
|
2021-11-05 00:17:40 -03:00
|
|
|
"\t--serial9 device set device string for SERIAL9\n"
|
2017-01-30 06:52:58 -04:00
|
|
|
"\t--rtscts enable rtscts on serial ports (default false)\n"
|
|
|
|
"\t--base-port PORT set port num for base port(default 5670) must be before -I option\n"
|
|
|
|
"\t--rc-in-port PORT set port num for rc in\n"
|
2016-12-16 12:18:04 -04:00
|
|
|
"\t--sim-address ADDR set address string for simulator\n"
|
|
|
|
"\t--sim-port-in PORT set port num for simulator in\n"
|
|
|
|
"\t--sim-port-out PORT set port num for simulator out\n"
|
2016-10-24 07:29:49 -03:00
|
|
|
"\t--irlock-port PORT set port num for irlock\n"
|
2020-12-10 19:26:42 -04:00
|
|
|
"\t--start-time TIMESTR set simulation start time in UNIX timestamp\n"
|
2018-09-04 14:06:25 -03:00
|
|
|
"\t--sysid ID set SYSID_THISMAV\n"
|
2021-04-24 21:31:35 -03:00
|
|
|
"\t--slave number set the number of JSON slaves\n"
|
2015-05-10 19:48:42 -03:00
|
|
|
);
|
2015-05-04 18:08:42 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
static const struct {
|
|
|
|
const char *name;
|
2019-08-15 01:00:04 -03:00
|
|
|
Aircraft *(*constructor)(const char *frame_str);
|
2015-05-04 18:08:42 -03:00
|
|
|
} model_constructors[] = {
|
2015-12-31 23:16:22 -04:00
|
|
|
{ "quadplane", QuadPlane::create },
|
2016-06-17 19:17:27 -03:00
|
|
|
{ "xplane", XPlane::create },
|
2016-04-21 21:26:52 -03:00
|
|
|
{ "firefly", QuadPlane::create },
|
2015-06-01 20:08:55 -03:00
|
|
|
{ "+", MultiCopter::create },
|
|
|
|
{ "quad", MultiCopter::create },
|
|
|
|
{ "copter", MultiCopter::create },
|
|
|
|
{ "x", MultiCopter::create },
|
2020-01-14 02:25:22 -04:00
|
|
|
{ "bfxrev", MultiCopter::create },
|
2019-01-31 23:31:01 -04:00
|
|
|
{ "bfx", MultiCopter::create },
|
|
|
|
{ "djix", MultiCopter::create },
|
|
|
|
{ "cwx", MultiCopter::create },
|
2015-06-01 20:08:55 -03:00
|
|
|
{ "hexa", MultiCopter::create },
|
2020-02-01 05:16:12 -04:00
|
|
|
{ "hexa-cwx", MultiCopter::create },
|
2020-01-28 08:03:49 -04:00
|
|
|
{ "hexa-dji", MultiCopter::create },
|
2015-06-01 20:08:55 -03:00
|
|
|
{ "octa", MultiCopter::create },
|
2020-02-01 05:16:12 -04:00
|
|
|
{ "octa-cwx", MultiCopter::create },
|
2020-01-28 08:03:49 -04:00
|
|
|
{ "octa-dji", MultiCopter::create },
|
2020-02-01 05:16:12 -04:00
|
|
|
{ "octa-quad-cwx", MultiCopter::create },
|
2017-05-27 01:47:55 -03:00
|
|
|
{ "dodeca-hexa", MultiCopter::create },
|
2016-04-21 07:29:58 -03:00
|
|
|
{ "tri", MultiCopter::create },
|
2016-04-21 08:11:46 -03:00
|
|
|
{ "y6", MultiCopter::create },
|
2020-09-16 11:59:23 -03:00
|
|
|
{ "deca", MultiCopter::create },
|
|
|
|
{ "deca-cwx", MultiCopter::create },
|
2015-06-01 20:08:55 -03:00
|
|
|
{ "heli", Helicopter::create },
|
|
|
|
{ "heli-dual", Helicopter::create },
|
|
|
|
{ "heli-compound", Helicopter::create },
|
2021-09-07 00:26:23 -03:00
|
|
|
{ "heli-blade360", Helicopter::create },
|
2016-05-26 03:29:17 -03:00
|
|
|
{ "singlecopter", SingleCopter::create },
|
2016-05-26 04:09:22 -03:00
|
|
|
{ "coaxcopter", SingleCopter::create },
|
2015-10-21 08:04:05 -03:00
|
|
|
{ "rover", SimRover::create },
|
2018-06-18 10:24:20 -03:00
|
|
|
{ "balancebot", BalanceBot::create },
|
2018-09-24 04:17:02 -03:00
|
|
|
{ "sailboat", Sailboat::create },
|
2019-09-22 21:50:27 -03:00
|
|
|
{ "motorboat", MotorBoat::create },
|
2015-06-01 20:08:55 -03:00
|
|
|
{ "crrcsim", CRRCSim::create },
|
|
|
|
{ "jsbsim", JSBSim::create },
|
2015-11-19 02:11:25 -04:00
|
|
|
{ "flightaxis", FlightAxis::create },
|
2015-06-01 20:08:55 -03:00
|
|
|
{ "gazebo", Gazebo::create },
|
|
|
|
{ "last_letter", last_letter::create },
|
|
|
|
{ "tracker", Tracker::create },
|
2015-12-31 21:27:07 -04:00
|
|
|
{ "balloon", Balloon::create },
|
|
|
|
{ "plane", Plane::create },
|
2016-04-26 19:44:56 -03:00
|
|
|
{ "calibration", Calibration::create },
|
2017-02-03 00:18:26 -04:00
|
|
|
{ "vectored", Submarine::create },
|
2019-08-05 13:44:46 -03:00
|
|
|
{ "vectored_6dof", Submarine::create },
|
2019-06-23 05:49:48 -03:00
|
|
|
{ "silentwings", SilentWings::create },
|
2018-12-02 18:36:17 -04:00
|
|
|
{ "morse", Morse::create },
|
2019-05-20 19:51:42 -03:00
|
|
|
{ "airsim", AirSim::create},
|
2019-07-18 14:39:00 -03:00
|
|
|
{ "scrimmage", Scrimmage::create },
|
2023-01-21 21:16:44 -04:00
|
|
|
{ "webots-python", WebotsPython::create },
|
2019-08-14 09:40:25 -03:00
|
|
|
{ "webots", Webots::create },
|
2020-05-19 22:01:08 -03:00
|
|
|
{ "JSON", JSON::create },
|
2021-03-18 00:12:54 -03:00
|
|
|
{ "blimp", Blimp::create },
|
2015-05-04 18:08:42 -03:00
|
|
|
};
|
|
|
|
|
2016-07-13 10:27:56 -03:00
|
|
|
void SITL_State::_set_signal_handlers(void) const
|
|
|
|
{
|
2016-07-29 08:19:00 -03:00
|
|
|
struct sigaction sa_fpe = {};
|
2016-07-13 10:27:56 -03:00
|
|
|
sigemptyset(&sa_fpe.sa_mask);
|
|
|
|
sa_fpe.sa_handler = _sig_fpe;
|
|
|
|
sigaction(SIGFPE, &sa_fpe, nullptr);
|
|
|
|
|
2016-07-29 08:19:00 -03:00
|
|
|
struct sigaction sa_pipe = {};
|
2016-07-13 10:27:56 -03:00
|
|
|
sigemptyset(&sa_pipe.sa_mask);
|
|
|
|
sa_pipe.sa_handler = SIG_IGN; /* No-op SIGPIPE handler */
|
|
|
|
sigaction(SIGPIPE, &sa_pipe, nullptr);
|
2019-06-06 22:26:25 -03:00
|
|
|
|
|
|
|
struct sigaction sa_segv = {};
|
|
|
|
sigemptyset(&sa_segv.sa_mask);
|
|
|
|
sa_segv.sa_handler = _sig_segv;
|
|
|
|
sigaction(SIGSEGV, &sa_segv, nullptr);
|
|
|
|
|
2016-07-13 10:27:56 -03:00
|
|
|
}
|
|
|
|
|
2015-05-04 18:08:42 -03:00
|
|
|
void SITL_State::_parse_command_line(int argc, char * const argv[])
|
|
|
|
{
|
2015-05-04 21:59:07 -03:00
|
|
|
int opt;
|
2017-01-30 06:52:58 -04:00
|
|
|
float speedup = 1.0f;
|
2023-07-14 02:48:23 -03:00
|
|
|
float sim_rate_hz = 0;
|
2017-01-30 06:52:58 -04:00
|
|
|
_instance = 0;
|
|
|
|
_synthetic_clock_mode = false;
|
2015-11-22 22:21:33 -04:00
|
|
|
// default to CMAC
|
2019-08-15 03:52:50 -03:00
|
|
|
const char *home_str = nullptr;
|
2016-10-30 02:24:21 -03:00
|
|
|
const char *model_str = nullptr;
|
2022-10-03 13:52:29 -03:00
|
|
|
const char *vehicle_str = SKETCH;
|
2017-01-30 06:52:58 -04:00
|
|
|
_use_fg_view = true;
|
2016-10-30 02:24:21 -03:00
|
|
|
char *autotest_dir = nullptr;
|
2018-06-16 22:39:41 -03:00
|
|
|
_fg_address = "127.0.0.1";
|
2019-07-18 14:39:00 -03:00
|
|
|
const char* config = "";
|
2015-05-04 18:08:42 -03:00
|
|
|
|
2016-12-08 10:46:21 -04:00
|
|
|
const int BASE_PORT = 5760;
|
|
|
|
const int RCIN_PORT = 5501;
|
|
|
|
const int FG_VIEW_PORT = 5503;
|
|
|
|
_base_port = BASE_PORT;
|
|
|
|
_rcin_port = RCIN_PORT;
|
|
|
|
_fg_view_port = FG_VIEW_PORT;
|
2017-01-30 06:52:58 -04:00
|
|
|
|
|
|
|
const int SIM_IN_PORT = 9003;
|
|
|
|
const int SIM_OUT_PORT = 9002;
|
|
|
|
const int IRLOCK_PORT = 9005;
|
2018-06-16 22:44:33 -03:00
|
|
|
const char * simulator_address = "127.0.0.1";
|
|
|
|
uint16_t simulator_port_in = SIM_IN_PORT;
|
|
|
|
uint16_t simulator_port_out = SIM_OUT_PORT;
|
2016-12-08 10:46:21 -04:00
|
|
|
_irlock_port = IRLOCK_PORT;
|
2020-08-23 03:24:22 -03:00
|
|
|
struct AP_Param::defaults_table_struct temp_cmdline_param{};
|
2015-05-10 02:36:18 -03:00
|
|
|
|
2020-09-04 19:23:51 -03:00
|
|
|
// Set default start time to the real system time.
|
|
|
|
// This will be overwritten if argument provided.
|
|
|
|
static struct timeval first_tv;
|
|
|
|
gettimeofday(&first_tv, nullptr);
|
|
|
|
time_t start_time_UTC = first_tv.tv_sec;
|
2020-11-15 23:30:33 -04:00
|
|
|
const bool is_replay = APM_BUILD_TYPE(APM_BUILD_Replay);
|
2020-09-04 19:23:51 -03:00
|
|
|
|
2015-05-10 02:36:18 -03:00
|
|
|
enum long_options {
|
2018-06-16 22:38:21 -03:00
|
|
|
CMDLINE_GIMBAL = 1,
|
2017-01-30 06:52:58 -04:00
|
|
|
CMDLINE_FGVIEW,
|
2015-11-03 16:59:43 -04:00
|
|
|
CMDLINE_AUTOTESTDIR,
|
2017-01-30 06:52:58 -04:00
|
|
|
CMDLINE_DEFAULTS,
|
2023-12-10 23:55:21 -04:00
|
|
|
CMDLINE_SERIAL0, // must be in 0-9 order and numbered consecutively
|
2021-04-25 20:36:49 -03:00
|
|
|
CMDLINE_SERIAL1,
|
|
|
|
CMDLINE_SERIAL2,
|
|
|
|
CMDLINE_SERIAL3,
|
|
|
|
CMDLINE_SERIAL4,
|
|
|
|
CMDLINE_SERIAL5,
|
|
|
|
CMDLINE_SERIAL6,
|
|
|
|
CMDLINE_SERIAL7,
|
|
|
|
CMDLINE_SERIAL8,
|
2021-11-05 00:17:40 -03:00
|
|
|
CMDLINE_SERIAL9,
|
2016-05-16 03:51:09 -03:00
|
|
|
CMDLINE_RTSCTS,
|
2017-01-30 06:52:58 -04:00
|
|
|
CMDLINE_BASE_PORT,
|
|
|
|
CMDLINE_RCIN_PORT,
|
2019-07-12 01:58:43 -03:00
|
|
|
CMDLINE_SIM_ADDRESS,
|
2016-12-16 12:18:04 -04:00
|
|
|
CMDLINE_SIM_PORT_IN,
|
|
|
|
CMDLINE_SIM_PORT_OUT,
|
2016-10-24 07:29:49 -03:00
|
|
|
CMDLINE_IRLOCK_PORT,
|
2020-09-04 19:23:51 -03:00
|
|
|
CMDLINE_START_TIME,
|
2018-09-04 14:06:25 -03:00
|
|
|
CMDLINE_SYSID,
|
2021-04-24 21:31:35 -03:00
|
|
|
CMDLINE_SLAVE,
|
2021-06-10 23:30:38 -03:00
|
|
|
#if STORAGE_USE_FLASH
|
|
|
|
CMDLINE_SET_STORAGE_FLASH_ENABLED,
|
|
|
|
#endif
|
|
|
|
#if STORAGE_USE_POSIX
|
|
|
|
CMDLINE_SET_STORAGE_POSIX_ENABLED,
|
2021-07-12 22:41:18 -03:00
|
|
|
#endif
|
|
|
|
#if STORAGE_USE_FRAM
|
|
|
|
CMDLINE_SET_STORAGE_FRAM_ENABLED,
|
2021-06-10 23:30:38 -03:00
|
|
|
#endif
|
2015-05-10 02:36:18 -03:00
|
|
|
};
|
2015-05-04 18:08:42 -03:00
|
|
|
|
2015-05-04 21:59:07 -03:00
|
|
|
const struct GetOptLong::option options[] = {
|
2015-05-04 18:08:42 -03:00
|
|
|
{"help", false, 0, 'h'},
|
|
|
|
{"wipe", false, 0, 'w'},
|
2016-10-25 18:31:50 -03:00
|
|
|
{"unhide-groups", false, 0, 'u'},
|
2015-05-04 18:08:42 -03:00
|
|
|
{"speedup", true, 0, 's'},
|
|
|
|
{"rate", true, 0, 'r'},
|
|
|
|
{"console", false, 0, 'C'},
|
|
|
|
{"instance", true, 0, 'I'},
|
|
|
|
{"param", true, 0, 'P'},
|
|
|
|
{"synthetic-clock", false, 0, 'S'},
|
|
|
|
{"home", true, 0, 'O'},
|
|
|
|
{"model", true, 0, 'M'},
|
2019-07-18 14:39:00 -03:00
|
|
|
{"config", true, 0, 'c'},
|
2018-06-16 22:39:41 -03:00
|
|
|
{"fg", true, 0, 'F'},
|
2017-01-30 06:52:58 -04:00
|
|
|
{"gimbal", false, 0, CMDLINE_GIMBAL},
|
|
|
|
{"disable-fgview", false, 0, CMDLINE_FGVIEW},
|
|
|
|
{"autotest-dir", true, 0, CMDLINE_AUTOTESTDIR},
|
|
|
|
{"defaults", true, 0, CMDLINE_DEFAULTS},
|
2023-12-10 23:55:21 -04:00
|
|
|
{"uartA", true, 0, CMDLINE_SERIAL0},
|
|
|
|
{"uartB", true, 0, CMDLINE_SERIAL3}, // ordering captures the historical use of uartB as SERIAL3
|
|
|
|
{"uartC", true, 0, CMDLINE_SERIAL1},
|
|
|
|
{"uartD", true, 0, CMDLINE_SERIAL2},
|
|
|
|
{"uartE", true, 0, CMDLINE_SERIAL4},
|
|
|
|
{"uartF", true, 0, CMDLINE_SERIAL5},
|
|
|
|
{"uartG", true, 0, CMDLINE_SERIAL6},
|
|
|
|
{"uartH", true, 0, CMDLINE_SERIAL7},
|
|
|
|
{"uartI", true, 0, CMDLINE_SERIAL8},
|
|
|
|
{"uartJ", true, 0, CMDLINE_SERIAL9},
|
2021-04-25 20:36:49 -03:00
|
|
|
{"serial0", true, 0, CMDLINE_SERIAL0},
|
|
|
|
{"serial1", true, 0, CMDLINE_SERIAL1},
|
|
|
|
{"serial2", true, 0, CMDLINE_SERIAL2},
|
|
|
|
{"serial3", true, 0, CMDLINE_SERIAL3},
|
|
|
|
{"serial4", true, 0, CMDLINE_SERIAL4},
|
|
|
|
{"serial5", true, 0, CMDLINE_SERIAL5},
|
|
|
|
{"serial6", true, 0, CMDLINE_SERIAL6},
|
|
|
|
{"serial7", true, 0, CMDLINE_SERIAL7},
|
|
|
|
{"serial8", true, 0, CMDLINE_SERIAL8},
|
2021-11-05 00:17:40 -03:00
|
|
|
{"serial9", true, 0, CMDLINE_SERIAL9},
|
2016-05-16 03:51:09 -03:00
|
|
|
{"rtscts", false, 0, CMDLINE_RTSCTS},
|
2017-01-30 06:52:58 -04:00
|
|
|
{"base-port", true, 0, CMDLINE_BASE_PORT},
|
|
|
|
{"rc-in-port", true, 0, CMDLINE_RCIN_PORT},
|
2016-12-16 12:18:04 -04:00
|
|
|
{"sim-address", true, 0, CMDLINE_SIM_ADDRESS},
|
|
|
|
{"sim-port-in", true, 0, CMDLINE_SIM_PORT_IN},
|
|
|
|
{"sim-port-out", true, 0, CMDLINE_SIM_PORT_OUT},
|
2016-12-08 10:46:21 -04:00
|
|
|
{"irlock-port", true, 0, CMDLINE_IRLOCK_PORT},
|
2020-09-04 19:23:51 -03:00
|
|
|
{"start-time", true, 0, CMDLINE_START_TIME},
|
2018-09-04 14:06:25 -03:00
|
|
|
{"sysid", true, 0, CMDLINE_SYSID},
|
2021-04-24 21:31:35 -03:00
|
|
|
{"slave", true, 0, CMDLINE_SLAVE},
|
2021-06-10 23:30:38 -03:00
|
|
|
#if STORAGE_USE_FLASH
|
|
|
|
{"set-storage-flash-enabled", true, 0, CMDLINE_SET_STORAGE_FLASH_ENABLED},
|
|
|
|
#endif
|
|
|
|
#if STORAGE_USE_POSIX
|
|
|
|
{"set-storage-posix-enabled", true, 0, CMDLINE_SET_STORAGE_POSIX_ENABLED},
|
2021-07-12 22:41:18 -03:00
|
|
|
#endif
|
|
|
|
#if STORAGE_USE_FRAM
|
|
|
|
{"set-storage-fram-enabled", true, 0, CMDLINE_SET_STORAGE_FRAM_ENABLED},
|
2021-06-10 23:30:38 -03:00
|
|
|
#endif
|
2022-10-03 13:52:29 -03:00
|
|
|
{"vehicle", true, 0, 'v'},
|
2015-05-04 21:59:07 -03:00
|
|
|
{0, false, 0, 0}
|
|
|
|
};
|
2015-05-04 18:08:42 -03:00
|
|
|
|
2020-11-15 23:30:33 -04:00
|
|
|
if (is_replay) {
|
|
|
|
model_str = "quad";
|
|
|
|
HALSITL::UARTDriver::_console = true;
|
|
|
|
}
|
2020-11-08 16:46:03 -04:00
|
|
|
|
2021-06-10 23:30:38 -03:00
|
|
|
// storage defaults are set here:
|
|
|
|
bool storage_posix_enabled = true;
|
|
|
|
bool storage_flash_enabled = false;
|
2021-07-12 22:41:18 -03:00
|
|
|
bool storage_fram_enabled = false;
|
2021-06-10 23:30:38 -03:00
|
|
|
bool erase_all_storage = false;
|
|
|
|
|
2023-10-18 22:39:13 -03:00
|
|
|
if (asprintf(&autotest_dir, AP_BUILD_ROOT "/Tools/autotest") <= 0) {
|
2017-01-30 06:52:58 -04:00
|
|
|
AP_HAL::panic("out of memory");
|
|
|
|
}
|
|
|
|
_set_signal_handlers();
|
|
|
|
|
|
|
|
setvbuf(stdout, (char *)0, _IONBF, 0);
|
|
|
|
setvbuf(stderr, (char *)0, _IONBF, 0);
|
|
|
|
|
2021-07-12 22:41:18 -03:00
|
|
|
bool wiping_storage = false;
|
|
|
|
|
2022-10-03 13:52:29 -03:00
|
|
|
GetOptLong gopt(argc, argv, "hwus:r:CI:P:SO:M:F:c:v:",
|
2015-05-04 18:08:42 -03:00
|
|
|
options);
|
2020-11-15 23:30:33 -04:00
|
|
|
while (!is_replay && (opt = gopt.getoption()) != -1) {
|
2015-05-04 21:59:07 -03:00
|
|
|
switch (opt) {
|
|
|
|
case 'w':
|
2021-06-10 23:30:38 -03:00
|
|
|
erase_all_storage = true;
|
2015-05-04 21:59:07 -03:00
|
|
|
break;
|
2016-10-25 18:31:50 -03:00
|
|
|
case 'u':
|
|
|
|
AP_Param::set_hide_disabled_groups(false);
|
|
|
|
break;
|
2017-01-30 06:52:58 -04:00
|
|
|
case 's':
|
|
|
|
speedup = strtof(gopt.optarg, nullptr);
|
2020-08-23 03:24:22 -03:00
|
|
|
temp_cmdline_param = {"SIM_SPEEDUP", speedup};
|
|
|
|
cmdline_param.push_back(temp_cmdline_param);
|
|
|
|
printf("Setting SIM_SPEEDUP=%f\n", speedup);
|
2017-01-30 06:52:58 -04:00
|
|
|
break;
|
2023-07-14 02:48:23 -03:00
|
|
|
case 'r':
|
|
|
|
sim_rate_hz = strtof(gopt.optarg, nullptr);
|
|
|
|
temp_cmdline_param = {"SIM_RATE_HZ", sim_rate_hz};
|
|
|
|
cmdline_param.push_back(temp_cmdline_param);
|
|
|
|
printf("Setting SIM_RATE_HZ=%f\n", sim_rate_hz);
|
|
|
|
break;
|
2015-05-04 21:59:07 -03:00
|
|
|
case 'C':
|
2016-01-10 02:23:32 -04:00
|
|
|
HALSITL::UARTDriver::_console = true;
|
2015-05-04 21:59:07 -03:00
|
|
|
break;
|
|
|
|
case 'I': {
|
2018-05-09 20:56:38 -03:00
|
|
|
_instance = atoi(gopt.optarg);
|
2016-12-08 10:46:21 -04:00
|
|
|
if (_base_port == BASE_PORT) {
|
|
|
|
_base_port += _instance * 10;
|
|
|
|
}
|
|
|
|
if (_rcin_port == RCIN_PORT) {
|
|
|
|
_rcin_port += _instance * 10;
|
|
|
|
}
|
|
|
|
if (_fg_view_port == FG_VIEW_PORT) {
|
|
|
|
_fg_view_port += _instance * 10;
|
|
|
|
}
|
2018-06-16 22:44:33 -03:00
|
|
|
if (simulator_port_in == SIM_IN_PORT) {
|
|
|
|
simulator_port_in += _instance * 10;
|
2016-12-08 10:46:21 -04:00
|
|
|
}
|
2018-06-16 22:44:33 -03:00
|
|
|
if (simulator_port_out == SIM_OUT_PORT) {
|
|
|
|
simulator_port_out += _instance * 10;
|
2016-12-08 10:46:21 -04:00
|
|
|
}
|
|
|
|
if (_irlock_port == IRLOCK_PORT) {
|
|
|
|
_irlock_port += _instance * 10;
|
|
|
|
}
|
2015-05-04 18:08:42 -03:00
|
|
|
}
|
2015-05-04 21:59:07 -03:00
|
|
|
break;
|
|
|
|
case 'P':
|
2015-05-04 18:08:42 -03:00
|
|
|
_set_param_default(gopt.optarg);
|
2015-05-04 21:59:07 -03:00
|
|
|
break;
|
|
|
|
case 'S':
|
2015-05-04 18:08:42 -03:00
|
|
|
_synthetic_clock_mode = true;
|
2015-05-04 21:59:07 -03:00
|
|
|
break;
|
2015-05-04 18:08:42 -03:00
|
|
|
case 'O':
|
|
|
|
home_str = gopt.optarg;
|
|
|
|
break;
|
|
|
|
case 'M':
|
|
|
|
model_str = gopt.optarg;
|
|
|
|
break;
|
2019-07-18 14:39:00 -03:00
|
|
|
case 'c':
|
|
|
|
config = gopt.optarg;
|
|
|
|
break;
|
2015-05-04 18:08:42 -03:00
|
|
|
case 'F':
|
2018-06-16 22:39:41 -03:00
|
|
|
_fg_address = gopt.optarg;
|
2015-05-04 18:08:42 -03:00
|
|
|
break;
|
2022-10-03 13:52:29 -03:00
|
|
|
case 'v':
|
|
|
|
vehicle_str = gopt.optarg;
|
|
|
|
break;
|
2015-05-24 23:11:16 -03:00
|
|
|
case CMDLINE_GIMBAL:
|
|
|
|
enable_gimbal = true;
|
|
|
|
break;
|
2017-01-30 06:52:58 -04:00
|
|
|
case CMDLINE_FGVIEW:
|
|
|
|
_use_fg_view = false;
|
2016-05-16 03:51:09 -03:00
|
|
|
break;
|
2015-05-25 00:04:17 -03:00
|
|
|
case CMDLINE_AUTOTESTDIR:
|
|
|
|
autotest_dir = strdup(gopt.optarg);
|
|
|
|
break;
|
2016-01-06 18:09:40 -04:00
|
|
|
case CMDLINE_DEFAULTS:
|
|
|
|
defaults_path = strdup(gopt.optarg);
|
|
|
|
break;
|
2021-04-25 20:36:49 -03:00
|
|
|
case CMDLINE_SERIAL0:
|
|
|
|
case CMDLINE_SERIAL1:
|
|
|
|
case CMDLINE_SERIAL2:
|
|
|
|
case CMDLINE_SERIAL3:
|
|
|
|
case CMDLINE_SERIAL4:
|
|
|
|
case CMDLINE_SERIAL5:
|
|
|
|
case CMDLINE_SERIAL6:
|
|
|
|
case CMDLINE_SERIAL7:
|
2021-04-26 17:47:21 -03:00
|
|
|
case CMDLINE_SERIAL8:
|
2023-12-10 23:55:21 -04:00
|
|
|
case CMDLINE_SERIAL9:
|
|
|
|
_serial_path[opt - CMDLINE_SERIAL0] = gopt.optarg;
|
2021-04-25 20:36:49 -03:00
|
|
|
break;
|
2017-01-30 06:52:58 -04:00
|
|
|
case CMDLINE_RTSCTS:
|
|
|
|
_use_rtscts = true;
|
|
|
|
break;
|
|
|
|
case CMDLINE_BASE_PORT:
|
|
|
|
_base_port = atoi(gopt.optarg);
|
|
|
|
break;
|
|
|
|
case CMDLINE_RCIN_PORT:
|
|
|
|
_rcin_port = atoi(gopt.optarg);
|
|
|
|
break;
|
2016-12-16 12:18:04 -04:00
|
|
|
case CMDLINE_SIM_ADDRESS:
|
2018-06-16 22:44:33 -03:00
|
|
|
simulator_address = gopt.optarg;
|
2016-09-21 17:46:12 -03:00
|
|
|
break;
|
2016-12-16 12:18:04 -04:00
|
|
|
case CMDLINE_SIM_PORT_IN:
|
2018-06-16 22:44:33 -03:00
|
|
|
simulator_port_in = atoi(gopt.optarg);
|
2016-09-21 17:46:12 -03:00
|
|
|
break;
|
2016-12-16 12:18:04 -04:00
|
|
|
case CMDLINE_SIM_PORT_OUT:
|
2018-06-16 22:44:33 -03:00
|
|
|
simulator_port_out = atoi(gopt.optarg);
|
2016-09-21 17:46:12 -03:00
|
|
|
break;
|
2016-10-24 07:29:49 -03:00
|
|
|
case CMDLINE_IRLOCK_PORT:
|
|
|
|
_irlock_port = atoi(gopt.optarg);
|
|
|
|
break;
|
2020-09-04 19:23:51 -03:00
|
|
|
case CMDLINE_START_TIME:
|
|
|
|
start_time_UTC = atoi(gopt.optarg);
|
|
|
|
break;
|
2018-09-04 14:06:25 -03:00
|
|
|
case CMDLINE_SYSID: {
|
|
|
|
const int32_t sysid = atoi(gopt.optarg);
|
|
|
|
if (sysid < 1 || sysid > 255) {
|
|
|
|
fprintf(stderr, "You must specify a SYSID greater than 0 and less than 256\n");
|
|
|
|
exit(1);
|
|
|
|
}
|
2020-08-23 03:24:22 -03:00
|
|
|
temp_cmdline_param = {"SYSID_THISMAV", static_cast<float>(sysid)};
|
|
|
|
cmdline_param.push_back(temp_cmdline_param);
|
|
|
|
printf("Setting SYSID_THISMAV=%d\n", sysid);
|
2018-09-04 14:06:25 -03:00
|
|
|
break;
|
|
|
|
}
|
2021-06-10 23:30:38 -03:00
|
|
|
#if STORAGE_USE_POSIX
|
|
|
|
case CMDLINE_SET_STORAGE_POSIX_ENABLED:
|
|
|
|
storage_posix_enabled = atoi(gopt.optarg);
|
|
|
|
break;
|
|
|
|
#endif
|
|
|
|
#if STORAGE_USE_FLASH
|
|
|
|
case CMDLINE_SET_STORAGE_FLASH_ENABLED:
|
|
|
|
storage_flash_enabled = atoi(gopt.optarg);
|
|
|
|
break;
|
2021-07-12 22:41:18 -03:00
|
|
|
#endif
|
|
|
|
#if STORAGE_USE_FRAM
|
|
|
|
case CMDLINE_SET_STORAGE_FRAM_ENABLED:
|
|
|
|
storage_fram_enabled = atoi(gopt.optarg);
|
|
|
|
break;
|
2021-06-10 23:30:38 -03:00
|
|
|
#endif
|
2021-05-26 18:19:41 -03:00
|
|
|
case 'h':
|
|
|
|
_usage();
|
|
|
|
exit(0);
|
2021-04-24 21:31:35 -03:00
|
|
|
case CMDLINE_SLAVE: {
|
2021-11-01 04:03:23 -03:00
|
|
|
#if HAL_SIM_JSON_MASTER_ENABLED
|
2021-04-24 21:31:35 -03:00
|
|
|
const int32_t slaves = atoi(gopt.optarg);
|
|
|
|
if (slaves > 0) {
|
|
|
|
ride_along.init(slaves);
|
|
|
|
}
|
2021-11-01 04:03:23 -03:00
|
|
|
#endif
|
2021-04-24 21:31:35 -03:00
|
|
|
break;
|
|
|
|
}
|
2015-05-04 21:59:07 -03:00
|
|
|
default:
|
|
|
|
_usage();
|
|
|
|
exit(1);
|
|
|
|
}
|
|
|
|
}
|
2015-05-04 18:08:42 -03:00
|
|
|
|
2015-11-22 22:21:33 -04:00
|
|
|
if (!model_str) {
|
2020-02-05 23:33:57 -04:00
|
|
|
printf("You must specify a vehicle model. Options are:\n");
|
|
|
|
for (uint8_t i=0; i < ARRAY_SIZE(model_constructors); i++) {
|
|
|
|
printf(" %s\n", model_constructors[i].name);
|
|
|
|
}
|
|
|
|
// spit this out again as the original message probably just
|
|
|
|
// scrolled off the screen:
|
|
|
|
printf("You must specify a vehicle model.\n");
|
2015-11-22 22:21:33 -04:00
|
|
|
exit(1);
|
|
|
|
}
|
|
|
|
|
|
|
|
for (uint8_t i=0; i < ARRAY_SIZE(model_constructors); i++) {
|
|
|
|
if (strncasecmp(model_constructors[i].name, model_str, strlen(model_constructors[i].name)) == 0) {
|
2019-08-15 03:52:50 -03:00
|
|
|
// printf("Creating model %f,%f,%f,%f at speed %.1f\n", opos.lat, opos.lng, opos.alt, opos.hdg, speedup);
|
2019-08-15 01:00:04 -03:00
|
|
|
sitl_model = model_constructors[i].constructor(model_str);
|
2019-08-15 03:52:50 -03:00
|
|
|
if (home_str != nullptr) {
|
|
|
|
Location home;
|
|
|
|
float home_yaw;
|
2020-12-30 15:56:35 -04:00
|
|
|
if (strchr(home_str,',') == nullptr) {
|
|
|
|
if (!lookup_location(home_str, home, home_yaw)) {
|
|
|
|
::printf("Failed to find location (%s). Should be in locations.txt or LAT,LON,ALT,HDG e.g. 37.4003371,-122.0800351,0,353\n", home_str);
|
|
|
|
exit(1);
|
|
|
|
}
|
|
|
|
} else if (!parse_home(home_str, home, home_yaw)) {
|
2019-08-15 03:52:50 -03:00
|
|
|
::printf("Failed to parse home string (%s). Should be LAT,LON,ALT,HDG e.g. 37.4003371,-122.0800351,0,353\n", home_str);
|
|
|
|
exit(1);
|
|
|
|
}
|
|
|
|
sitl_model->set_start_location(home, home_yaw);
|
|
|
|
}
|
2018-06-16 22:44:33 -03:00
|
|
|
sitl_model->set_interface_ports(simulator_address, simulator_port_in, simulator_port_out);
|
2015-11-22 22:21:33 -04:00
|
|
|
sitl_model->set_speedup(speedup);
|
|
|
|
sitl_model->set_instance(_instance);
|
|
|
|
sitl_model->set_autotest_dir(autotest_dir);
|
2019-07-18 14:39:00 -03:00
|
|
|
sitl_model->set_config(config);
|
2015-11-22 22:21:33 -04:00
|
|
|
_synthetic_clock_mode = true;
|
|
|
|
break;
|
2015-05-04 18:08:42 -03:00
|
|
|
}
|
|
|
|
}
|
2016-05-16 00:02:25 -03:00
|
|
|
if (sitl_model == nullptr) {
|
|
|
|
printf("Vehicle model (%s) not found\n", model_str);
|
|
|
|
exit(1);
|
|
|
|
}
|
2015-05-04 18:08:42 -03:00
|
|
|
|
2021-06-10 23:30:38 -03:00
|
|
|
if (storage_posix_enabled && storage_flash_enabled) {
|
|
|
|
// this will change in the future!
|
|
|
|
printf("Only one of flash or posix storage may be selected");
|
|
|
|
exit(1);
|
|
|
|
}
|
|
|
|
|
2020-09-09 20:12:56 -03:00
|
|
|
if (AP::sitl()) {
|
|
|
|
// Set SITL start time.
|
|
|
|
AP::sitl()->start_time_UTC = start_time_UTC;
|
|
|
|
}
|
2020-09-04 19:23:51 -03:00
|
|
|
|
2021-10-05 03:29:45 -03:00
|
|
|
hal.set_storage_posix_enabled(storage_posix_enabled);
|
|
|
|
hal.set_storage_flash_enabled(storage_flash_enabled);
|
2021-07-12 22:41:18 -03:00
|
|
|
hal.set_storage_fram_enabled(storage_fram_enabled);
|
2021-06-10 23:30:38 -03:00
|
|
|
|
|
|
|
if (erase_all_storage) {
|
|
|
|
AP_Param::erase_all();
|
|
|
|
unlink("flash.dat");
|
2021-07-12 22:41:18 -03:00
|
|
|
hal.set_wipe_storage(wiping_storage);
|
2021-06-10 23:30:38 -03:00
|
|
|
}
|
|
|
|
|
2022-10-03 13:52:29 -03:00
|
|
|
fprintf(stdout, "Starting sketch '%s'\n", vehicle_str);
|
2015-05-04 21:59:07 -03:00
|
|
|
|
2022-10-03 13:52:29 -03:00
|
|
|
if (strcmp(vehicle_str, "ArduCopter") == 0) {
|
2015-05-04 21:59:07 -03:00
|
|
|
_vehicle = ArduCopter;
|
2022-10-03 13:52:29 -03:00
|
|
|
} else if (strcmp(vehicle_str, "Rover") == 0) {
|
2020-03-26 21:51:15 -03:00
|
|
|
_vehicle = Rover;
|
2015-05-04 21:59:07 -03:00
|
|
|
// set right default throttle for rover (allowing for reverse)
|
2015-05-04 18:08:42 -03:00
|
|
|
pwm_input[2] = 1500;
|
2022-10-03 13:52:29 -03:00
|
|
|
} else if (strcmp(vehicle_str, "ArduSub") == 0) {
|
2017-10-27 14:24:53 -03:00
|
|
|
_vehicle = ArduSub;
|
2017-11-22 16:43:46 -04:00
|
|
|
for(uint8_t i = 0; i < 8; i++) {
|
|
|
|
pwm_input[i] = 1500;
|
|
|
|
}
|
2022-10-03 13:52:29 -03:00
|
|
|
} else if (strcmp(vehicle_str, "Blimp") == 0) {
|
2021-08-06 01:09:40 -03:00
|
|
|
_vehicle = Blimp;
|
|
|
|
for(uint8_t i = 0; i < 8; i++) {
|
|
|
|
pwm_input[i] = 1500;
|
|
|
|
}
|
2015-05-04 21:59:07 -03:00
|
|
|
} else {
|
|
|
|
_vehicle = ArduPlane;
|
|
|
|
}
|
|
|
|
|
2021-12-11 05:49:33 -04:00
|
|
|
_sitl_setup();
|
2015-05-04 18:08:42 -03:00
|
|
|
}
|
|
|
|
|
2019-08-15 03:52:50 -03:00
|
|
|
/*
|
|
|
|
parse a home string into a location and yaw
|
|
|
|
*/
|
|
|
|
bool SITL_State::parse_home(const char *home_str, Location &loc, float &yaw_degrees)
|
|
|
|
{
|
|
|
|
char *saveptr = nullptr;
|
|
|
|
char *s = strdup(home_str);
|
|
|
|
if (!s) {
|
|
|
|
free(s);
|
|
|
|
::printf("No home string supplied\n");
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
char *lat_s = strtok_r(s, ",", &saveptr);
|
|
|
|
if (!lat_s) {
|
|
|
|
free(s);
|
|
|
|
::printf("Failed to parse latitude\n");
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
char *lon_s = strtok_r(nullptr, ",", &saveptr);
|
|
|
|
if (!lon_s) {
|
|
|
|
free(s);
|
|
|
|
::printf("Failed to parse longitude\n");
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
char *alt_s = strtok_r(nullptr, ",", &saveptr);
|
|
|
|
if (!alt_s) {
|
|
|
|
free(s);
|
|
|
|
::printf("Failed to parse altitude\n");
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
char *yaw_s = strtok_r(nullptr, ",", &saveptr);
|
|
|
|
if (!yaw_s) {
|
|
|
|
free(s);
|
|
|
|
::printf("Failed to parse yaw\n");
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
loc = {};
|
|
|
|
loc.lat = static_cast<int32_t>(strtod(lat_s, nullptr) * 1.0e7);
|
|
|
|
loc.lng = static_cast<int32_t>(strtod(lon_s, nullptr) * 1.0e7);
|
|
|
|
loc.alt = static_cast<int32_t>(strtod(alt_s, nullptr) * 1.0e2);
|
|
|
|
|
|
|
|
if (loc.lat == 0 && loc.lng == 0) {
|
|
|
|
// default to CMAC instead of middle of the ocean. This makes
|
|
|
|
// SITL in MissionPlanner a bit more useful
|
|
|
|
loc.lat = -35.363261*1e7;
|
|
|
|
loc.lng = 149.165230*1e7;
|
|
|
|
loc.alt = 584*100;
|
|
|
|
}
|
|
|
|
|
|
|
|
yaw_degrees = strtof(yaw_s, nullptr);
|
|
|
|
free(s);
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2020-12-30 15:56:35 -04:00
|
|
|
/*
|
|
|
|
lookup a location in locations.txt in ROMFS
|
|
|
|
*/
|
|
|
|
bool SITL_State::lookup_location(const char *home_str, Location &loc, float &yaw_degrees)
|
|
|
|
{
|
|
|
|
const char *locations = "@ROMFS/locations.txt";
|
|
|
|
FileData *fd = AP::FS().load_file(locations);
|
|
|
|
if (fd == nullptr) {
|
|
|
|
::printf("Missing %s\n", locations);
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
char *str = strndup((const char *)fd->data, fd->length);
|
|
|
|
if (!str) {
|
|
|
|
delete fd;
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
size_t len = strlen(home_str);
|
|
|
|
char *saveptr = nullptr;
|
|
|
|
for (char *s = strtok_r(str, "\r\n", &saveptr);
|
|
|
|
s;
|
|
|
|
s=strtok_r(nullptr, "\r\n", &saveptr)) {
|
|
|
|
if (strncasecmp(s, home_str, len) == 0 && s[len]=='=') {
|
|
|
|
bool ok = parse_home(&s[len+1], loc, yaw_degrees);
|
|
|
|
free(str);
|
|
|
|
delete fd;
|
|
|
|
return ok;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
free(str);
|
|
|
|
delete fd;
|
|
|
|
::printf("Failed to find location '%s'\n", home_str);
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2015-05-04 18:08:42 -03:00
|
|
|
#endif
|