From a6faffccf0d362269c920b72722363c8db3b719a Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Mon, 2 Nov 2020 17:34:09 +0100 Subject: [PATCH 1/2] Enable HIL simulation for jsbsim This commit adds configuration options for running HITL simulation with the PX4 JSBSim bridge The device path / baudrate can be passed with the command line flag -d and -b. The bridge is configured to be run by HITL when the -d is provided. the baudrate is configured to be 921600 by default Enable HITL on mavlink interface This PR enables HITL on the mavlink interface Handle gcs udp port in config file --- include/configuration_parser.h | 7 +++++++ include/jsbsim_bridge.h | 3 ++- src/configuration_parser.cpp | 21 +++++++++++++++++---- src/jsbsim_bridge.cpp | 32 +++++++++++++++++++++++++++----- 4 files changed, 53 insertions(+), 10 deletions(-) diff --git a/include/configuration_parser.h b/include/configuration_parser.h index 9c60e55..582a958 100644 --- a/include/configuration_parser.h +++ b/include/configuration_parser.h @@ -60,6 +60,9 @@ class ConfigurationParser { std::string getInitScriptPath() { return _init_script_path; } std::string getModelName() { return _model_name; } int getRealtimeFactor() { return _realtime_factor; } + bool getSerialEnabled() { return _serial_enabled; } + int getBaudrate() { return _baudrate; } + std::string getDevice() { return _device; } void setHeadless(bool headless) { _headless = headless; } void setInitScriptPath(std::string path) { _init_script_path = path; } static void PrintHelpMessage(char* argv[]); @@ -72,4 +75,8 @@ class ConfigurationParser { std::string _init_script_path; std::string _model_name; float _realtime_factor{1.0}; + // HITL Configs + bool _serial_enabled{false}; + std::string _device; + int _baudrate{921600}; }; diff --git a/include/jsbsim_bridge.h b/include/jsbsim_bridge.h index bb18eb3..96ef36e 100644 --- a/include/jsbsim_bridge.h +++ b/include/jsbsim_bridge.h @@ -58,6 +58,7 @@ #include static constexpr int kDefaultSITLTcpPort = 4560; +static constexpr int kDefaultGCSPort = 14550; class JSBSimBridge { public: @@ -67,7 +68,7 @@ class JSBSimBridge { private: bool SetFdmConfigs(ConfigurationParser &cfg); - bool SetMavlinkInterfaceConfigs(std::unique_ptr &interface, TiXmlHandle &config); + bool SetMavlinkInterfaceConfigs(std::unique_ptr &interface, ConfigurationParser &cfg); JSBSim::FGFDMExec *_fdmexec; // FDMExec pointer ConfigurationParser &_cfg; diff --git a/src/configuration_parser.cpp b/src/configuration_parser.cpp index edc9933..b29b26b 100644 --- a/src/configuration_parser.cpp +++ b/src/configuration_parser.cpp @@ -56,10 +56,12 @@ bool ConfigurationParser::ParseEnvironmentVariables() { ArgResult ConfigurationParser::ParseArgV(int argc, char* const argv[]) { static const struct option options[] = { {"scene", required_argument, nullptr, 's'}, + {"device", required_argument, nullptr, 'd'}, + {"baudrate", required_argument, nullptr, 'b'}, }; int c; - while ((c = getopt_long(argc, argv, "s:h", options, nullptr)) >= 0) { + while ((c = getopt_long(argc, argv, "s:d:b:h", options, nullptr)) >= 0) { switch (c) { case 'h': { return ArgResult::Help; @@ -69,6 +71,15 @@ ArgResult ConfigurationParser::ParseArgV(int argc, char* const argv[]) { _init_script_path = std::string(optarg); break; } + case 'd': { + _device = std::string(optarg); + _serial_enabled = true; + break; + } + case 'b': { + _baudrate = std::stoi(std::string(optarg)); + break; + } case '?': default: { std::cout << "Unknown Options" << std::endl; @@ -101,7 +112,9 @@ bool ConfigurationParser::ParseConfigFile(const std::string& path) { void ConfigurationParser::PrintHelpMessage(char* argv[]) { std::cout << argv[0] << " aircraft [options]\n\n" - << " aircraft Aircraft config file name e.g. rascal" - << " -h | --help Print available options\n" - << " -s | --scene Location / scene where the vehicle should be spawned in e.g. LSZH\n"; + << " aircraft Aircraft config file name e.g. rascal" + << " -h | --help Print available options\n" + << " -s | --scene Location / scene where the vehicle should be spawned in e.g. LSZH\n" + << " -d | --device Device path for FMU for HITL simulation e.g. /dev/ttyACM0\n" + << " -b | --baudrate Device baudrate for FMU for HITL simulation e.g. 921600\n"; } diff --git a/src/jsbsim_bridge.cpp b/src/jsbsim_bridge.cpp index cddd408..dd8b465 100644 --- a/src/jsbsim_bridge.cpp +++ b/src/jsbsim_bridge.cpp @@ -53,7 +53,7 @@ JSBSimBridge::JSBSimBridge(JSBSim::FGFDMExec *fdmexec, ConfigurationParser &cfg) // Configure Mavlink HIL interface _mavlink_interface = std::make_unique(); - SetMavlinkInterfaceConfigs(_mavlink_interface, config); + SetMavlinkInterfaceConfigs(_mavlink_interface, _cfg); _mavlink_interface->Load(); @@ -149,18 +149,35 @@ bool JSBSimBridge::SetFdmConfigs(ConfigurationParser &cfg) { } } -bool JSBSimBridge::SetMavlinkInterfaceConfigs(std::unique_ptr &interface, TiXmlHandle &config) { +bool JSBSimBridge::SetMavlinkInterfaceConfigs(std::unique_ptr &interface, ConfigurationParser &cfg) { + TiXmlHandle config = *cfg.XmlHandle(); TiXmlElement *mavlink_configs = config.FirstChild("mavlink_interface").Element(); if (!mavlink_configs) return true; // Nothing to set int tcp_port = kDefaultSITLTcpPort; GetConfigElement(config, "mavlink_interface", "tcp_port", tcp_port); + int udp_port = kDefaultGCSPort; + GetConfigElement(config, "mavlink_interface", "udp_port", udp_port); bool enable_lockstep = true; GetConfigElement(config, "mavlink_interface", "enable_lockstep", enable_lockstep); - interface->SetMavlinkTcpPort(tcp_port); - interface->SetUseTcp(true); + if (cfg.getSerialEnabled()) { + // Configure for HITL when serial is enabled + interface->SetSerialEnabled(cfg.getSerialEnabled()); + interface->SetDevice(cfg.getDevice()); + interface->SetBaudrate(cfg.getBaudrate()); + interface->SetHILStateLevel(true); + interface->SetHILMode(true); + interface->SetGcsAddr("INADDR_ANY"); + interface->SetGcsUdpPort(udp_port); + + } else { + // Configure for SITL as default + interface->SetMavlinkTcpPort(tcp_port); + interface->SetUseTcp(true); + } + interface->SetEnableLockstep(enable_lockstep); return true; @@ -199,7 +216,12 @@ void JSBSimBridge::Run() { } // Receive and handle actuator controls - _mavlink_interface->pollForMAVLinkMessages(); + bool hil_mode_ = true; + if (hil_mode_) { + _mavlink_interface->pollFromGcsAndSdk(); + } else { + _mavlink_interface->pollForMAVLinkMessages(); + } Eigen::VectorXd actuator_controls = _mavlink_interface->GetActuatorControls(); if (actuator_controls.size() >= 16) { From 1f50d53c6cc71b5362e5bdbc6b1d9d22a199d606 Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Sat, 12 Dec 2020 22:38:26 +0100 Subject: [PATCH 2/2] Update src/configuration_parser.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Beat Küng --- src/configuration_parser.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/configuration_parser.cpp b/src/configuration_parser.cpp index b29b26b..6496e39 100644 --- a/src/configuration_parser.cpp +++ b/src/configuration_parser.cpp @@ -77,7 +77,7 @@ ArgResult ConfigurationParser::ParseArgV(int argc, char* const argv[]) { break; } case 'b': { - _baudrate = std::stoi(std::string(optarg)); + _baudrate = atoi(optarg); break; } case '?':