Trying to debug linearization.

scicoslab/sci_gateway/sci_jsbsimComm.cpp

@@ -28,20 +28,20 @@ class JSBSimComm
 {
 public:
     JSBSimComm(std::string aircraftPath, std::string enginePath,
-			std::string systemsPath, std::string modelName, 
-			double * x0, double * u0) : m_prop(), m_fdm(&m_prop), ss(m_fdm)
+               std::string systemsPath, std::string modelName,
+               double * x0, double * u0) : prop(), fdm(&prop), ss(fdm)
     {
         using namespace JSBSim;
         std::cout << "initializing" << std::endl;
-        m_fdm.LoadModel(aircraftPath,enginePath,systemsPath,modelName);
-        m_fdm.SetDebugLevel(0);
-        m_fdm.Setdt(1./120.);
+        fdm.LoadModel(aircraftPath,enginePath,systemsPath,modelName);
+        fdm.SetDebugLevel(0);
+        fdm.Setdt(1./120.);
 
         // defaults
         bool variablePropPitch = false;
 
-		// get propulsion pointer to determine type/ etc.
-        FGEngine * engine0 = m_fdm.GetPropulsion()->GetEngine(0);
+        // get propulsion pointer to determine type/ etc.
+        FGEngine * engine0 = fdm.GetPropulsion()->GetEngine(0);
         FGThruster * thruster0 = engine0->GetThruster();
 
         // state space
@@ -86,20 +86,19 @@ public:
         // state feedback
         ss.y = ss.x;
 
-		// set initial conditions
-		ss.x.set(x0);
-		ss.u.set(u0);
+        // set initial conditions
+        ss.x.set(x0);
+        ss.u.set(u0);
 
-		// make sure it's steady, shouldn't be needed if state is full
-		m_fdm.GetPropulsion()->GetSteadyState();
+        // make sure it's steady, shouldn't be needed if state is full
+        fdm.GetPropulsion()->GetSteadyState();
+    }
+    virtual ~JSBSimComm()
+    {
     }
-	virtual ~JSBSimComm()
-	{
-	}
-private:
-	JSBSim::FGPropertyManager m_prop;
-    JSBSim::FGFDMExec m_fdm;
 public:
+    JSBSim::FGPropertyManager prop;
+    JSBSim::FGFDMExec fdm;
     JSBSim::FGStateSpace ss;
 };
 
@@ -112,8 +111,8 @@ extern "C"
 
     void sci_jsbsimComm(scicos_block *block, scicos::enumScicosFlags flag)
     {
-		static JSBSimComm * comm = NULL;
-		static JSBSim::FGPropertyManager propManager;
+        static JSBSimComm * comm = NULL;
+        static JSBSim::FGPropertyManager propManager;
 
         // data
         double *u=(double*)GetInPortPtrs(block,1);
@@ -125,26 +124,26 @@ extern "C"
         //handle flags
         if (flag==scicos::initialize || flag==scicos::reinitialize)
         {
-			std::string jsbsimPath = getenv("JSBSim");
-			std::string aircraftPath = jsbsimPath + "/aircraft";
-			std::string enginePath = jsbsimPath + "/engine";
-			std::string systemsPath = jsbsimPath + "/systems";
-			std::string modelName = "f16";
-			if (comm)
-			{
-				std::cout << "deleting comm" << std::endl;
-				delete comm;
-				comm = NULL;
-			}
-			comm = new JSBSimComm(aircraftPath,enginePath,systemsPath,modelName,x,u);
+            std::string jsbsimPath = getenv("JSBSim");
+            std::string aircraftPath = jsbsimPath + "/aircraft";
+            std::string enginePath = jsbsimPath + "/engine";
+            std::string systemsPath = jsbsimPath + "/systems";
+            std::string modelName = "f16";
+            if (comm)
+            {
+                std::cout << "deleting comm" << std::endl;
+                delete comm;
+                comm = NULL;
+            }
+            comm = new JSBSimComm(aircraftPath,enginePath,systemsPath,modelName,x,u);
         }
         else if (flag==scicos::terminate)
         {
-			if (comm)
-			{
-				delete comm;
-				comm = NULL;
-			}
+            if (comm)
+            {
+                delete comm;
+                comm = NULL;
+            }
         }
         else if (flag==scicos::updateState)
         {
@@ -153,6 +152,7 @@ extern "C"
         }
         else if (flag==scicos::computeDeriv)
         {
+            comm->fdm.RunIC();
             comm->ss.x.getDeriv(xd);
         }
         else if (flag==scicos::computeOutput)

scicoslab/scripts/fgrun.sh

@@ -5,7 +5,7 @@ fgfs \
 --fdm=external \
 --aircraft=c172p \
 --geometry=400x300 \
---native-fdm=socket,in,30,,5500,udp \
+--native-fdm=socket,in,1,,5500,udp \
 --vc=30 \
 --altitude=1000 \
 --heading=90 \

src/FGComm.cpp

@@ -25,6 +25,7 @@ int main (int argc, char const* argv[])
     using namespace JSBSim;
     FGFDMExec fdm;
     fdm.LoadModel("../aircraft","../engine","../systems","c172p");
+    fdm.Setdt(1./120.);
 
     // Turn on propulsion system
     fdm.GetPropulsion()->InitRunning(-1);
@@ -112,10 +113,11 @@ int main (int argc, char const* argv[])
     {
         JSBSim2FlightGearNetFDM(fdm,netFdm,true);
         socket.Send((char *)(& netFdm), sizeof(netFdm));
-        fdm.Run();
-        //std::cout << "paused" << std::endl;
-        //std::cin.get();
-        usleep(1.0e6/150.0);
+        for (int i=0;i<4;i++)
+        {
+            fdm.Run();
+            usleep(1.0e6*fdm.GetDeltaT());
+        }
     }
     return 0;
 }

src/Trim.cpp

@@ -43,7 +43,6 @@ int main (int argc, char const* argv[])
 
     // variables
     FGFDMExec fdm;
-    fdm.SetDebugLevel(0); // hide messages
     fdm.Setdt(1./120);
     FGTrimmer::Constraints constraints;
 

src/math/FGStateSpace.cpp

@@ -32,7 +32,7 @@ void FGStateSpace::linearize(
     std::vector< std::vector<double> > & C,
     std::vector< std::vector<double> > & D)
 {
-    double h = 1e-3;
+    double h = 1e-10;
 
     // A, d(x)/dx
     numericalJacobian(A,x,x,x0,x0,h,true);

src/math/FGStateSpace.h

@@ -55,6 +55,8 @@ public:
             // by default should calculate using finite difference approx
             std::vector<double> x0 = m_stateSpace->x.get();
             double f0 = get();
+            double dt0 = m_fdm->GetDeltaT();
+            m_fdm->Setdt(1e-7);
             m_fdm->Run();
             double f1 = get();
             m_stateSpace->x.set(x0);
@@ -68,6 +70,7 @@ public:
                           << "\tdf/dt: " << (f1-f0)/m_fdm->GetDeltaT()
                           << std::fixed << std::endl;
             }
+            m_fdm->Setdt(dt0); // restore original value
             return (f1-f0)/m_fdm->GetDeltaT();
         };
         void setStateSpace(FGStateSpace * stateSpace)