Skip to content
Snippets Groups Projects
Commit b6bd115b authored by James Goppert's avatar James Goppert
Browse files

Working on scicoslab.

parent b8a8d2a2
No related branches found
No related tags found
No related merge requests found
mode(-1)
names=['sci_jsbsimComm'];
files=['sci_jsbsimComm.o'];
libs=[jsbsimLibDir+'/libjsbsim'];
libs=[jsbsimLibDir+'/libJSBSim'];
flag='c';
makename='Makelib';
loadername='loader.sce';
......
/*
* 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
* 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
* 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.
......@@ -23,6 +23,69 @@
#include <iostream>
#include <string>
class JSBSimComm
{
public:
JSBSimComm() : ss(m_fdm)
{
using namespace JSBSim;
m_fdm.SetDebugLevel(0);
m_fdm.Setdt(1./120);
std::cout << "initializing" << std::endl;
m_fdm.LoadModel("../aircraft","../engine","../systems","f16");
// defaults
bool variablePropPitch = false;
// get propulsion pointer to determine type/ etc.
FGEngine * engine0 = m_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;
}
JSBSim::FGStateSpace ss;
private:
JSBSim::FGFDMExec m_fdm;
};
extern "C"
{
......@@ -30,102 +93,53 @@ extern "C"
#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;
}
}
void sci_jsbsimComm(scicos_block *block, scicos::enumScicosFlags flag)
{
//definitions
static JSBSimComm comm;
// 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;
sci_jsbsimComm(block,scicos::updateState);
std::cin.get();
}
else if (flag==scicos::terminate)
{
std::cout << "terminating" << std::endl;
}
else if (flag==scicos::updateState)
{
comm.ss.u.set(u);
comm.ss.x.set(x);
std::cout << "updating state" << std::endl;
std::cout << comm.ss << std::endl;
}
else if (flag==scicos::computeDeriv)
{
//comm.ss.x.getDeriv(xd);
std::cout << "computing deriv" << std::endl;
std::cout << comm.ss.x.getDeriv() << std::endl;
}
else if (flag==scicos::computeOutput)
{
comm.ss.x.get(xOut);
comm.ss.y.get(y);
std::cout << "computing output" << std::endl;
}
else
{
std::cout << "unhandled flag: " << flag << std::endl;
}
}
} // extern c
......
mode(-1);
genlib('lib_jsbsimBlocks',pwd()+'/jsbsim');
create_palette('JSBSim');
create_palette('jsbsim');
scs_m=scicos_diagram(..
version="scicos4.4",..
props=scicos_params(..
wpar=[600,450,0,0,600,450],..
Title=["jsbsim","jsbsim/"],..
tol=[0.000001,0.000001,1.000D-10,31,0,0],..
tf=30,..
context=[],..
void1=[],..
options=tlist(["scsopt","3D","Background","Link","ID","Cmap"],list(%t,33),[8,1],[1,5,2],..
list([4,1,10,1],[4,1,2,1]),[0.8,0.8,0.8]),..
void2=[],..
void3=[],..
doc=list()))
scs_m.objs(1)=scicos_block(..
gui="PAL_f",..
graphics=scicos_graphics(..
orig=[0,0],..
sz=[40,40],..
flip=%t,..
theta=0,..
exprs=[],..
pin=[],..
pout=[],..
pein=[],..
peout=[],..
gr_i=list(..
["thick=xget(''thickness'');xset(''thickness'',2);";
"xx=orig(1)+ [1 3 5 1 3 5 1 3 5]*(sz(1)/7);";
"yy=orig(2)+sz(2)-[1 1 1 4 4 4 7 7 7]*(sz(2)/10);";
"xrects([xx;yy;[sz(1)/7;sz(2)/5]*ones(1,9)]);";
"xset(''thickness'',thick)"],8),..
id="jsbsim",..
in_implicit=[],..
out_implicit=[]),..
model=scicos_model(..
sim="jsbsim",..
in=[],..
in2=[],..
intyp=1,..
out=[],..
out2=[],..
outtyp=1,..
evtin=[],..
evtout=[],..
state=[],..
dstate=[],..
odstate=list(),..
rpar=mlist(["diagram","props","objs","version","contrib","codegen"],..
tlist(..
["params","wpar","title","tol","tf","context","void1","options","void2","void3",..
"doc"],[600,450,0,0,600,450],["jsbsim","jsbsim/"],..
[0.000001,0.000001,1.000D-10,31,0,0],30,[],[],..
tlist(["scsopt","3D","Background","Link","ID","Cmap"],list(%t,33),[8,1],[1,5,2],..
list([4,1,10,1],[4,1,2,1]),[0.8,0.8,0.8]),[],[],list()),..
list(..
mlist(["Block","graphics","model","gui","doc"],..
mlist(..
["graphics","orig","sz","flip","theta","exprs","pin","pout","pein","peout","gr_i",..
"id","in_implicit","out_implicit"],[0,0],[100,40],%t,0,..
["EasyStar","~/Projects/oooark/data/","~/Projects/oooark/data/EasyStar/Engines",..
"~/Projects/oooark/data/EasyStar/Engines","[]"],0,[0;0],[],[],..
list("xstringb(orig(1),orig(2),''JSBSimComm'',sz(1),sz(2),''fill'');",8),"","E",..
["E";"E"]),..
mlist(..
["model","sim","in","in2","intyp","out","out2","outtyp","evtin","evtout","state",..
"dstate","odstate","rpar","ipar","opar","blocktype","firing","dep_ut","label",..
"nzcross","nmode","equations"],list("sci_jsbsimComm",4),4,[],1,[13;13],[],1,[],..
[],[],[],list(),[],[],list(),"c",[],[%f,%t],"",0,0,list()),"jsbsimComm",list())),..
"",list(),..
mlist(..
["codegeneration","silent","cblock","rdnom","rpat","libs","opt","enable_debug",..
"scopes","remove","replace"],0,0,[],[],[],1,0,[],[],[])),..
ipar=[],..
opar=list(),..
blocktype="h",..
firing=[],..
dep_ut=[%f,%f],..
label="",..
nzcross=0,..
nmode=0,..
equations=list()),..
doc=list())
function [x,y,typ]=jsbsimComm(job,arg1,arg2)
//
// flightGearComm.sci
// jsbsimComm.sci
// Copyright (C) James Goppert 2010 <jgoppert@users.sourceforge.net>
//
// flightGearComm.sci is free software: you can redistribute it and/or modify it
// jsbsimComm.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
// jsbsimComm.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.
......@@ -38,30 +38,15 @@ select job
'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]=..
'x0'];
[ok,ModelName,AircraftPath,EnginePath,SystemsPath,x0,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);
'vec',-1),exprs);
if ~ok then break,end
[model,graphics,ok]=check_io(model,graphics,[4],[11;8],[],[])
[model,graphics,ok]=check_io(model,graphics,[4],[13;13],[],[])
if ok then
model.state=[velocityTrue;alpha;pitch;pitchRate;altitude;..
sideSlip;roll;rollRate;yawRate;heading;rpm];
model.state=[x0];
graphics.exprs=exprs;
x.graphics=graphics;
x.model=model;
......@@ -73,7 +58,7 @@ select job
model=scicos_model()
model.sim=list('sci_jsbsimComm',4)
model.in=4
model.out=[11;8]
model.out=[13;13]
model.blocktype='c'
model.dep_ut=[%f %t]
......@@ -84,29 +69,14 @@ select job
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
x0=[]
// save state
model.state=[velocityTrue;alpha;pitch;pitchRate;altitude;..
sideSlip;roll;rollRate;yawRate;heading;rpm];
model.state=x;
// 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))];
strcat(sci2exp(x))];
// setup icon
gr_i=['xstringb(orig(1),orig(2),''JSBSimComm'',sz(1),sz(2),''fill'');']
......
File added
......@@ -126,12 +126,22 @@ void FGStateSpace::numericalJacobian(std::vector< std::vector<double> > & J, Co
}
}
std::ostream &operator<<(std::ostream &out, const std::vector<double> &vec )
{
for (int i=0;i<vec.size();i++)
{
out << "\t" << vec[i] << std::endl;
}
out << std::ends;
}
std::ostream &operator<<( std::ostream &out, const FGStateSpace::Component &c )
{
out << "\t" << c.getName()
<< "\t" << c.getUnit()
<< "\t:\t" << c.get() << std::ends;
}
std::ostream &operator<<( std::ostream &out, const FGStateSpace::ComponentVector &v )
{
for (int i=0; i< v.getSize(); i++)
......@@ -140,6 +150,7 @@ std::ostream &operator<<( std::ostream &out, const FGStateSpace::ComponentVector
}
out << std::ends;
}
std::ostream &operator<<( std::ostream &out, const FGStateSpace &ss )
{
out << "\nX:\n" << ss.x
......@@ -147,6 +158,7 @@ std::ostream &operator<<( std::ostream &out, const FGStateSpace &ss )
<< "\nY:\n" << ss.y
<< std::ends;
}
std::ostream &operator<<( std::ostream &out, const std::vector< std::vector<double> > &vec2d )
{
for (int i=0;i<vec2d.size();i++)
......
......@@ -654,6 +654,7 @@ public:
};
// stream output
std::ostream &operator<<(std::ostream &out, const std::vector<double> &vec );
std::ostream &operator<<(std::ostream &out, const FGStateSpace::Component &c );
std::ostream &operator<<(std::ostream &out, const FGStateSpace::ComponentVector &v );
std::ostream &operator<<(std::ostream &out, const FGStateSpace &ss );
......
......@@ -5,9 +5,9 @@ EXTRA_DIST = \
SUBDIRS = initialization models input_output math simgear utilities
LIBRARY_SOURCES = FGFDMExec.cpp FGJSBBase.cpp
LIBRARY_SOURCES = FGFDMExec.cpp FGJSBBase.cpp FGStateSpace.cpp
LIBRARY_INCLUDES = FGFDMExec.h FGJSBBase.h
LIBRARY_INCLUDES = FGFDMExec.h FGJSBBase.h FGStateSpace.h
noinst_PROGRAMS = JSBSim Trim
......@@ -52,7 +52,7 @@ JSBSim_LDADD = \
simgear/magvar/libcoremag.a \
-lm
Trim_SOURCES = Trim.cpp FGStateSpace.cpp $(LIBRARY_SOURCES)
Trim_SOURCES = Trim.cpp $(LIBRARY_SOURCES)
Trim_LDADD = \
initialization/libInit.a \
models/libModels.a \
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment