diff --git a/include/configuration_parser.h b/include/configuration_parser.h index 59c7646..ceea085 100644 --- a/include/configuration_parser.h +++ b/include/configuration_parser.h @@ -42,17 +42,14 @@ #include "common.h" -#include #include #include +#include -enum class ArgResult { - Success, Help, Error -}; +enum class ArgResult { Success, Help, Error }; class ConfigurationParser { public: - ConfigurationParser() = default; ~ConfigurationParser() = default; bool ParseEnvironmentVariables(); @@ -60,9 +57,12 @@ class ConfigurationParser { ArgResult ParseArgV(int argc, char* const argv[]); inline bool isHeadless() { return _headless; } inline std::shared_ptr XmlHandle() { return _config; } + inline std::string getDevice() { return _device; } inline std::string getInitScriptPath() { return _init_script_path; } inline std::string getModelName() { return _model_name; } - static void PrintHelpMessage(char *argv[]); + inline bool getSerialEnabled() { return _serial_enabled; } + inline int getBaudrate() { return _baudrate; } + static void PrintHelpMessage(char* argv[]); private: TiXmlDocument _doc; @@ -71,4 +71,9 @@ class ConfigurationParser { bool _headless{false}; std::string _init_script_path; std::string _model_name; + + // 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 c721911..43e6bb3 100644 --- a/include/jsbsim_bridge.h +++ b/include/jsbsim_bridge.h @@ -67,7 +67,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/include/sensor_imu_plugin.h b/include/sensor_imu_plugin.h index 8cdd16a..337bb04 100644 --- a/include/sensor_imu_plugin.h +++ b/include/sensor_imu_plugin.h @@ -97,9 +97,9 @@ class SensorImuPlugin : public SensorPlugin { std::normal_distribution _standard_normal_distribution; /** Accelerations are affected by JSBSim airframe configuration - * ensure you have set the eyepoint location as to where you expect accelerometer measurements - * or more appropriately, use the px4_default_imu_sensor.xml configuration. - */ + * ensure you have set the eyepoint location as to where you expect accelerometer measurements + * or more appropriately, use the px4_default_imu_sensor.xml configuration. + */ std::string _jsb_acc_x = "accelerations/a-pilot-x-ft_sec2"; std::string _jsb_acc_y = "accelerations/a-pilot-y-ft_sec2"; std::string _jsb_acc_z = "accelerations/a-pilot-z-ft_sec2"; diff --git a/src/configuration_parser.cpp b/src/configuration_parser.cpp index 7a82a13..a90a0f0 100644 --- a/src/configuration_parser.cpp +++ b/src/configuration_parser.cpp @@ -50,28 +50,39 @@ bool ConfigurationParser::ParseEnvironmentVariables() { } ArgResult ConfigurationParser::ParseArgV(int argc, char* const argv[]) { - static const struct option options[] = { - {"scene", required_argument, nullptr, 's'}, - }; + 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) { - switch (c) { - case 'h': { - return ArgResult::Help; - break; - } - case 's': { - _init_script_path = std::string(optarg); - break; - } - case '?': - default: { - std::cout << "Unknown Options" << std::endl; - return ArgResult::Error; - } - } + int c; + while ((c = getopt_long(argc, argv, "s:d:b:h", options, nullptr)) >= 0) { + switch (c) { + case 'h': { + return ArgResult::Help; + break; + } + case 's': { + _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; + return ArgResult::Error; + } } + } return ArgResult::Success; } @@ -95,9 +106,11 @@ bool ConfigurationParser::ParseConfigFile(const std::string& path) { return true; } -void ConfigurationParser::PrintHelpMessage(char *argv[]) { +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 7017f70..085061a 100644 --- a/src/jsbsim_bridge.cpp +++ b/src/jsbsim_bridge.cpp @@ -54,7 +54,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(); @@ -132,8 +132,8 @@ bool JSBSimBridge::SetFdmConfigs(ConfigurationParser &cfg) { std::string jsb_script; if (config && CheckConfigElement(*config, "jsb_script")) { std::size_t found = aircraft_path.rfind(aircraft_model); - if (found==std::string::npos) { - std::cout << "JSBSIM SCRIPT LOADING DOES NOT SUPPORT: " << aircraft_path << " <> " << aircraft_model << std::endl; + if (found == std::string::npos) { + std::cout << "JSBSIM SCRIPT LOADING DOES NOT SUPPORT: " << aircraft_path << " <> " << aircraft_model << std::endl; return false; } else { _fdmexec->SetAircraftPath(SGPath("models/")); @@ -150,7 +150,8 @@ 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 @@ -160,8 +161,17 @@ bool JSBSimBridge::SetMavlinkInterfaceConfigs(std::unique_ptr 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()); + } else { + // Configure for SITL as default + interface->SetMavlinkTcpPort(tcp_port); + interface->SetUseTcp(true); + } + interface->SetEnableLockstep(enable_lockstep); return true; diff --git a/src/main.cpp b/src/main.cpp index d524de5..6169119 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -42,7 +42,6 @@ #include "jsbsim_bridge.h" int main(int argc, char *argv[]) { - // Parse Configurations ConfigurationParser config; if (argc > 1) { @@ -51,15 +50,15 @@ int main(int argc, char *argv[]) { config.ParseConfigFile(path); } switch (config.ParseArgV(argc, argv)) { - case ArgResult::Success : { + case ArgResult::Success: { break; } - case ArgResult::Help : { + case ArgResult::Help: { ConfigurationParser::PrintHelpMessage(argv); return 0; } default: - case ArgResult::Error : { + case ArgResult::Error: { ConfigurationParser::PrintHelpMessage(argv); return 1; } diff --git a/src/sensor_gps_plugin.cpp b/src/sensor_gps_plugin.cpp index bb273a6..5236c2b 100644 --- a/src/sensor_gps_plugin.cpp +++ b/src/sensor_gps_plugin.cpp @@ -46,17 +46,17 @@ SensorGpsPlugin::SensorGpsPlugin(JSBSim::FGFDMExec* jsbsim) : SensorPlugin(jsbsi SensorGpsPlugin::~SensorGpsPlugin() {} void SensorGpsPlugin::setSensorConfigs(const TiXmlElement& configs) { - GetConfigElement(configs, "jsb_gps_fix_type", jsb_gps_fix_type); - GetConfigElement(configs, "jsb_gps_lat", jsb_gps_lat); - GetConfigElement(configs, "jsb_gps_lon", jsb_gps_lon); - GetConfigElement(configs, "jsb_gps_alt", jsb_gps_alt); - GetConfigElement(configs, "jsb_gps_eph", jsb_gps_eph); - GetConfigElement(configs, "jsb_gps_epv", jsb_gps_epv); - GetConfigElement(configs, "jsb_gps_v_north", jsb_gps_v_north); - GetConfigElement(configs, "jsb_gps_v_east", jsb_gps_v_east); - GetConfigElement(configs, "jsb_gps_v_down", jsb_gps_v_down); - GetConfigElement(configs, "jsb_gps_velocity", jsb_gps_velocity); - GetConfigElement(configs, "jsb_gps_satellites", jsb_gps_satellites); + GetConfigElement(configs, "jsb_gps_fix_type", jsb_gps_fix_type); + GetConfigElement(configs, "jsb_gps_lat", jsb_gps_lat); + GetConfigElement(configs, "jsb_gps_lon", jsb_gps_lon); + GetConfigElement(configs, "jsb_gps_alt", jsb_gps_alt); + GetConfigElement(configs, "jsb_gps_eph", jsb_gps_eph); + GetConfigElement(configs, "jsb_gps_epv", jsb_gps_epv); + GetConfigElement(configs, "jsb_gps_v_north", jsb_gps_v_north); + GetConfigElement(configs, "jsb_gps_v_east", jsb_gps_v_east); + GetConfigElement(configs, "jsb_gps_v_down", jsb_gps_v_down); + GetConfigElement(configs, "jsb_gps_velocity", jsb_gps_velocity); + GetConfigElement(configs, "jsb_gps_satellites", jsb_gps_satellites); } SensorData::Gps SensorGpsPlugin::getData() { @@ -75,26 +75,26 @@ SensorData::Gps SensorGpsPlugin::getGpsFromJSBSim() { SensorData::Gps ret; ret.time_utc_usec = _sim_ptr->GetSimTime() * 1e6; - if(jsb_gps_fix_type == "none") { + if (jsb_gps_fix_type == "none") { ret.fix_type = 3; } else { - ret.fix_type = _sim_ptr->GetPropertyValue(jsb_gps_fix_type); + ret.fix_type = _sim_ptr->GetPropertyValue(jsb_gps_fix_type); } ret.latitude_deg = _sim_ptr->GetPropertyValue(jsb_gps_lat) * 1e7; ret.longitude_deg = _sim_ptr->GetPropertyValue(jsb_gps_lon) * 1e7; ret.altitude = _sim_ptr->GetPropertyValue(jsb_gps_alt) * 1e3; - if(jsb_gps_eph == "none") { + if (jsb_gps_eph == "none") { ret.eph = 1 * 100; } else { - ret.eph = _sim_ptr->GetPropertyValue(jsb_gps_eph)*100; + ret.eph = _sim_ptr->GetPropertyValue(jsb_gps_eph) * 100; } - if(jsb_gps_epv == "none") { + if (jsb_gps_epv == "none") { ret.epv = 2 * 100; } else { - ret.epv = _sim_ptr->GetPropertyValue(jsb_gps_epv)*100; + ret.epv = _sim_ptr->GetPropertyValue(jsb_gps_epv) * 100; } ret.velocity_north = ftToM(_sim_ptr->GetPropertyValue(jsb_gps_v_north)) * 100; @@ -102,10 +102,10 @@ SensorData::Gps SensorGpsPlugin::getGpsFromJSBSim() { ret.velocity_down = ftToM(_sim_ptr->GetPropertyValue(jsb_gps_v_down)) * 100; ret.velocity = ftToM(_sim_ptr->GetPropertyValue(jsb_gps_velocity)) * 100; - if(jsb_gps_satellites == "none") { + if (jsb_gps_satellites == "none") { ret.satellites_visible = 16; } else { - ret.satellites_visible = _sim_ptr->GetPropertyValue(jsb_gps_satellites); + ret.satellites_visible = _sim_ptr->GetPropertyValue(jsb_gps_satellites); } ret.id = 1; diff --git a/src/sensor_mag_plugin.cpp b/src/sensor_mag_plugin.cpp index bfd3215..7703547 100644 --- a/src/sensor_mag_plugin.cpp +++ b/src/sensor_mag_plugin.cpp @@ -78,52 +78,52 @@ SensorData::Magnetometer SensorMagPlugin::getData() { } Eigen::Vector3d SensorMagPlugin::getMagFromJSBSim() { - if(_jsb_mag_x == "none" && _jsb_mag_y == "none" && _jsb_mag_z == "none") { - double lat_deg, lon_deg, roll_rad, pitch_rad, heading_rad; + if (_jsb_mag_x == "none" && _jsb_mag_y == "none" && _jsb_mag_z == "none") { + double lat_deg, lon_deg, roll_rad, pitch_rad, heading_rad; - lat_deg = _sim_ptr->GetPropertyValue(_jsb_mag_lat); - lon_deg = _sim_ptr->GetPropertyValue(_jsb_mag_lon); - roll_rad = _sim_ptr->GetPropertyValue(_jsb_mag_roll); - pitch_rad = _sim_ptr->GetPropertyValue(_jsb_mag_pitch); - heading_rad = wrap_pi(_sim_ptr->GetPropertyValue(_jsb_mag_hdg)); + lat_deg = _sim_ptr->GetPropertyValue(_jsb_mag_lat); + lon_deg = _sim_ptr->GetPropertyValue(_jsb_mag_lon); + roll_rad = _sim_ptr->GetPropertyValue(_jsb_mag_roll); + pitch_rad = _sim_ptr->GetPropertyValue(_jsb_mag_pitch); + heading_rad = wrap_pi(_sim_ptr->GetPropertyValue(_jsb_mag_hdg)); - // Magnetic strength (10^5xnanoTesla) - float strength_ga = 0.01f * get_mag_strength(lat_deg, lon_deg); + // Magnetic strength (10^5xnanoTesla) + float strength_ga = 0.01f * get_mag_strength(lat_deg, lon_deg); - // Magnetic declination and inclination (radians) - float declination_rad = get_mag_declination(lat_deg, lon_deg) * 3.14159265f / 180; - float inclination_rad = get_mag_inclination(lat_deg, lon_deg) * 3.14159265f / 180; + // Magnetic declination and inclination (radians) + float declination_rad = get_mag_declination(lat_deg, lon_deg) * 3.14159265f / 180; + float inclination_rad = get_mag_inclination(lat_deg, lon_deg) * 3.14159265f / 180; - // Magnetic filed components are calculated by http://geomag.nrcan.gc.ca/mag_fld/comp-en.php - float H = strength_ga * cosf(inclination_rad); - float Z = H * tanf(inclination_rad); - float X = H * cosf(declination_rad); - float Y = H * sinf(declination_rad); + // Magnetic filed components are calculated by http://geomag.nrcan.gc.ca/mag_fld/comp-en.php + float H = strength_ga * cosf(inclination_rad); + float Z = H * tanf(inclination_rad); + float X = H * cosf(declination_rad); + float Y = H * sinf(declination_rad); - _mag_g[0] = X; - _mag_g[1] = Y; - _mag_g[2] = Z; + _mag_g[0] = X; + _mag_g[1] = Y; + _mag_g[2] = Z; - Eigen::AngleAxisd rollAngle(roll_rad, Eigen::Vector3d::UnitX()); - Eigen::AngleAxisd yawAngle(heading_rad, Eigen::Vector3d::UnitZ()); - Eigen::AngleAxisd pitchAngle(pitch_rad, Eigen::Vector3d::UnitY()); + Eigen::AngleAxisd rollAngle(roll_rad, Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd yawAngle(heading_rad, Eigen::Vector3d::UnitZ()); + Eigen::AngleAxisd pitchAngle(pitch_rad, Eigen::Vector3d::UnitY()); - Eigen::Quaternion q = yawAngle * pitchAngle * rollAngle; + Eigen::Quaternion q = yawAngle * pitchAngle * rollAngle; - Eigen::Matrix3d rotationMatrix = q.inverse().matrix(); + Eigen::Matrix3d rotationMatrix = q.inverse().matrix(); - Eigen::Vector3d mag1 = rotationMatrix * _mag_g; + Eigen::Vector3d mag1 = rotationMatrix * _mag_g; - return mag1; + return mag1; } else { - // Enable reading magnetic values directly from JSBSim when supplied with system file - // NOTE: Current JSBSim magnetometer startup sequence is not stable for PX4 in V1.1.0 - // See https://github.com/JSBSim-Team/jsbsim/issues/332 - _mag_g[0] = _sim_ptr->GetPropertyValue(_jsb_mag_x) * 1e-5; - _mag_g[1] = _sim_ptr->GetPropertyValue(_jsb_mag_y) * 1e-5; - _mag_g[2] = _sim_ptr->GetPropertyValue(_jsb_mag_z) * 1e-5; - return _mag_g; + // Enable reading magnetic values directly from JSBSim when supplied with system file + // NOTE: Current JSBSim magnetometer startup sequence is not stable for PX4 in V1.1.0 + // See https://github.com/JSBSim-Team/jsbsim/issues/332 + _mag_g[0] = _sim_ptr->GetPropertyValue(_jsb_mag_x) * 1e-5; + _mag_g[1] = _sim_ptr->GetPropertyValue(_jsb_mag_y) * 1e-5; + _mag_g[2] = _sim_ptr->GetPropertyValue(_jsb_mag_z) * 1e-5; + return _mag_g; } }