Runtime for the QSS methods

git-svn-id: https://openmodelica.org/svn/OpenModelica/trunk@8913 f25d12d1-65f4-0310-ae8a-bbce733d8d8e

c_runtime/Makefile.common

@@ -178,6 +178,6 @@ clean :
 	rm -f $(SIMOBJS)
 	cd sendData && rm -fr release debug *sendData*.o *sendData*.a
 	rm -rf sendData/release sendData/debug
-	rm -f sendData.o sendData.a sendData/sendDataHumbug.o sendData/releasesendData.o sendData/Makefile
+	rm -f sendData.o sendData.a sendData/sendDataHumbug.o sendData/releasesendData.o sendData/Makefile solver_qss/*.o
 
 

c_runtime/solver_qss/integrator.cpp

@@ -6,53 +6,63 @@ IntegratorQSS::IntegratorQSS(double dqm, double dqr)
   order=1;
   dQmin=dqm;
   dQrel=dqr;
+
 }
 
 void IntegratorQSS::init(Time t, unsigned int i)
 {
   index=i;
+  for (int j=0;j<inputRows;j++)
+  {
+    if (inputMatrix[j*2]==index) {
+      cout << "Integrator " << index << " computes state " << inputMatrix[j*2+1] <<endl;
+      state = inputMatrix[j*2+1];
+      break;
+    }
+  }
+
   sigma=0;
-  X[index].setCoeff(0,globalData->states[index]);
-  q[index].setOrder(order-1);
-  X[index].setOrder(order);
-  derX[index].setOrder(order);
-  q[index].setCoeff(0,X[index].value()); 
-  X[index].sampledAt(t);
-  q[index].sampledAt(t);
-  dQ = fabs(X[index].value())*dQrel;
+  X[state].setCoeff(0,globalData->states[state]);
+  q[state].setOrder(order-1);
+  X[state].setOrder(order);
+  derX[state].setOrder(order);
+  q[state].setCoeff(0,X[state].value()); 
+  X[state].sampledAt(t);
+  q[state].sampledAt(t);
+  dQ = fabs(X[state].value())*dQrel;
   if (dQ<dQmin)
     dQ = dQmin;
 }
 
 void IntegratorQSS::makeStep(Time t)
 {
-  X[index].advanceBy(sigma);
-  X[index].sampledAt(t);
-  q[index].sampledAt(t);
-  q[index].setCoeff(0,X[index].value()); 
-  dQ = fabs(X[index].value())*dQrel;
+  X[state].advanceBy(sigma);
+  X[state].sampledAt(t);
+  q[state].sampledAt(t);
+  q[state].setCoeff(0,X[state].value()); 
+  dQ = fabs(X[state].value())*dQrel;
   if (dQ<dQmin)
     dQ = dQmin;
-  if (X[index].coeff(1)==0.0)
+  if (X[state].coeff(1)==0.0)
     sigma = INF;
   else 
-    sigma = fabs(dQ/X[index].coeff(1));
+    sigma = fabs(dQ/X[state].coeff(1));
 }
 
 void IntegratorQSS::update(Time t) {
-  X[index].advanceTo(t);
-  X[index].setCoeff(1,derX[index].coeff(0));
-  QssSignal diff = q[index]- X[index];
+  X[state].advanceTo(t);
+  X[state].setCoeff(1,derX[state].coeff(0));
+  QssSignal diff = q[state]- X[state];
   if (sigma>0) {
-    if (X[index].coeff(1)==0) {
+    if (X[state].coeff(1)==0) {
       sigma=INF;
       return;
     }
     const double sigmaUpper = diff.offsetBy(dQ).minPosRoot();
     const double sigmaLower = diff.offsetBy(-dQ).minPosRoot();
     sigma=std::min(sigmaUpper,sigmaLower);
-    if (fabs(X[index].value()-q[index].value())>dQ)
+    if (fabs(X[state].value()-q[state].value())>dQ)
       sigma=0.0;
   }
-  X[index].sampledAt(t);
+  X[state].sampledAt(t);
 }

c_runtime/solver_qss/integrator.h

@@ -11,6 +11,7 @@ class IntegratorQSS: public Simulator
   virtual void makeStep(Time t);
   virtual void update(Time t);
 private:
+  unsigned int state;
   unsigned int index;
   double dQmin;
   double dQrel;

c_runtime/solver_qss/runtime.h

@@ -17,10 +17,13 @@ void function_staticBlocks(int,double,double*,double*);
 double minposroot(double *coeff, int order);
 
 extern QssSignal *X,*q,*derX,*alg,*zc;
-extern char incidenceMatrix[];
+extern int incidenceMatrix[];
+extern int incidenceRows;
 extern int staticBlocks;
-extern char inputMatrix[];
-extern char outputMatrix[];
+extern int inputMatrix[];
+extern int outputMatrix[];
+extern int outputRows;
+extern int inputRows;
 
 extern bool interactiveSimuation;
 int

c_runtime/solver_qss/solver_qss.cpp

@@ -78,7 +78,7 @@ int qss_main( int argc, char** argv,double &start,  double &stop, double &step,
   // Init static functions - One per state but could differ
   for (int i=0;i<staticBlocks;i++)
   {
-    childs[i+globalData->nStates] = new StaticFunction(3,dQmin,dQrel);
+    childs[i+globalData->nStates] = new StaticFunction(1,dQmin,dQrel,i+globalData->nStates);
     childs[i+globalData->nStates]->init(globalData->timeValue,i);
     tn[i+globalData->nStates] = globalData->timeValue+childs[i+globalData->nStates]->ta();
   }
@@ -127,12 +127,12 @@ int qss_main( int argc, char** argv,double &start,  double &stop, double &step,
       steps++;
 
     // Update corresponding childs
-    for (int i=0;i<size;i++)
+    for (int i=0;i<incidenceRows;i++)
     {
-      if (incidenceMatrix[index*size+i])
+      if (incidenceMatrix[i*2]==index)
       {
-        childs[i]->update(globalData->timeValue);
-        tn[i]=globalData->timeValue+childs[i]->ta();
+        childs[incidenceMatrix[i*2+1]]->update(globalData->timeValue);
+        tn[incidenceMatrix[i*2+1]]=globalData->timeValue+childs[incidenceMatrix[i*2+1]]->ta();
       }
     }
   }

c_runtime/solver_qss/static_function.cpp

@@ -1,15 +1,31 @@
 #include "runtime.h"
 #include "static_function.h"
 
-StaticFunction::StaticFunction(int ord, double dqm, double dqr)
+StaticFunction::StaticFunction(int ord, double dqm, double dqr,int in)
 {
   order=ord;
   dQmin=dqm;
   dQrel=dqr;
+  devsIndex=in;
+  for (int i=0;i<inputRows;i++)
+  {
+    if (inputMatrix[i*2]==devsIndex) {
+      cout << "Block " << devsIndex << " has as input var " << inputMatrix[i*2+1] <<endl;
+      inputs.push_back(inputMatrix[i*2+1]);
+    }
+  }
+  for (int i=0;i<outputRows;i++)
+  {
+    if (outputMatrix[i*2]==devsIndex) {
+      cout << "Block " << devsIndex << " has as output var " << outputMatrix[i*2+1] <<endl;
+      computes.push_back(outputMatrix[i*2+1]);
+    }
+  }
 }
 
 StaticFunction::~StaticFunction()
 {
+  delete []inp;
   delete []out;
   delete []outdt;
   delete []out_dt;
@@ -26,6 +42,7 @@ void StaticFunction::init(Time t, unsigned int i)
   sigma=1e-12;
   outVars=1;
   out = new double[outVars];
+  inp = new double[outVars];
   outdt = new double[outVars];
   out_dt = new double[outVars];
   if (order>1) {
@@ -37,9 +54,24 @@ void StaticFunction::init(Time t, unsigned int i)
 void StaticFunction::makeStep(Time t)
 {
   advanceInputs(t);
-  function_staticBlocks(index,t,NULL,out);
+  function_staticBlocks(index,t,inp,out);
+  derX[0].setCoeff(0,out[0]);
+  derX[0].sampledAt(t);
+
+  if (order>1) 
+  {
+    advanceInputs(t-dt);
+    function_staticBlocks(index,t,inp,out_dt);
+
+    advanceInputs(t+dt);
+    function_staticBlocks(index,t+dt,inp,outdt);
+
+  }
+
   writeOutputs(t);
   sigma=INF;
+  // Take back the time
+  globalData->timeValue=t;
 }
 
 void StaticFunction::update(Time t) 
@@ -49,23 +81,26 @@ void StaticFunction::update(Time t)
 
 void StaticFunction::writeOutputs(Time t)
 {
-  for (int i=0; i<globalData->nStates;i++)
+  std::list<int>::iterator it=computes.begin();
+  int i=0;
+  for (;it!=computes.end();it++,i++)
   {
-    if (outputMatrix[(globalData->nStates+index)*globalData->nStates +i])
-    {
-      derX[i].setCoeff(0,globalData->statesDerivatives[i]);
-      derX[i].sampledAt(t);
+    if (*it<globalData->nStates) {
+      derX[*it].sampledAt(t);  
+      derX[*it].setCoeff(0,out[i]);
+      // If order>1...
     }
-  }
+  } 
 }
 
 void StaticFunction::advanceInputs(Time t)
 {
-  for (int i=0; i<globalData->nStates;i++)
+  std::list<int>::iterator it=inputs.begin();
+  for (; it!=inputs.end();it++)
   {
-    if (inputMatrix[(globalData->nStates+index)*globalData->nStates +i])
+    if (*it<globalData->nStates) 
     {
-      globalData->states[i] = q[i].valueAt(t);
+      inp[*it] = q[*it].valueAt(t);
     }
   }
 }

c_runtime/solver_qss/static_function.h

@@ -6,7 +6,7 @@
 class StaticFunction: public Simulator
 {
 public:
-  StaticFunction(int ord, double dqm, double dqr);
+  StaticFunction(int ord, double dqm, double dqr, int devsIndex);
   ~StaticFunction();
   virtual void init(Time t, unsigned int i);
   virtual void makeStep(Time t);
@@ -17,11 +17,12 @@ class StaticFunction: public Simulator
   void writeOutputs(Time t);
 
   double dQmin,dQrel,dQ;
-  double *out,*outdt,*out_dt,*out2dt,*out_2dt;
+  double *out,*outdt,*out_dt,*out2dt,*out_2dt,*inp;
   unsigned int index;
   int order;
   unsigned int outVars;
   list<int> inputs;
   list<int> computes;
   double dt;
+  int devsIndex;
 };