From 769cc356ddeb7e6b128441c9d41b6906dc49ac31 Mon Sep 17 00:00:00 2001 From: tayyabkhalil-313 <109569555+tayyabkhalil-313@users.noreply.github.com> Date: Mon, 19 Dec 2022 09:56:57 +0500 Subject: [PATCH] Add support for HIL --- include/configuration_parser.h | 8 ++++++++ include/jsbsim_bridge.h | 4 +++- src/configuration_parser.cpp | 22 ++++++++++++++++---- src/jsbsim_bridge.cpp | 37 ++++++++++++++++++++++++++-------- 4 files changed, 58 insertions(+), 13 deletions(-) diff --git a/include/configuration_parser.h b/include/configuration_parser.h index 9c60e55..7009e27 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,9 @@ 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..6053775 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; @@ -84,3 +85,4 @@ class JSBSimBridge { double _realtime_factor{1.0}; bool _result{true}; }; + diff --git a/src/configuration_parser.cpp b/src/configuration_parser.cpp index edc9933..d0375c5 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 = atoi(optarg); + break; + } case '?': default: { std::cout << "Unknown Options" << std::endl; @@ -101,7 +112,10 @@ 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 79fb75a..719466b 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(); @@ -83,7 +83,7 @@ JSBSimBridge::JSBSimBridge(JSBSim::FGFDMExec *fdmexec, ConfigurationParser &cfg) if (CheckConfigElement(config, "sensors", "airspeed")) { _airspeed_sensor = std::make_unique(_fdmexec); - _airspeed_sensor->setSensorConfigs(GetXmlElement(config, "sensors", "airspeed")); + _mag_sensor->setSensorConfigs(GetXmlElement(config, "sensors", "airspeed")); } _actuators = std::make_unique(_fdmexec); @@ -149,18 +149,33 @@ 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; + 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->SetGcsUdpPort(udp_port); + + if (cfg.getSerialEnabled()) { + // Configure for HITL when serial is enabled + interface->SetSerialEnabled(cfg.getSerialEnabled()); + interface->SetDevice(cfg.getDevice()); + interface->SetBaudrate(cfg.getBaudrate()); + interface->SetHILMode(true); + cout <<"hilmode is enabled"<SetMavlinkTcpPort(tcp_port); + interface->SetUseTcp(true); + } - interface->SetMavlinkTcpPort(tcp_port); - interface->SetUseTcp(true); interface->SetEnableLockstep(enable_lockstep); return true; @@ -199,7 +214,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) { @@ -212,7 +232,8 @@ void JSBSimBridge::Run() { std::chrono::duration elapsed_time = step_start_time - step_stop_time; if (_realtime_factor > 0) { - double sleep = _dt / _realtime_factor - elapsed_time.count(); + double sleep = _dt/_realtime_factor - elapsed_time.count(); if (sleep > 0) usleep(sleep * 1e6); } } +