From 48f40b3c4c1f968dde342fa31e52d81b7b07272d Mon Sep 17 00:00:00 2001 From: Matt Vacanti Date: Wed, 14 Oct 2020 07:24:58 -0400 Subject: [PATCH 1/3] Enable user defined GPS Parameters from JSBSim (#19) * Add JSBSim GPS parameters to airframe * Update logic to read GPS parameters from config file * Move default GPS JSBSim parameters * Remove workflow due to scope * Add JSBSim systems definition file for GPS sensor * Update jsbsim_bridge.cpp with JSBSIm systems path * Update quadrotor_x.xml with reference to GPS systems file * Revert "Remove workflow due to scope" This reverts commit 6f515489 --- configs/quadrotor_x.xml | 11 ++ include/sensor_gps_plugin.h | 11 ++ models/quadrotor_x/quadrotor_x.xml | 1 + src/jsbsim_bridge.cpp | 1 + src/sensor_gps_plugin.cpp | 59 ++++++++-- systems/px4_default_gps_sensor.xml | 179 +++++++++++++++++++++++++++++ 6 files changed, 250 insertions(+), 12 deletions(-) create mode 100644 systems/px4_default_gps_sensor.xml diff --git a/configs/quadrotor_x.xml b/configs/quadrotor_x.xml index 84161b2..7be5ca2 100644 --- a/configs/quadrotor_x.xml +++ b/configs/quadrotor_x.xml @@ -6,6 +6,17 @@ + px4/gps-fix-type + px4/gps-lat + px4/gps-lon + px4/gps-alt + px4/gps-eph + px4/gps-epv + px4/gps-v-north + px4/gps-v-east + px4/gps-v-down + px4/gps-velocity + px4/gps-satellites-visible diff --git a/include/sensor_gps_plugin.h b/include/sensor_gps_plugin.h index 54f3973..e673839 100644 --- a/include/sensor_gps_plugin.h +++ b/include/sensor_gps_plugin.h @@ -53,4 +53,15 @@ class SensorGpsPlugin : public SensorPlugin { private: SensorData::Gps getGpsFromJSBSim(); + std::string jsb_gps_fix_type = "none"; + std::string jsb_gps_lat = "position/lat-geod-deg"; + std::string jsb_gps_lon = "position/long-gc-deg"; + std::string jsb_gps_alt = "position/h-sl-meters"; + std::string jsb_gps_eph = "none"; + std::string jsb_gps_epv = "none"; + std::string jsb_gps_v_north = "velocities/v-north-fps"; + std::string jsb_gps_v_east = "velocities/v-east-fps"; + std::string jsb_gps_v_down = "velocities/v-down-fps"; + std::string jsb_gps_velocity = "velocities/ned-velocity-mag-fps"; + std::string jsb_gps_satellites = "none"; }; diff --git a/models/quadrotor_x/quadrotor_x.xml b/models/quadrotor_x/quadrotor_x.xml index 1f6fc8b..a491306 100644 --- a/models/quadrotor_x/quadrotor_x.xml +++ b/models/quadrotor_x/quadrotor_x.xml @@ -452,4 +452,5 @@ + diff --git a/src/jsbsim_bridge.cpp b/src/jsbsim_bridge.cpp index c48dbf6..1375c38 100644 --- a/src/jsbsim_bridge.cpp +++ b/src/jsbsim_bridge.cpp @@ -108,6 +108,7 @@ bool JSBSimBridge::SetFdmConfigs(ConfigurationParser &cfg) { } _fdmexec->SetAircraftPath(SGPath(aircraft_path.c_str())); _fdmexec->SetEnginePath(SGPath("Engines")); + _fdmexec->SetSystemsPath(SGPath("systems")); if (!cfg.isHeadless()) { // Check if HEADLESS mode is enabled _fdmexec->SetOutputDirectives(SGPath("data_out/flightgear.xml")); diff --git a/src/sensor_gps_plugin.cpp b/src/sensor_gps_plugin.cpp index 258723b..bb273a6 100644 --- a/src/sensor_gps_plugin.cpp +++ b/src/sensor_gps_plugin.cpp @@ -45,7 +45,19 @@ SensorGpsPlugin::SensorGpsPlugin(JSBSim::FGFDMExec* jsbsim) : SensorPlugin(jsbsi SensorGpsPlugin::~SensorGpsPlugin() {} -void SensorGpsPlugin::setSensorConfigs(const TiXmlElement& configs) {} +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); +} SensorData::Gps SensorGpsPlugin::getData() { double sim_time = _sim_ptr->GetSimTime(); @@ -62,17 +74,40 @@ SensorData::Gps SensorGpsPlugin::getData() { SensorData::Gps SensorGpsPlugin::getGpsFromJSBSim() { SensorData::Gps ret; ret.time_utc_usec = _sim_ptr->GetSimTime() * 1e6; - ret.fix_type = 3; - ret.latitude_deg = _sim_ptr->GetPropertyValue("position/lat-geod-deg") * 1e7; - ret.longitude_deg = _sim_ptr->GetPropertyValue("position/long-gc-deg") * 1e7; - ret.altitude = _sim_ptr->GetPropertyValue("position/h-sl-meters") * 1e7; - ret.eph = 1 * 100; - ret.epv = 2 * 100; - ret.velocity_north = ftToM(_sim_ptr->GetPropertyValue("velocities/v-north-fps")) * 100; - ret.velocity_east = ftToM(_sim_ptr->GetPropertyValue("velocities/v-east-fps")) * 100; - ret.velocity_down = ftToM(_sim_ptr->GetPropertyValue("velocities/v-down-fps")) * 100; - ret.velocity = ftToM(_sim_ptr->GetPropertyValue("velocities/ned-velocity-mag-fps")) * 100; - ret.satellites_visible = 16; + + if(jsb_gps_fix_type == "none") { + ret.fix_type = 3; + } else { + 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") { + ret.eph = 1 * 100; + } else { + ret.eph = _sim_ptr->GetPropertyValue(jsb_gps_eph)*100; + } + + if(jsb_gps_epv == "none") { + ret.epv = 2 * 100; + } else { + ret.epv = _sim_ptr->GetPropertyValue(jsb_gps_epv)*100; + } + + ret.velocity_north = ftToM(_sim_ptr->GetPropertyValue(jsb_gps_v_north)) * 100; + ret.velocity_east = ftToM(_sim_ptr->GetPropertyValue(jsb_gps_v_east)) * 100; + 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") { + ret.satellites_visible = 16; + } else { + ret.satellites_visible = _sim_ptr->GetPropertyValue(jsb_gps_satellites); + } + ret.id = 1; return ret; diff --git a/systems/px4_default_gps_sensor.xml b/systems/px4_default_gps_sensor.xml new file mode 100644 index 0000000..7b92a71 --- /dev/null +++ b/systems/px4_default_gps_sensor.xml @@ -0,0 +1,179 @@ + + + + + px4/gps-eph-static-default + px4/gps-epv-static-default + + px4/gps-fix-type + px4/gps-satellites-visible + + + + position/lat-geod-deg + 0 + 0.00000001 + + + + + + + position/long-gc-deg + 0 + 0.00000001 + + + + + + + position/h-sl-meters + 0 + 0.3 + + + + + + + px4/gps-eph-static-default + 0 + 0.1 + + 24 + 0 + 10000 + + 0 + 0 + 0 + 0 + + + + + + px4/gps-epv-static-default + 0 + 0.1 + + 24 + 0 + 10000 + + 0 + 0 + 0 + 0 + + + + + + velocities/v-north-fps + 0 + 0.05 + + + + + + + velocities/v-east-fps + 0 + 0.05 + + + + + + + velocities/v-down-fps + 0 + 0.05 + + + + + + + velocities/ned-velocity-mag-fps + 0 + 0.05 + + 24 + 0 + 10000 + + 0 + 0 + 0 + 0 + + + \ No newline at end of file From 73638af0788c1c5c86bd665f26c7712264ab7223 Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Thu, 15 Oct 2020 08:41:02 +0200 Subject: [PATCH 2/3] Add support for flying wing vehicle (malolo) (#23) * Add malolo model as submodule * Add malolo configfile for the jsbsim bridge This commit adds the config file for malolo --- .gitmodules | 3 +++ configs/malolo.xml | 43 +++++++++++++++++++++++++++++++++++++++++++ models/ATI-Resolution | 1 + 3 files changed, 47 insertions(+) create mode 100644 configs/malolo.xml create mode 160000 models/ATI-Resolution diff --git a/.gitmodules b/.gitmodules index 107c5aa..94746f6 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,6 @@ [submodule "models/Rascal"] path = models/Rascal url = https://github.com/ThunderFly-aerospace/FlightGear-Rascal.git +[submodule "models/ATI-Resolution"] + path = models/ATI-Resolution + url = https://github.com/FGMEMBERS/ATI-Resolution.git diff --git a/configs/malolo.xml b/configs/malolo.xml new file mode 100644 index 0000000..b35e728 --- /dev/null +++ b/configs/malolo.xml @@ -0,0 +1,43 @@ + + + models/ATI-Resolution + Malolo1 + + + 4560 + + + + + + + + + + + + + + + + 2 + -1 + fcs/rudder-cmd-norm + + + 5 + -1 + fcs/aileron-cmd-norm + + + 7 + -1 + fcs/elevator-cmd-norm + + + 4 + 1 + fcs/throttle-cmd-norm + + + diff --git a/models/ATI-Resolution b/models/ATI-Resolution new file mode 160000 index 0000000..40571cd --- /dev/null +++ b/models/ATI-Resolution @@ -0,0 +1 @@ +Subproject commit 40571cd808a204f4062128daeb6bb6db8c134c48 From 405ea52dd10179d7eab4cba82fd09dc04623af49 Mon Sep 17 00:00:00 2001 From: Matt Vacanti Date: Thu, 15 Oct 2020 16:55:18 -0400 Subject: [PATCH 3/3] Pr merge script (#22) * Enable motor fail parameter * Add demo * Include input_output/FGScript.h * New logic to allow JSBSim script loading OR standard * Remove workflows due to workflow scope * Add motor_0_fail.xml scenario demo * Add motor_0_fail.xml scenario demo * Revert "Remove workflows due to workflow scope" This reverts commit 5aa19ed3 * Enable motor fail parameter * Add demo * Include input_output/FGScript.h * New logic to allow JSBSim script loading OR standard * Remove workflows due to workflow scope * Add motor_0_fail.xml scenario demo * Revert "Remove workflows due to workflow scope" This reverts commit 5aa19ed3 * Merge upstream master * Remove engine fail scenario * Remove debug print --- include/jsbsim_bridge.h | 1 + models/quadrotor_x/quadrotor_x.xml | 3 ++ scenario/motor_0_fail.xml | 28 +++++++++++++++++++ src/jsbsim_bridge.cpp | 44 ++++++++++++++++++++++-------- 4 files changed, 64 insertions(+), 12 deletions(-) create mode 100644 scenario/motor_0_fail.xml diff --git a/include/jsbsim_bridge.h b/include/jsbsim_bridge.h index d5640b9..c721911 100644 --- a/include/jsbsim_bridge.h +++ b/include/jsbsim_bridge.h @@ -52,6 +52,7 @@ #include #include +#include #include #include diff --git a/models/quadrotor_x/quadrotor_x.xml b/models/quadrotor_x/quadrotor_x.xml index a491306..c03787a 100644 --- a/models/quadrotor_x/quadrotor_x.xml +++ b/models/quadrotor_x/quadrotor_x.xml @@ -298,6 +298,8 @@ fcs/lift_bypass[2] fcs/lift_bypass[3] + fcs/motor_health[0] + @@ -312,6 +314,7 @@ 1 + fcs/motor_health[0] fcs/lift_bypass[0] diff --git a/scenario/motor_0_fail.xml b/scenario/motor_0_fail.xml new file mode 100644 index 0000000..ddb03ec --- /dev/null +++ b/scenario/motor_0_fail.xml @@ -0,0 +1,28 @@ + + + + + + Template for generic motor fail test. + + + + + + + + Demonstrate Motor Failure Trigger + + position/h-sl-meters ge 425 + + + + simulation/sim-time-sec + + + + + + \ No newline at end of file diff --git a/src/jsbsim_bridge.cpp b/src/jsbsim_bridge.cpp index 1375c38..7017f70 100644 --- a/src/jsbsim_bridge.cpp +++ b/src/jsbsim_bridge.cpp @@ -100,34 +100,54 @@ bool JSBSimBridge::SetFdmConfigs(ConfigurationParser &cfg) { _fdmexec->SetRootDir(SGPath(JSBSIM_ROOT_DIR)); + // Define aircraft path configuration std::string aircraft_path; if (config && CheckConfigElement(*config, "aircraft_directory")) { GetConfigElement(*config, "aircraft_directory", aircraft_path); } else { aircraft_path = "models/" + cfg.getModelName(); } - _fdmexec->SetAircraftPath(SGPath(aircraft_path.c_str())); - _fdmexec->SetEnginePath(SGPath("Engines")); - _fdmexec->SetSystemsPath(SGPath("systems")); - - if (!cfg.isHeadless()) { // Check if HEADLESS mode is enabled - _fdmexec->SetOutputDirectives(SGPath("data_out/flightgear.xml")); - } + // Define aircraft model name configuration std::string aircraft_model; if (config && CheckConfigElement(*config, "aircraft_model")) { GetConfigElement(*config, "aircraft_model", aircraft_model); } else { aircraft_model = cfg.getModelName(); } - _fdmexec->LoadModel(aircraft_model.c_str(), false); - // Load Initial Conditions - JSBSim::FGInitialCondition *initial_condition = _fdmexec->GetIC(); + // Check if HEADLESS mode is enabled + if (!cfg.isHeadless()) { + _fdmexec->SetOutputDirectives(SGPath("data_out/flightgear.xml")); + } + + // Define JSBSim initialization script (scene or world) SGPath init_script_path = SGPath::fromLocal8Bit((cfg.getInitScriptPath()).c_str()); - initial_condition->Load(SGPath(init_script_path), false); - return true; + // Set JSBSim paths + _fdmexec->SetEnginePath(SGPath("Engines")); + _fdmexec->SetSystemsPath(SGPath("systems")); + + // Select & Load JSBSim Run Configuration + 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; + return false; + } else { + _fdmexec->SetAircraftPath(SGPath("models/")); + GetConfigElement(*config, "jsb_script", jsb_script); + _fdmexec->LoadScript(SGPath("scenario/" + jsb_script), _dt, SGPath(init_script_path)); + return true; + } + } else { + _fdmexec->SetAircraftPath(SGPath(aircraft_path.c_str())); + _fdmexec->LoadModel(aircraft_model.c_str(), false); + JSBSim::FGInitialCondition *initial_condition = _fdmexec->GetIC(); + initial_condition->Load(SGPath(init_script_path), false); + return true; + } } bool JSBSimBridge::SetMavlinkInterfaceConfigs(std::unique_ptr &interface, TiXmlHandle &config) {