Merge pull request #10 from PX4/pr-upstream-merge

Upstream merge for latest master on Auterion/px4-jsbsim-bridge
PX4 · Oct 16, 2020 · 9f46d55 · 9f46d55
2 parents 7df4fd9 + 405ea52
commit 9f46d55
Show file tree

Hide file tree

Showing 11 changed files with 360 additions and 23 deletions.
diff --git 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
@@ -0,0 +1,43 @@
+<model name="rascal">
+    <jsbsimbridge>
+        <aircraft_directory>models/ATI-Resolution</aircraft_directory>
+        <aircraft_model>Malolo1</aircraft_model>
+    </jsbsimbridge>
+    <mavlink_interface>
+        <tcp_port>4560</tcp_port>
+    </mavlink_interface>
+    <sensors>
+        <imu>
+        </imu>
+        <gps>
+        </gps>
+        <barometer>
+        </barometer>
+        <magnetometer>
+        </magnetometer>
+        <airspeed>
+        </airspeed>
+    </sensors>
+    <actuators>
+        <channel name="rudder">
+            <index>2</index>
+            <scale>-1</scale>
+            <property>fcs/rudder-cmd-norm</property>
+        </channel>
+        <channel name="aileron">
+            <index>5</index>
+            <scale>-1</scale>
+            <property>fcs/aileron-cmd-norm</property>
+        </channel>
+        <channel name="elevator">
+            <index>7</index>
+            <scale>-1</scale>
+            <property>fcs/elevator-cmd-norm</property>
+        </channel>
+        <channel name="throttle">
+            <index>4</index>
+            <scale>1</scale>
+            <property>fcs/throttle-cmd-norm</property>
+        </channel>
+    </actuators>
+</model>
diff --git a/configs/quadrotor_x.xml b/configs/quadrotor_x.xml
@@ -6,6 +6,17 @@
         <imu>
         </imu>
         <gps>
+            <jsb_gps_fix_type>px4/gps-fix-type</jsb_gps_fix_type>
+            <jsb_gps_lat>px4/gps-lat</jsb_gps_lat>
+            <jsb_gps_lon>px4/gps-lon</jsb_gps_lon>
+            <jsb_gps_alt>px4/gps-alt</jsb_gps_alt>
+            <jsb_gps_eph>px4/gps-eph</jsb_gps_eph>
+            <jsb_gps_epv>px4/gps-epv</jsb_gps_epv>
+            <jsb_gps_v_north>px4/gps-v-north</jsb_gps_v_north>
+            <jsb_gps_v_east>px4/gps-v-east</jsb_gps_v_east>
+            <jsb_gps_v_down>px4/gps-v-down</jsb_gps_v_down>
+            <jsb_gps_velocity>px4/gps-velocity</jsb_gps_velocity>
+            <jsb_gps_satellites>px4/gps-satellites-visible</jsb_gps_satellites>
         </gps>
         <barometer>
         </barometer>

diff --git a/include/jsbsim_bridge.h b/include/jsbsim_bridge.h
@@ -52,6 +52,7 @@
 
 #include <FGFDMExec.h>
 #include <initialization/FGInitialCondition.h>
+#include <input_output/FGScript.h>
 
 #include <tinyxml.h>
 #include <chrono>

diff --git 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/ATI-Resolution b/models/ATI-Resolution
diff --git a/models/quadrotor_x/quadrotor_x.xml b/models/quadrotor_x/quadrotor_x.xml
@@ -298,6 +298,8 @@
      <property value="0.0">fcs/lift_bypass[2]</property>
      <property value="0.0">fcs/lift_bypass[3]</property>
 
+     <property value="1.0">fcs/motor_health[0]</property>
+
       <force name="MOTOR_0_LIFT" frame="BODY">
         <function>
           <sum>
@@ -312,6 +314,7 @@
                 </tableData>
             </table>
             <value>1</value> <!--Set to 0 remove PX4 control outputs to JSBSim -->
+          <property>fcs/motor_health[0]</property>
           </product>
           <property>fcs/lift_bypass[0]</property>
           </sum>
@@ -452,4 +455,5 @@
       </function>
     </axis>
   </aerodynamics>
+  <system file="px4_default_gps_sensor"/>
 </fdm_config>
diff --git a/scenario/motor_0_fail.xml b/scenario/motor_0_fail.xml
@@ -0,0 +1,28 @@
+<?xml version="1.0" encoding="utf-8"?>
+<?xml-stylesheet type="text/xsl" href="http://jsbsim.sf.net/JSBSimScript.xsl"?>
+<runscript xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
+    xsi:noNamespaceSchemaLocation="http://jsbsim.sf.net/JSBSimScript.xsd"
+    name="Testing Template">
+
+  <description>
+      Template for generic motor fail test.
+  </description>
+
+  <use aircraft="quadrotor_x"/>
+
+  <run start="0" end="10000000" dt="0.004">
+
+    <event name="Fail Motor 0">
+      <description> Demonstrate Motor Failure Trigger </description>
+      <condition>
+          position/h-sl-meters ge 425
+      </condition>
+      <set name="fcs/motor_health[0]" value="0.0" action="FG_RAMP" tc="5.0"/>
+      <notify>
+        <property>simulation/sim-time-sec</property>
+      </notify>
+    </event>
+
+  </run>
+
+</runscript>
diff --git a/src/jsbsim_bridge.cpp b/src/jsbsim_bridge.cpp
@@ -100,33 +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<std::string>(*config, "aircraft_directory", aircraft_path);
   } else {
     aircraft_path = "models/" + cfg.getModelName();
   }
-  _fdmexec->SetAircraftPath(SGPath(aircraft_path.c_str()));
-  _fdmexec->SetEnginePath(SGPath("Engines"));
-
-  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<std::string>(*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<std::string>(*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<MavlinkInterface> &interface, TiXmlHandle &config) {

diff --git 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<std::string>(configs, "jsb_gps_fix_type", jsb_gps_fix_type);
+    GetConfigElement<std::string>(configs, "jsb_gps_lat", jsb_gps_lat);
+    GetConfigElement<std::string>(configs, "jsb_gps_lon", jsb_gps_lon);
+    GetConfigElement<std::string>(configs, "jsb_gps_alt", jsb_gps_alt);
+    GetConfigElement<std::string>(configs, "jsb_gps_eph", jsb_gps_eph);
+    GetConfigElement<std::string>(configs, "jsb_gps_epv", jsb_gps_epv);
+    GetConfigElement<std::string>(configs, "jsb_gps_v_north", jsb_gps_v_north);
+    GetConfigElement<std::string>(configs, "jsb_gps_v_east", jsb_gps_v_east);
+    GetConfigElement<std::string>(configs, "jsb_gps_v_down", jsb_gps_v_down);
+    GetConfigElement<std::string>(configs, "jsb_gps_velocity", jsb_gps_velocity);
+    GetConfigElement<std::string>(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;