Added scicoslab files.

scicoslab/.gitignore

+Makelib
+*.bin
+*.so
+*.a
+*.lo
+*.la
+lib
+names
+src/loader.sce

scicoslab/Makefile.am

+EXTRA_DIST=
+
+scicoslabdir=$(pkgdatadir)/scicoslab
+scicoslab_DATA= \
+	builder.sce \
+	loader.sce
+EXTRA_DIST+= $(scicoslab_DATA)
+
+scicoslabdemosdir=$(scicoslabdir)/demos
+scicoslabdemos_DATA= \
+	demos/loader.sce \
+	demos/autopilotBackside.cosf \
+	demos/autopilotBacksideOld.cosf \
+	demos/autopilotHybrid.cosf \
+	demos/invertedPendulum.cosf \
+	demos/invertedPendulum.sce \
+	demos/jsbsimDynamics.sce \
+	demos/jsbsimPorts.cosf \
+	demos/loader.sce \
+	demos/trimTest.cosf
+
+EXTRA_DIST+= $(scicoslabdemos_DATA)
+
+scicoslabincludesdir=$(scicoslabdir)/includes
+scicoslabincludes_DATA= \
+	includes/definitions.hpp
+EXTRA_DIST+= $(scicoslabincludes_DATA)
+
+scicoslabsci_gatewaydir=$(scicoslabdir)/sci_gateway
+scicoslabsci_gateway_DATA= \
+	sci_gateway/builder.sce \
+	sci_gateway/sci_flightGearComm.cpp
+EXTRA_DIST+= $(scicoslabsci_gateway_DATA)
+
+scicoslabscicosdir=$(scicoslabdir)/scicos
+scicoslabscicos_DATA= \
+	scicos/builder.sce \
+	scicos/loader.sce
+EXTRA_DIST+= $(scicoslabscicos_DATA)
+
+scicoslabscicosoooarkdir=$(scicoslabscicosdir)/oooark
+scicoslabscicosoooark_DATA= \
+	scicos/oooark/flightGearComm.sci
+EXTRA_DIST+= $(scicoslabscicosoooark_DATA)
+
+scicoslabscriptsdir=$(scicoslabdir)/scripts
+scicoslabscripts_DATA= \
+	scripts/fgrun.sh
+EXTRA_DIST+= $(scicoslabscripts_DATA)

scicoslab/builder.sce

+mode(-1);
+
+JSBSimIncludeDir=getenv("JSBSim")+'/src';
+JSBSimLibDir=getenv("JSBSim")+'/src/.libs';
+
+// check version
+ierr = execstr("getversion(""scilab"")", "errcatch");
+if ierr == 0 then
+    disp("JSBSim doesn''t work with Scilab >= 5. Please use ScicosLab.");
+    abort;
+end
+
+// build subdirectories
+CurrentDirectory = pwd();
+mainpathB = get_absolute_file_path("builder.sce");
+chdir(mainpathB);
+if isdir("src") then
+    chdir("src");
+    exec("builder.sce");
+    chdir("..");
+end
+if isdir("sci_gateway") then
+    chdir("sci_gateway");
+    exec("builder.sce");
+    chdir("..");
+end
+if isdir("macros") then
+    chdir("macros");
+    exec("builder.sce");
+    chdir("..");
+end
+if ~MSDOS & isdir("scicos") then
+    chdir("scicos");
+	exec("builder.sce");
+    chdir("..");
+end
+chdir(CurrentDirectory);
+
+// clean workspace
+clear mainpathB get_absolute_file_path isdir CurrentDirectory ierr

scicoslab/includes/definitions.hpp

+/*
+ * definitions.hpp
+ * Copyright (C) James Goppert 2010 <jgoppert@users.sourceforge.net>
+ *
+ * definitions.hpp is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * definitions.hpp is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef definitions_HPP
+#define definitions_HPP
+
+namespace scicos
+{
+
+enum enumScicosFlags
+{
+    computeDeriv=0,
+    computeOutput=1,
+    updateState=2,
+    outputTimeDelays=3,
+    initialize=4,
+    terminate=5,
+    reinitialize=6,
+    internal=7,
+    computeZeroCrossSurfsSetModes=8,
+    computeZeroCrossSurfs=9
+};
+
+}
+
+#endif
+
+// vim:ts=4:sw=4

scicoslab/loader.sce

+mode(-1);
+
+CurrentDirectory = pwd();
+mainpathL = get_absolute_file_path("loader.sce");
+chdir(mainpathL);
+if isdir("sci_gateway") then
+    chdir("sci_gateway");
+    exec("loader.sce");
+    chdir("..");
+end
+if isdir("macros") then
+    chdir("macros");
+    exec("loader.sce");
+    chdir("..");
+end
+if isdir("src") then
+    chdir("src");
+    exec("loader.sce");
+    chdir("..");
+end
+if isdir("help") then
+    chdir("help");
+    exec("loader.sce");
+    chdir("..");
+end
+if isdir("scicos") then
+    chdir("scicos");
+    exec("loader.sce");
+    chdir("..");
+end
+if isdir("demos") then
+    chdir("demos");
+    exec("loader.sce");
+    chdir("..");
+end
+chdir(CurrentDirectory);
+clear mainpathL get_absolute_file_path isdir get_file_path functions CurrentDirectory

scicoslab/macros/builder.sce

+mode(-1);
+genlib('lib_oooarkMacros',pwd());

scicoslab/macros/steadycos2.sci

+function [X,U,Y,XP]=steadycos2(scs_m,X,U,Y,Indx,Indu,Indy,Indxp,param)
+// steadycos2
+// Finds an equilibrium state of a general 
+// dynamical system described by a scicos diagram
+// Same as standard steadycos but takes the optimization method as a
+// parameter.
+
+// CALLING SEQUENCE
+//
+// [X,U,Y,XP]=steadycos(scs_m,X,U,Y,Indx,Indu,Indy [,Indxp [,param ] ])
+//
+// scs_m: a Scicos data structure
+// X: column vector. Continuous state. Can be set to [] if zero.
+// U: column vector. Input. Can be set to [] if zero.
+// Y: column vector. Output. Can be set to [] if zero.
+// Indx: index of entries of X that are not fixed. If all can vary, set to 1:$
+// Indu: index of entries of U that are not fixed. If all can vary, set to 1:$
+// Indy: index of entries of Y that are not fixed. If all can vary, set to 1:$
+// Indxp: index of entries of XP (derivative of x) that need not be zero. 
+//        If all can vary, set to 1:$. Default [].
+// param: list with three elements (default list(1.d-6,0,'qn'))
+//   param(1): scalar. Perturbation level for linearization; the following variation is used
+//             del([x;u])_i = param(1)+param(1)*1d-4*abs([x;u])_i
+//   param(2): scalar. Time t.
+//   param(3): method, 'qn': quasi netwon (default), 'nd' : nelder meade, ..
+//   param(4): max func evals
+//   param(5): max interations
+
+load SCI/macros/scicos/lib
+
+[lhs,rhs]=argn(0)
+IN=[];OUT=[];
+
+[ierr,scicos_ver,scs_m]=update_version(scs_m)
+if ierr<>0 then
+  message("Can''t convert old diagram (problem in version)")
+  return
+end
+
+// //check version
+// current_version = get_scicos_version()
+// scicos_ver = find_scicos_version(scs_m)
+// 
+// //do version
+// if scicos_ver<>current_version then
+//   ierr=execstr('scs_m=do_version(scs_m,scicos_ver)','errcatch')
+//   if ierr<>0 then
+//     error('Can''t convert old diagram (problem in version)')
+//     return
+//   end
+// end
+
+if rhs==7 then
+  Indxp=[ ];param=list(1.d-6,0,'qn',100,100)
+elseif rhs==8 then
+  param=list(1.d-6,0,'qn',100,100)
+elseif rhs==9 then
+else
+  error('wrong number of arguments. 7, 8 or 9 expected.')
+end
+
+for i=1:length(scs_m.objs)
+  if typeof(scs_m.objs(i))=='Block' then  
+    if scs_m.objs(i).gui=='IN_f' then
+      scs_m.objs(i).gui='INPUTPORT';
+      IN=[IN scs_m.objs(i).model.ipar]
+    elseif scs_m.objs(i).gui=='OUT_f' then
+      scs_m.objs(i).gui='OUTPUTPORT';
+      OUT=[OUT  scs_m.objs(i).model.ipar]
+    end
+  end
+end
+IN=-sort(-IN);
+if or(IN<>[1:size(IN,'*')]) then 
+  error('Input ports are not numbered properly.')
+end
+OUT=-sort(-OUT);
+if or(OUT<>[1:size(OUT,'*')]) then 
+  error('Output ports are not numbered properly.')
+end
+//load scicos lib
+load('SCI/macros/scicos/lib')
+//compile scs_m
+[bllst,connectmat,clkconnect,cor,corinv,ok]=c_pass1(scs_m);
+if ~ok then
+  error('Diagram does not compile in pass 1');
+end
+%cpr=c_pass2(bllst,connectmat,clkconnect,cor,corinv,'silent');
+if %cpr==list() then 
+  ok=%f,
+end
+if ~ok then
+  error('Diagram does not compile in pass 2');
+end 
+sim=%cpr.sim;state=%cpr.state;
+//
+inplnk=sim.inplnk;inpptr=sim.inpptr;
+outlnk=sim.outlnk;outptr=sim.outptr;ipptr=sim.ipptr;
+
+ki=[];ko=[];nyptr=1;
+for kfun=1:length(sim.funs)
+  if sim.funs(kfun)=='output' then
+    sim.funs(kfun)='bidon'
+    ko=[ko;[kfun,sim.ipar(ipptr(kfun))]];
+
+  elseif sim.funs(kfun)=='input' then
+    sim.funs(kfun)='bidon'
+    ki=[ki;[kfun,sim.ipar(ipptr(kfun))]];
+    
+  end
+end
+[junk,ind]=sort(-ko(:,2));ko=ko(ind,1);
+[junk,ind]=sort(-ki(:,2));ki=ki(ind,1);
+
+pointo=[];
+for k=ko' 
+  pointo=[pointo;inplnk(inpptr(k))]
+end
+pointi=[];
+for k=ki'
+  pointi=[pointi;outlnk(outptr(k))]
+end
+nx=size(state.x,'*');
+nu=0; for k=pointi', nu=nu+size(state.outtb(k),'*'), end
+ny=0; for k=pointo', ny=ny+size(state.outtb(k),'*'), end
+
+if X==[] then X=zeros(nx,1);end
+if Y==[] then Y=zeros(ny,1);end
+if U==[] then U=zeros(nu,1);end
+if param(1)==0 then param(1)=1.d-6;end
+t=param(2)
+optimMethod=param(3)
+nap=param(4);
+iter=param(5);
+
+ux0=[U(Indu);X(Indx)];
+sindu=size(U(Indu),'*');sindx=size(X(Indx),'*');
+[err,uxopt,gopt]=optim(cost,ux0,optimMethod,'ar',nap,iter)
+U(Indu)=uxopt(1:sindu);
+X(Indx)=uxopt(sindu+1:sindx+sindu);
+state.x=X;
+Uind=1
+for k=pointi'
+ state.outtb(k)=matrix(U(Uind:Uind+size(state.outtb(k),'*')-1),size(state.outtb(k)));
+ Uind=size(state.outtb(k),'*')+1;
+end
+[state,t]=scicosim(state,t,t,sim,'linear',[.1,.1,.1,.1]);
+XP=state.x;
+Yind=1
+for k=pointo'
+ Y(Yind:Yind+size(state.outtb(k),'*')-1)=state.outtb(k)(:);
+ Yind=size(state.outtb(k),'*')+Yind
+end
+endfunction
+
+function [f,g,ind]=cost(ux,ind)
+state;
+X;
+U;
+X(Indx)=ux(sindu+1:sindx+sindu);
+U(Indu)=ux(1:sindu);
+state.x=X;
+Uind=1
+for k=pointi'
+ state.outtb(k)=matrix(U(Uind:Uind+size(state.outtb(k),'*')-1),size(state.outtb(k)));
+ Uind=size(state.outtb(k),'*')+1;
+end
+// state.outtb(pointi)=U;
+[state,t]=scicosim(state,t,t,sim,'linear',[.1,.1,.1,.1]);
+zer=ones(X);zer(Indxp)=0;xp=zer.*state.x;
+Yind=1
+for k=pointo'
+ y(Yind:Yind+size(state.outtb(k),'*')-1)=state.outtb(k)(:);
+ Yind=size(state.outtb(k),'*')+Yind
+end
+// y=state.outtb(pointo);
+zer=ones(y);zer(Indy)=0;err=zer.*(Y-y);
+f=.5*(norm(xp,2)+norm(err,2));
+
+sys=lincos(scs_m,X,U,param)
+g=xp'*[sys.B(:,Indu) sys.A(:,Indx)]-..
+    err'*[sys.D(:,Indu) sys.C(:,Indx)];
+endfunction

scicoslab/sci_gateway/.gitignore

+loader.sce

scicoslab/sci_gateway/builder.sce

+mode(-1)
+names=['sci_jsbSimComm'];
+files=['sci_jsbSimComm.o'];
+libs=[JSBSimLibDir+'/libJSBSim'];
+flag='c';
+makename='Makelib';
+loadername='loader.sce';
+libname='JSBSim_sci_gateway';
+ldflags='';
+cflags='-I../includes -I'+JSBSimIncludeDir;
+fflags='';
+cc='';
+ilib_for_link(names,files,libs,flag,makename,loadername,libname,ldflags,cflags,fflags,cc);
+
+clear names files libs flag makename loadername
+clear libname ldflags cflags fflags cc

scicoslab/sci_gateway/sci_jsbSimComm.cpp

+/*
+ * sci_JSBSimComm.cpp
+ * Copyright (C) James Goppert 2010 <jgoppert@users.sourceforge.net>
+ *
+ * sci_JSBSimComm.cpp is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * sci_JSBSimComm.cpp is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "FGFDMExec.h"
+#include "models/FGFCS.h"
+#include "Trim.h"
+#include "FGStateSpace.h"
+#include <iostream>
+#include <string>
+
+extern "C"
+{
+
+#include <scicos/scicos_block4.h>
+#include <math.h>
+#include "definitions.hpp"
+
+    void sci_JSBSimComm(scicos_block *block, scicos::enumScicosFlags flag)
+    {
+        using namespace JSBSim;
+        //definitions
+        static FGFDMExec fdm;
+        static FGStateSpace ss(fdm);
+
+        // data
+        double *u=(double*)GetInPortPtrs(block,1);
+        double *xOut=(double*)GetOutPortPtrs(block,1);
+        double *y=(double*)GetOutPortPtrs(block,2);
+        double *x=(double*)GetState(block);
+        double *xd=(double*)GetDerState(block);
+
+        //handle flags
+        if (flag==scicos::initialize || flag==scicos::reinitialize)
+        {
+            std::cout << "initializing" << std::endl;
+            fdm.LoadModel("../aircraft","../engine","../systems","f16");
+
+            // defaults
+            bool variablePropPitch = false;
+
+            // get propulsion pointer to determine type/ etc.
+            FGEngine * engine0 = fdm.GetPropulsion()->GetEngine(0);
+            FGThruster * thruster0 = engine0->GetThruster();
+
+            // state space
+            ss.x.add(new FGStateSpace::Vt);
+            ss.x.add(new FGStateSpace::Alpha);
+            ss.x.add(new FGStateSpace::Theta);
+            ss.x.add(new FGStateSpace::Q);
+
+            if (thruster0->GetType()==FGThruster::ttPropeller)
+            {
+                ss.x.add(new FGStateSpace::Rpm);
+                if (variablePropPitch) ss.x.add(new FGStateSpace::Pitch);
+            }
+            switch (engine0->GetType())
+            {
+            case FGEngine::etTurbine:
+                ss.x.add(new FGStateSpace::N2);
+                break;
+            case FGEngine::etTurboprop:
+                ss.x.add(new FGStateSpace::N1);
+                break;
+            default:
+                break;
+            }
+            ss.x.add(new FGStateSpace::Beta);
+            ss.x.add(new FGStateSpace::Phi);
+            ss.x.add(new FGStateSpace::P);
+            ss.x.add(new FGStateSpace::R);
+
+            ss.x.add(new FGStateSpace::ThrottlePos);
+            ss.x.add(new FGStateSpace::DaPos);
+            ss.x.add(new FGStateSpace::DePos);
+            ss.x.add(new FGStateSpace::DrPos);
+
+            ss.u.add(new FGStateSpace::ThrottleCmd);
+            ss.u.add(new FGStateSpace::DaCmd);
+            ss.u.add(new FGStateSpace::DeCmd);
+            ss.u.add(new FGStateSpace::DrCmd);
+
+            // state feedback
+            ss.y = ss.x;
+        }
+        else if (flag==scicos::terminate)
+        {
+            std::cout << "terminating" << std::endl;
+        }
+        else if (flag==scicos::updateState)
+        {
+            // make sure we have initialized first
+
+            // update the state
+            //fdm->updateState(x);
+        }
+        else if (flag==scicos::computeDeriv)
+        {
+            // update the state
+            //sci_JSBSimComm(block,scicos::updateState);
+
+            // run the communications
+            //fdm->calculateXd(x,u,xd);
+        }
+        else if (flag==scicos::computeOutput)
+        {
+            // calculate the output
+            //fdm->calculateY(x,u,y,xOut);
+        }
+        else
+        {
+            std::cout << "unhandled flag: " << flag << std::endl;
+        }
+    }
+
+} // extern c
+
+// vim:ts=4:sw=4

scicoslab/scicos/JSBSim/.gitignore

+oooark.cosf

scicoslab/scicos/JSBSim/jsbSimComm.sci

+function [x,y,typ]=jsbSimComm(job,arg1,arg2)
+//
+// flightGearComm.sci
+// Copyright (C) James Goppert 2010 <jgoppert@users.sourceforge.net>
+//
+// flightGearComm.sci is free software: you can redistribute it and/or modify it
+// under the terms of the GNU General Public License as published by the
+// Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// flightGearComm.sci is distributed in the hope that it will be useful, but
+// WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+// See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License along
+// with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+
+x=[];y=[];typ=[];
+
+select job
+	case 'plot' then
+	 	standard_draw(arg1)
+	case 'getinputs' then
+	 	[x,y,typ]=standard_inputs(arg1)
+	case 'getoutputs' then
+	 	[x,y,typ]=standard_outputs(arg1)
+	case 'getorigin' then
+	 	[x,y]=standard_origin(arg1)
+	case 'set' then
+		x=arg1;
+		graphics=arg1.graphics;exprs=graphics.exprs
+		model=arg1.model;
+		while %t do
+			labels=[..
+				'model name';..
+				'aircraft path';..
+				'engine path';..
+				'systems path';..
+				'velocity, fps';..
+				'alpah,rad';..
+				'pitch, rad';..
+				'pitchRate, rad/s';..
+				'altitude ASL, ft';..
+				'sideSlip, rad';..
+				'roll, rad';..
+				'rollRate, rad/s';..
+				'yawRate, rad/s';..
+				'heading, rad';..
+				'rpm'];
+			[ok,ModelName,AircraftPath,EnginePath,SystemsPath,..
+			velocityTrue,alpha,pitch,pitchRate,altitude,sideSlip,..
+			roll,rollRate,yawRate,heading,rpm,exprs]=..
+				getvalue('Set JSBSim Parameters',labels,..
+				list('str',-1,'str',-1,'str',-1,'str',-1,..
+					'vec',1,'vec',1,'vec',1,'vec',1,..
+					'vec',1,'vec',1,'vec',1,'vec',1,..
+					'vec',1,'vec',1,'vec',1),exprs);
+			if ~ok then break,end
+			[model,graphics,ok]=check_io(model,graphics,[4],[11;8],[],[])
+			if ok then
+				model.state=[velocityTrue;alpha;pitch;pitchRate;altitude;..
+					sideSlip;roll;rollRate;yawRate;heading;rpm];
+				graphics.exprs=exprs;
+				x.graphics=graphics;
+				x.model=model;
+				break
+			end
+		end
+	case 'define' then
+		// set model properties
+		model=scicos_model()
+		model.sim=list('sci_jsbSimComm',4)
+		model.in=4
+		model.out=[11;8]
+		model.blocktype='c'
+		model.dep_ut=[%f %t]
+
+		// jsbsim parameters
+		ModelName="EasyStar";
+		AircraftPath="~/Projects/oooark/data/";
+		EnginePath="~/Projects/oooark/data/EasyStar/Engines";
+		SystemsPath="~/Projects/oooark/data/EasyStar/Engines";
+			
+		// intial state
+		velocityTrue=40; // ft/s
+		alpha=0; // rad
+		pitch=0; // rad
+		pitchRate=0;  // rad/s
+		altitude=1000; // ft ASL, don't want to start in ground
+		sideSlip=0; // rad
+		roll=0; // rad
+		rollRate=0; // rad/s
+		yawRate=0; // rad/s
+		heading=0; // rad
+		rpm=0; // rpm
+
+		// save state
+		model.state=[velocityTrue;alpha;pitch;pitchRate;altitude;..
+			sideSlip;roll;rollRate;yawRate;heading;rpm];
+
+			// initialize strings for gui
+		exprs=[strcat(ModelName),strcat(AircraftPath),strcat(EnginePath),strcat(SystemsPath),..
+		strcat(sci2exp(velocityTrue)),strcat(sci2exp(alpha)),..
+		strcat(sci2exp(pitch)),strcat(sci2exp(pitchRate)),..
+		strcat(sci2exp(altitude)),strcat(sci2exp(sideSlip)),..
+		strcat(sci2exp(roll)),strcat(sci2exp(rollRate)),..
+		strcat(sci2exp(yawRate)),strcat(sci2exp(heading)),strcat(sci2exp(rpm))];
+
+		// setup icon
+	  	gr_i=['xstringb(orig(1),orig(2),''JsbSimCom'',sz(1),sz(2),''fill'');']
+	  	x=standard_define([5 2],model,exprs,gr_i)
+	end
+endfunction
+
+// vim:ts=4:sw=4

scicoslab/scicos/builder.sce

+mode(-1);
+genlib('lib_JSBSimBlocks',pwd()+'/JSBSim');
+create_palette('JSBSim');

scicoslab/scicos/loader.sce

+mode(-1);
+add_palette('oooark',pwd()+'/oooark/oooark.cosf');

scicoslab/scripts/fgrun.sh

+#!/bin/bash
+fgfs \
+--aircraft=EasyStar \
+--geometry=400x300 \
+--native-fdm=socket,out,120,,5500,udp \
+--native-ctrls=socket,out,120,,5501,udp \
+--native-ctrls=socket,in,120,,5502,udp \
+--vc=30 \
+--altitude=1000 \
+--heading=90 \
+--roll=0 \
+--pitch=0 \
+--wind=0@0 \
+--turbulence=0.0 \
+--timeofday=noon \
+--shading-flat \
+--notrim \
+--fog-disable \
+--disable-textures \
+--disable-specular-highlight \
+--disable-skyblend \
+--disable-random-objects \
+--disable-panel \
+--disable-horizon-effect \
+--disable-clouds \
+--disable-anti-alias-hud

src/FGStateSpace.cpp

@@ -32,7 +32,7 @@ void FGStateSpace::linearize(
     std::vector< std::vector<double> > & D)
 {
     double h = 1e-8;
-	double dt = 1e-2;
+    double dt = 1e-2;
 
     m_fdm.Setdt(dt);
 
@@ -43,7 +43,7 @@ void FGStateSpace::linearize(
     // C, d(y)/dx
     numericalJacobian(C,y,x,y0,x0,h);
     // D, d(y)/du
-	numericalJacobian(D,y,u,y0,u0,h);
+    numericalJacobian(D,y,u,y0,u0,h);
 
 }
 
@@ -63,62 +63,65 @@ void FGStateSpace::numericalJacobian(std::vector< std::vector<double> >  & J, Co
             y.set(y0);
             x.set(j,x.get(j)+h);
             if (computeYDerivative)
-			{
-				m_fdm.SuspendIntegration();
-				m_fdm.Run();
-				m_fdm.ResumeIntegration();
-				f1 = y.getDeriv(i);
-			}
+            {
+                m_fdm.SuspendIntegration();
+                m_fdm.Run();
+                m_fdm.ResumeIntegration();
+                f1 = y.getDeriv(i);
+            }
             else f1 = y.get(i);
 
             x.set(x0);
             y.set(y0);
             x.set(j,x.get(j)+2*h);
             if (computeYDerivative)
-			{
-				m_fdm.SuspendIntegration();
-				m_fdm.Run();
-				m_fdm.ResumeIntegration();
-				f2 = y.getDeriv(i);
-			}
+            {
+                m_fdm.SuspendIntegration();
+                m_fdm.Run();
+                m_fdm.ResumeIntegration();
+                f2 = y.getDeriv(i);
+            }
             else f2 = y.get(i);
 
             x.set(x0);
             y.set(y0);
             x.set(j,x.get(j)-h);
             if (computeYDerivative)
-			{
-				m_fdm.SuspendIntegration();
-				m_fdm.Run();
-				m_fdm.ResumeIntegration();
-				fn1 = y.getDeriv(i);
-			}
+            {
+                m_fdm.SuspendIntegration();
+                m_fdm.Run();
+                m_fdm.ResumeIntegration();
+                fn1 = y.getDeriv(i);
+            }
             else fn1 = y.get(i);
 
             x.set(x0);
             y.set(y0);
             x.set(j,x.get(j)-2*h);
             if (computeYDerivative)
-			{
-				m_fdm.SuspendIntegration();
-				m_fdm.Run();
-				m_fdm.ResumeIntegration();
-				fn2 = y.getDeriv(i);
-			}
+            {
+                m_fdm.SuspendIntegration();
+                m_fdm.Run();
+                m_fdm.ResumeIntegration();
+                fn2 = y.getDeriv(i);
+            }
             else fn2 = y.get(i);
 
             J[i][j] = (8*(f1-fn1)-(f2-fn2))/(12*h); // 3rd order taylor approx from lewis, pg 203
             x.set(x0);
             y.set(y0);
 
-			std::cout << std::scientific << "\ti:\t" << y.getName(i) << "\tj:\t"
-					  << x.getName(j)
-					  << "\tfn2:\t" << fn2 << "\tfn1:\t" << fn1
-					  << "\tf1:\t" << f1 << "\tf2:\t" << f2
-					  << "\tf1-fn1:\t" << f1-fn1
-					  << "\tf2-fn2:\t" << f2-fn2
-					  << "\tdf/dx:\t" << J[i][j]
-					  << std::fixed << std::endl;
+            if (m_fdm.GetDebugLevel() > 0)
+            {
+                std::cout << std::scientific << "\ti:\t" << y.getName(i) << "\tj:\t"
+                          << x.getName(j)
+                          << "\tfn2:\t" << fn2 << "\tfn1:\t" << fn1
+                          << "\tf1:\t" << f1 << "\tf2:\t" << f2
+                          << "\tf1-fn1:\t" << f1-fn1
+                          << "\tf2-fn2:\t" << f2-fn2
+                          << "\tdf/dx:\t" << J[i][j]
+                          << std::fixed << std::endl;
+            }
         }
     }
 }

src/FGStateSpace.h

@@ -37,11 +37,11 @@ class FGStateSpace
 {
 public:
 
-	// component class
+    // component class
     class Component
     {
     protected:
-		FGStateSpace * m_stateSpace;
+        FGStateSpace * m_stateSpace;
         FGFDMExec * m_fdm;
         std::string m_name, m_unit;
     public:
@@ -52,24 +52,27 @@ public:
         virtual void set(double val) = 0;
         virtual double getDeriv() const
         {
-        	// by default should calculate using finite difference approx
-			std::vector<double> x0 = m_stateSpace->x.get();	
-			std::vector<double> y0 = m_stateSpace->y.get();	
-			double f0 = get();
-			m_fdm->Run();
-			double f1 = get();
-			m_stateSpace->x.set(x0);
-			m_stateSpace->y.set(y0);
-			std::cout << std::scientific
-				<< "name: " << m_name
-				<< "\nf1: " << f0
-				<< "\nf2: " << f1
-				<< "\ndt: " << m_fdm->GetDeltaT()
-				<< "\tdf/dt: " << (f1-f0)/m_fdm->GetDeltaT()
-				<< std::fixed << std::endl;
-			return (f1-f0)/m_fdm->GetDeltaT();
+            // by default should calculate using finite difference approx
+            std::vector<double> x0 = m_stateSpace->x.get();
+            std::vector<double> y0 = m_stateSpace->y.get();
+            double f0 = get();
+            m_fdm->Run();
+            double f1 = get();
+            m_stateSpace->x.set(x0);
+            m_stateSpace->y.set(y0);
+            if (m_fdm->GetDebugLevel() > 0)
+            {
+                std::cout << std::scientific
+                          << "name: " << m_name
+                          << "\nf1: " << f0
+                          << "\nf2: " << f1
+                          << "\ndt: " << m_fdm->GetDeltaT()
+                          << "\tdf/dt: " << (f1-f0)/m_fdm->GetDeltaT()
+                          << std::fixed << std::endl;
+            }
+            return (f1-f0)/m_fdm->GetDeltaT();
         };
- 		void setStateSpace(FGStateSpace * stateSpace)
+        void setStateSpace(FGStateSpace * stateSpace)
         {
             m_stateSpace = stateSpace;
         }
@@ -87,27 +90,27 @@ public:
         }
     };
 
-	// component vector class
+    // component vector class
     class ComponentVector
     {
     public:
-        ComponentVector(FGFDMExec & fdm, FGStateSpace * stateSpace) : 
-			m_stateSpace(stateSpace), m_fdm(fdm), m_components() {}
+        ComponentVector(FGFDMExec & fdm, FGStateSpace * stateSpace) :
+                m_stateSpace(stateSpace), m_fdm(fdm), m_components() {}
         ComponentVector & operator=(ComponentVector & componentVector)
         {
             m_fdm = componentVector.m_fdm;
             m_components = componentVector.m_components;
             return *this;
         }
-        ComponentVector(const ComponentVector & componentVector) : 
-			m_stateSpace(componentVector.m_stateSpace),
-			m_fdm(componentVector.m_fdm),
-			m_components(componentVector.m_components)
-		{
-		}
+        ComponentVector(const ComponentVector & componentVector) :
+                m_stateSpace(componentVector.m_stateSpace),
+                m_fdm(componentVector.m_fdm),
+                m_components(componentVector.m_components)
+        {
+        }
         void add(Component * comp)
         {
-			comp->setStateSpace(m_stateSpace);
+            comp->setStateSpace(m_stateSpace);
             comp->setFdm(&m_fdm);
             m_components.push_back(comp);
         }
@@ -141,6 +144,10 @@ public:
             for (int i=0;i<getSize();i++) val.push_back(m_components[i]->get());
             return val;
         }
+        void get(double * array) const
+        {
+            for (int i=0;i<getSize();i++) array[i] = m_components[i]->get();
+        }
         double getDeriv(int i)
         {
             return m_components[i]->getDeriv();
@@ -151,10 +158,18 @@ public:
             for (int i=0;i<getSize();i++) val.push_back(m_components[i]->getDeriv());
             return val;
         }
+        void getDeriv(double * array) const
+        {
+            for (int i=0;i<getSize();i++) array[i] = m_components[i]->getDeriv();
+        }
         void set(vector<double> vals)
         {
             for (int i=0;i<getSize();i++) m_components[i]->set(vals[i]);
         }
+        void set(double * array)
+        {
+            for (int i=0;i<getSize();i++) m_components[i]->set(array[i]);
+        }
         std::string getName(int i) const
         {
             return m_components[i]->getName();
@@ -176,21 +191,21 @@ public:
             return unit;
         }
     private:
-		FGStateSpace * m_stateSpace;
+        FGStateSpace * m_stateSpace;
         FGFDMExec & m_fdm;
         std::vector<Component *> m_components;
     };
 
-	// component vectors
+    // component vectors
     ComponentVector x, u, y;
 
-	// constructor
+    // constructor
     FGStateSpace(FGFDMExec & fdm) : x(fdm,this), u(fdm,this), y(fdm,this), m_fdm(fdm) {};
 
-	// deconstructor
+    // deconstructor
     virtual ~FGStateSpace() {};
 
-	// linearization function
+    // linearization function
     void linearize(std::vector<double> x0, std::vector<double> u0, std::vector<double> y0,
                    std::vector< std::vector<double> > & A,
                    std::vector< std::vector<double> > & B,
@@ -199,19 +214,19 @@ public:
 
 private:
 
-	// compute numerical jacobian of a matrix
+    // compute numerical jacobian of a matrix
     void numericalJacobian(std::vector< std::vector<double> > & J, ComponentVector & y,
-    	ComponentVector & x, const std::vector<double> & y0,
-    	const std::vector<double> & x0, double h=1e-5, bool computeYDerivative = false);
+                           ComponentVector & x, const std::vector<double> & y0,
+                           const std::vector<double> & x0, double h=1e-5, bool computeYDerivative = false);
 
-	// flight dynamcis model
+    // flight dynamcis model
     FGFDMExec & m_fdm;
 
 public:
 
-	// components
-    
-	class Vt : public Component
+    // components
+
+    class Vt : public Component
     {
     public:
         Vt() : Component("Vt","ft/s") {};
@@ -223,12 +238,12 @@ public:
         {
             m_fdm->GetAuxiliary()->SetVt(val);
         }
-		double getDeriv() const
+        double getDeriv() const
         {
-    		return (m_fdm->GetPropagate()->GetUVW(1)*m_fdm->GetPropagate()->GetUVWdot(1) +
-                  m_fdm->GetPropagate()->GetUVW(2)*m_fdm->GetPropagate()->GetUVWdot(2) +
-                  m_fdm->GetPropagate()->GetUVW(3)*m_fdm->GetPropagate()->GetUVWdot(3))/
-                 m_fdm->GetAuxiliary()->GetVt(); // from lewis, vtrue dot
+            return (m_fdm->GetPropagate()->GetUVW(1)*m_fdm->GetPropagate()->GetUVWdot(1) +
+                    m_fdm->GetPropagate()->GetUVW(2)*m_fdm->GetPropagate()->GetUVWdot(2) +
+                    m_fdm->GetPropagate()->GetUVW(3)*m_fdm->GetPropagate()->GetUVWdot(3))/
+                   m_fdm->GetAuxiliary()->GetVt(); // from lewis, vtrue dot
         }
 
     };
@@ -505,7 +520,7 @@ public:
             m_fdm->GetFCS()->SetDePos(ofRad,val);
         }
     };
-	
+
     class DrCmd : public Component
     {
     public:
@@ -519,7 +534,7 @@ public:
             m_fdm->GetFCS()->SetDrCmd(val);
         }
     };
-	
+
     class DrPos : public Component
     {
     public:

src/Trim.cpp

@@ -182,15 +182,15 @@ FGNelderMead::FGNelderMead(Function & f, const std::vector<double> & initialGues
             std::cin.get();
         }
 
-		// costs
+        // costs
 
         // try inversion
         double costTry = tryStretch(-1.0);
 
         // if lower cost than best, then try further stretch by speed factor
-		if (costTry < minCost)
+        if (costTry < minCost)
         {
-        	costTry = tryStretch(speed);
+            costTry = tryStretch(speed);
         }
         // otherwise try a contraction
         else if (costTry > nextMaxCost)
@@ -203,11 +203,11 @@ FGNelderMead::FGNelderMead(Function & f, const std::vector<double> & initialGues
             {
                 if (showSimplex)
                     std::cout << "multiD contraction about: " << m_iMin << std::endl;
-				contract();
+                contract();
             }
         }
-		
-		// iteration
+
+        // iteration
         iter++;
     }
     std::cout << "\ti\t: " << iter << std::endl;
@@ -261,15 +261,15 @@ double FGNelderMead::tryStretch(double factor)
 
 void FGNelderMead::contract()
 {
- 	for (int dim=0;dim<m_nDim;dim++)
-	{
-		for (int vertex=0;vertex<m_nVert;vertex++)
-		{
-			m_simplex[vertex][dim] =
-				0.5*(m_simplex[vertex][dim] +
-					 m_simplex[m_iMin][dim]);
-		}
-	}
+    for (int dim=0;dim<m_nDim;dim++)
+    {
+        for (int vertex=0;vertex<m_nVert;vertex++)
+        {
+            m_simplex[vertex][dim] =
+                0.5*(m_simplex[vertex][dim] +
+                     m_simplex[m_iMin][dim]);
+        }
+    }
 }
 
 void FGNelderMead::constructSimplex(const std::vector<double> & guess,
@@ -687,6 +687,7 @@ int main (int argc, char const* argv[])
 
     // variables
     FGFDMExec fdm;
+    fdm.SetDebugLevel(0); // hide messages
     FGTrimmer::Constraints constraints;
 
     std::cout << "\n==============================================\n";

src/Trim.h

@@ -54,7 +54,7 @@ private:
 
     // methods
     double tryStretch(double factor);
-	void contract();
+    void contract();
     void constructSimplex(const vector<double> & guess, const vector<double> & stepSize);
     void boundVertex(vector<double> & vertex,
                      const vector<double> & upperBound,