From 6026278b0921a7fb79f29971eadf448fce696adf Mon Sep 17 00:00:00 2001
From: Stephen Dwyer <scdwyer@ualberta.ca>
Date: Mon, 21 Feb 2011 17:42:57 -0700
Subject: [PATCH] modified JSBSim fw sim to easily use either defaults or an
 initialization file, launch velocity can now be specified in SIMU section,
 added example intialization file, modified jsbsim.xml example airframe to use
 init file and updated to include firmware section instead of old makefile
 section

---
 conf/airframes/jsbsim.xml             | 44 +++++++++++++++++-------
 conf/simulator/Malolo1/Malolo1-IC.xml | 49 +++++++++++++++++++++++++++
 sw/simulator/sim_ac_fw.c              |  8 +++--
 sw/simulator/sim_ac_jsbsim.c          |  2 +-
 4 files changed, 87 insertions(+), 16 deletions(-)
 create mode 100644 conf/simulator/Malolo1/Malolo1-IC.xml

diff --git a/conf/airframes/jsbsim.xml b/conf/airframes/jsbsim.xml
index 67fe2f0a1a..18dc8cc3ca 100644
--- a/conf/airframes/jsbsim.xml
+++ b/conf/airframes/jsbsim.xml
@@ -191,25 +191,43 @@
 
   <section name="SIMU">
     <define name="JSBSIM_MODEL" value="&quot;Malolo1&quot;"/>
+    <define name="JSBSIM_INIT"	value="&quot;Malolo1-IC&quot;"/>
+    <define name="JSBSIM_LAUNCHSPEED"	value="15.0"/>
     <define name="JSBSIM_IR_ROLL_NEUTRAL" value="RadOfDeg(0.)"/>
     <define name="JSBSIM_IR_PITCH_NEUTRAL" value="RadOfDeg(0.)"/>
   </section>
 
- <makefile>
-#### Config for SITL simulation
-include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
+  <firmware name="fixedwing">
+    <target name="sim" 			board="pc" />
+    <target name="jsbsim" 	    board="pc"/>
+    <target name="ap" 			board="tiny_2.1"/>
 
-sim.CFLAGS += -DBOARD_CONFIG=\"boards/tiny_sim.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
+    <define name="AGR_CLIMB" />
+    <define name="LOITER_TRIM" />
+    <define name="ALT_KALMAN" />
+
+    <subsystem name="radio_control" type="ppm"/>
+
+    <!-- Communication -->
+    <subsystem name="telemetry" 	type="transparent">
+      <configure name="MODEM_BAUD" 		value="B9600"/>
+    </subsystem>
+
+    <subsystem name="control"/>
+    <!-- Sensors -->
+    <subsystem name="gyro"          type="roll"/>
+    <subsystem name="attitude" 		type="infrared"/>
+    <subsystem name="gps" 		    type="ublox_lea4p"/>
+    <subsystem name="navigation"/>
+
+  </firmware>
 
 
-#### Config for SITL simulation with JSBSim
-SRC_FIRMWARE = firmwares/fixedwing
-include $(PAPARAZZI_SRC)/conf/autopilot/sitl_jsbsim.makefile
+  <firmware name="setup">
+    <target name="tunnel"           board="tiny_2.1" />
+    <target name="usb_tunnel_0"     board="tiny_2.1" />
+    <target name="usb_tunnel_1"     board="tiny_2.1" />
+    <target name="setup_actuators"  board="tiny_2.1" />
+  </firmware>
 
-jsbsim.CFLAGS += -DBOARD_CONFIG=\"boards/tiny_sim.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-jsbsim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
-jsbsim.srcs += $(SIMDIR)/sim_ac_fw.c
-
-  </makefile>
 </airframe>
diff --git a/conf/simulator/Malolo1/Malolo1-IC.xml b/conf/simulator/Malolo1/Malolo1-IC.xml
new file mode 100644
index 0000000000..89c46bef55
--- /dev/null
+++ b/conf/simulator/Malolo1/Malolo1-IC.xml
@@ -0,0 +1,49 @@
+<?xml version="1.0"?>
+
+  <!--
+	This file sets up the aircraft initial conditions.
+
+	Allowable units are:
+		Length:			M IN FT
+		Angles:			DEG RAD
+		Velocity:		M/S FT/SEC KTS
+		Area:			M2 FT2
+		Volume:			CC LTR M3 IN3 FT3
+		Mass/Weight:		KG LBS
+		Moments of Inertia: KG*M2 SLUG*FT2
+		Torque:			N*M FT*LBS
+		Force:			N LBS
+		Spring Force:	N/M LBS/FT
+		Damping Force:	N/M/SEC LBS/FT/SEC
+		Power:			WATTS HP
+		Pressure:		PA ATM PSI PSF INHG
+
+	Some example initial conditions are given below. Not all are needed.
+
+	Note that the launch condition body aligned forward velocity is given in sim_ac_fw.c
+	and can be defined in the SIMU section of the airframe file. See jsbsim.xml for example.
+
+	Other ICs are allowable, see JSBSim source for more info:
+		FGInitialCondition::Load	()
+		FGInitialCondition::Load_v1()
+		FGInitialCondition::Load_v2()
+
+  -->
+
+<initialize name="beforelaunch">
+<!-- Location -->
+  <latitude 		unit="DEG">		43.46223	</latitude> <!-- Muret, FR -->
+  <longitude 	unit="DEG"> 		1.27289 	</longitude> <!-- Muret, FR -->
+  <elevation		unit="M"	>		186.0		</elevation> <!-- terrain elevation -->
+  <altitudeAGL	unit="M"> 		2.0 		</altitudeAGL> <!-- altitude above ground level -->
+<!-- Orientation -->
+  <psi 			unit="DEG">		0.0 		</psi> <!-- heading (yaw) -->
+  <theta 		unit="DEG">		0.0 		</theta> <!-- pitch -->
+  <phi 			unit="DEG">		0.0 		</phi> <!-- roll -->
+<!-- Wind -->
+  <winddir 		unit="DEG">		0.0 		</winddir> <!-- positive velocity direction (0.0 is wind from south) -->
+  <vwind 		unit="KTS">		20.0 		</vwind>
+<!--Do not modify-->
+  <vground		unit="KTS">		0.0		</vground> <!-- ground speed -->
+  <running>						-1		</running> <!-- set all engines running -->
+</initialize>
diff --git a/sw/simulator/sim_ac_fw.c b/sw/simulator/sim_ac_fw.c
index fac1a433b5..e9dde43ab0 100644
--- a/sw/simulator/sim_ac_fw.c
+++ b/sw/simulator/sim_ac_fw.c
@@ -127,6 +127,9 @@ static inline double normalize_from_pprz(int command) {
 void copy_inputs_to_jsbsim(FGFDMExec* FDMExec) {
   static double throttle_slewed = 0.;
   static double th = 0.;
+#ifndef JSBSIM_LAUNCHSPEED
+#define JSBSIM_LAUNCHSPEED 20.0 //launch speed in m/s aligned with airframe body forward
+#endif
   if (run_model) th += 0.01;
   if (th >= 1) th = 1;
   // detect launch
@@ -134,8 +137,9 @@ void copy_inputs_to_jsbsim(FGFDMExec* FDMExec) {
     run_model = true;
     //set_value(FDMExec, "propulsion/set-running", 1);
     // set initial speed
-    FDMExec->GetIC()->SetAltitudeAGLFtIC(5.0 / FT2M);
-    FDMExec->GetIC()->SetVgroundFpsIC(20./FT2M);
+    //FDMExec->GetIC()->SetAltitudeAGLFtIC(5.0 / FT2M);
+    //FDMExec->GetIC()->SetVgroundFpsIC(20./FT2M);
+    FDMExec->GetIC()->SetUBodyFpsIC( JSBSIM_LAUNCHSPEED / FT2M);
     FDMExec->RunIC();
     th = 0.;
   }
diff --git a/sw/simulator/sim_ac_jsbsim.c b/sw/simulator/sim_ac_jsbsim.c
index 3806af2909..55f66a17a8 100644
--- a/sw/simulator/sim_ac_jsbsim.c
+++ b/sw/simulator/sim_ac_jsbsim.c
@@ -253,7 +253,7 @@ void jsbsim_init(void) {
       IC->SetLatitudeDegIC(NAV_LAT0 / 1e7);
       IC->SetLongitudeDegIC(NAV_LON0 / 1e7);
           
-      IC->SetAltitudeASLFtIC(GROUND_ALT / FT2M);
+      IC->SetAltitudeASLFtIC((GROUND_ALT + 2.0) / FT2M);
       IC->SetTerrainElevationFtIC(GROUND_ALT / FT2M);
       IC->SetPsiDegIC(QFU);
       IC->SetVgroundFpsIC(0.);