Enable Hardware-In-The-Loop simulation for JSBSim

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};
 };

include/jsbsim_bridge.h

@@ -58,6 +58,7 @@
 #include <chrono>
 
 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<MavlinkInterface> &interface, TiXmlHandle &config);
+  bool SetMavlinkInterfaceConfigs(std::unique_ptr<MavlinkInterface> &interface, ConfigurationParser &cfg);
 
   JSBSim::FGFDMExec *_fdmexec;  // FDMExec pointer
   ConfigurationParser &_cfg;

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,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";
 }

src/jsbsim_bridge.cpp

@@ -53,7 +53,7 @@ JSBSimBridge::JSBSimBridge(JSBSim::FGFDMExec *fdmexec, ConfigurationParser &cfg)
 
   // Configure Mavlink HIL interface
   _mavlink_interface = std::make_unique<MavlinkInterface>();
-  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<MavlinkInterface> &interface, TiXmlHandle &config) {
+bool JSBSimBridge::SetMavlinkInterfaceConfigs(std::unique_ptr<MavlinkInterface> &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<int>(config, "mavlink_interface", "tcp_port", tcp_port);
+  int udp_port = kDefaultGCSPort;
+  GetConfigElement<int>(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) {