예제 #1
0
// convert current matrix to NumericsMatrix structure
void OSNSMatrix::convert()
{
  DEBUG_BEGIN("OSNSMatrix::convert()\n");
  DEBUG_PRINTF("_storageType = %i\n", _storageType);
  _numericsMat->storageType = _storageType;
  _numericsMat->size0 = _dimRow;
  _numericsMat->size1 = _dimColumn;
  switch (_storageType)
  {
  case NM_DENSE:
  {
    _numericsMat->matrix0 = _M1->getArray(); // Pointer link
    // _numericsMat->matrix1 = NULL; matrix1 is not set to NULL: we
    // keep previous allocation. May be usefull if we switch between
    // different storages during simu
    break;
  }
  case NM_SPARSE_BLOCK:
  {
    _M2->convert();
    _numericsMat->matrix1 = &*_M2->getNumericsMatSparse();
    break;
  }
  case NM_SPARSE:
  {
    // we already filled the matrix
    break;
  }
  default:
  {
     RuntimeException::selfThrow("OSNSMatrix::convert unknown _storageType");
  }
  }
  DEBUG_END("OSNSMatrix::convert()\n");
}
예제 #2
0
OSNSMatrix::OSNSMatrix(unsigned int n, unsigned int m, int stor):
  _dimRow(n),  _dimColumn(m), _storageType(stor)
{
  // Note:

  // for _storageType = NM_DENSE (dense) n represents the real dimension of
  // the matrix and for sparse storage (_storageType == 1) the number
  // of interactionBlocks in a row or column.
  DEBUG_BEGIN("OSNSMatrix::OSNSMatrix(unsigned int n, unsigned int m, int stor)\n");
  switch(_storageType)
  {
  case NM_DENSE:
  {
    // A zero matrix M of size nXn is built.  interactionBlocksPositions
    // remains empty (=NULL) since we have no information concerning
    // the Interaction.
    _M1.reset(new SimpleMatrix(n, n));
    break;
  }
  case NM_SPARSE_BLOCK:
  {
    _M2.reset(new BlockCSRMatrix(n));
    break;
  }
  default: {} // do nothing here
  }

  _numericsMat.reset(new NumericsMatrix);
  NM_null(_numericsMat.get());
  DEBUG_END("OSNSMatrix::OSNSMatrix(unsigned int n, unsigned int m, int stor)\n");

}
예제 #3
0
void NewtonEulerR::computeOutput(double time, Interaction& inter, unsigned int derivativeNumber)
{

  DEBUG_BEGIN("NewtonEulerR::computeOutput(...)\n");
  DEBUG_PRINTF("with time = %f and derivativeNumber = %i starts\n", time, derivativeNumber);

  VectorOfBlockVectors& DSlink = inter.linkToDSVariables();
  SiconosVector& y = *inter.y(derivativeNumber);
  BlockVector& q = *DSlink[NewtonEulerR::q0];


  if (derivativeNumber == 0)
  {
    computeh(time, q, y);
  }
  else
  {
    /* \warning  V.A. 15/04/2016
     * We decide finally not to update the Jacobian there. To be discussed
     */
    // computeJachq(time, inter, DSlink[NewtonEulerR::q0]);
    // computeJachqT(inter, DSlink[NewtonEulerR::q0]);

    if (derivativeNumber == 1)
    {
      assert(_jachqT);
      assert(DSlink[NewtonEulerR::velocity]);
      DEBUG_EXPR(_jachqT->display();); DEBUG_EXPR((*DSlink[NewtonEulerR::velocity]).display(););
예제 #4
0
파일: graph.c 프로젝트: Forilan/fflp
//p_表示plugin,pi_表示plugin_info
//建立proto_graph并建立连接
void init_plugin_graph() {
	//add plugin到graph
	DEBUG_BEGIN();
	vector<plugin_info>::iterator begin = plugin_info_list.begin();
	cout << "plugin size:" << plugin_info_list.size() << endl;
	for (; begin != plugin_info_list.end(); begin++) {
		plugin_graph.add(begin->plug_in);
		DEBUG("%s%s","pluin_info_vector add:",begin->plug_in.proto_name);
	}

	//link各个plugin
	begin = plugin_info_list.begin();
	for (; begin != plugin_info_list.end(); begin++) {
		plugin& p = begin->plug_in;
		const char *p_lower_name = p.lower_proto_name;
		vector<plugin_info>::iterator pi_lower_itr;
		pi_lower_itr = find_if(plugin_info_list.begin(), plugin_info_list.end(),
				bind2nd(find_plugin_info(), p_lower_name));
		if (pi_lower_itr != plugin_info_list.end()) {
			plugin& p_lower = pi_lower_itr->plug_in;
			plugin_graph.link(p_lower,p);
			DEBUG("%s%s%s%s","proto_graph link:",p_lower.proto_name," ==> ",p.proto_name);
		}
	}
	DEBUG_END();
}
void setup() {
  DEBUG_BEGIN(CONSOLE_BAUD);
  DEBUG_PRINTLN(F("Full Wifi Single Battery Test starting"));

  pinMode(LED_BUILTIN, OUTPUT);

  runNum = GbUtility::IncrementRunNum();
  DEBUG_PRINT(F("Starting runNum "));
  DEBUG_PRINTLN(runNum);
  delay(1000);
}
예제 #6
0
// Basic constructor
BlockCSRMatrix::BlockCSRMatrix(SP::InteractionsGraph indexSet):
  _nr(indexSet->size()), 
  _blockCSR(new CompressedRowMat(_nr, _nr)),
  _sparseBlockStructuredMatrix(new SparseBlockStructuredMatrix()),
  _diagsize0(new IndexInt(_nr)),
  _diagsize1(new IndexInt(_nr)),
  rowPos(new IndexInt(_nr)),
  colPos(new IndexInt(_nr))
{
  DEBUG_BEGIN("BlockCSRMatrix::BlockCSRMatrix(SP::InteractionsGraph indexSet)\n");
  fill(indexSet);
  DEBUG_END("BlockCSRMatrix::BlockCSRMatrix(SP::InteractionsGraph indexSet)\n");
}
예제 #7
0
void NewtonEulerR::initialize(Interaction& inter)
{


  DEBUG_BEGIN("NewtonEulerR::initialize(Interaction& inter)\n");

  unsigned int ySize = inter.dimension();
  unsigned int xSize = inter.getSizeOfDS();
  unsigned int qSize = 7 * (xSize / 6);

  if (!_jachq)
    _jachq.reset(new SimpleMatrix(ySize, qSize));
  else
  {
    if (_jachq->size(0) == 0)
    {
      // if the matrix dim are null
      _jachq->resize(ySize, qSize);
    }
    else
    {
      assert((_jachq->size(1) == qSize && _jachq->size(0) == ySize) ||
             (printf("NewtonEuler::initializeWorkVectorsAndMatrices _jachq->size(1) = %d ,_qsize = %d , _jachq->size(0) = %d ,_ysize =%d \n", _jachq->size(1), qSize, _jachq->size(0), ySize) && false) ||
             ("NewtonEuler::initializeWorkVectorsAndMatrices inconsistent sizes between _jachq matrix and the interaction." && false));
    }
  }

  DEBUG_EXPR(_jachq->display());

  if (! _jachqT)
    _jachqT.reset(new SimpleMatrix(ySize, xSize));

  if (! _T)
  {
    _T.reset(new SimpleMatrix(7, 6));
    _T->zero();
    _T->setValue(0, 0, 1.0);
    _T->setValue(1, 1, 1.0);
    _T->setValue(2, 2, 1.0);
  }
  DEBUG_EXPR(_jachqT->display());
  VectorOfBlockVectors& DSlink = inter.linkToDSVariables();
  if (!_contactForce)
  {
    _contactForce.reset(new SiconosVector(DSlink[NewtonEulerR::p1]->size()));
    _contactForce->zero();
  }
  DEBUG_END("NewtonEulerR::initialize(Interaction& inter)\n");
}
예제 #8
0
void ControlZOHSimulation::run()
{
  DEBUG_BEGIN("void ControlZOHSimulation::run()\n");
  EventsManager& eventsManager = *_processSimulation->eventsManager();
  unsigned k = 0;
  boost::progress_display show_progress(_N);
  boost::timer time;
  time.restart();

  TimeStepping& sim = static_cast<TimeStepping&>(*_processSimulation);

  while (sim.hasNextEvent())
  {
    Event& nextEvent = *eventsManager.nextEvent();
    if (nextEvent.getType() == TD_EVENT)
    {
      sim.computeOneStep();
    }

    sim.nextStep();

    if (sim.hasNextEvent() && eventsManager.nextEvent()->getType() == TD_EVENT)  // We store only on TD_EVENT
    {
      (*_dataM)(k, 0) = sim.startingTime();
      storeData(k);
      ++k;
      if (!_silent)
      {
        ++show_progress;
      }
    }
  }

  /* saves last status */
  (*_dataM)(k, 0) = sim.startingTime();
  storeData(k);
  ++k;

  _elapsedTime = time.elapsed();
  _dataM->resize(k, _nDim + 1);
  DEBUG_END("void ControlZOHSimulation::run()\n");
}
예제 #9
0
void BulletR::computeh(double time, BlockVector& q0, SiconosVector& y)
{
  DEBUG_BEGIN("BulletR::computeh(...)\n");

  NewtonEulerR::computeh(time, q0, y);

  DEBUG_PRINT("start of computeh\n");

  btVector3 posa = _contactPoints->getPositionWorldOnA();
  btVector3 posb = _contactPoints->getPositionWorldOnB();
  if (_flip) {
      posa = _contactPoints->getPositionWorldOnB();
      posb = _contactPoints->getPositionWorldOnA();
  }

  (*pc1())(0) = posa[0];
  (*pc1())(1) = posa[1];
  (*pc1())(2) = posa[2];
  (*pc2())(0) = posb[0];
  (*pc2())(1) = posb[1];
  (*pc2())(2) = posb[2];

  {
    y.setValue(0, _contactPoints->getDistance());

    (*nc())(0) = _contactPoints->m_normalWorldOnB[0] * (_flip ? -1 : 1);
    (*nc())(1) = _contactPoints->m_normalWorldOnB[1] * (_flip ? -1 : 1);
    (*nc())(2) = _contactPoints->m_normalWorldOnB[2] * (_flip ? -1 : 1);
  }

  DEBUG_PRINTF("distance : %g\n",  y.getValue(0));


  DEBUG_PRINTF("position on A : %g,%g,%g\n", posa[0], posa[1], posa[2]);
  DEBUG_PRINTF("position on B : %g,%g,%g\n", posb[0], posb[1], posb[2]);
  DEBUG_PRINTF("normal on B   : %g,%g,%g\n", (*nc())(0), (*nc())(1), (*nc())(2));

  DEBUG_END("BulletR::computeh(...)\n");


}
예제 #10
0
void KneeJointR::computeh(double time, BlockVector& q0, SiconosVector& y)
{
  DEBUG_BEGIN("KneeJointR::computeh(double time, BlockVector& q0, SiconosVector& y)\n");
  DEBUG_EXPR(q0.display());

  double X1 = q0.getValue(0);
  double Y1 = q0.getValue(1);
  double Z1 = q0.getValue(2);
  double q10 = q0.getValue(3);
  double q11 = q0.getValue(4);
  double q12 = q0.getValue(5);
  double q13 = q0.getValue(6);
  DEBUG_PRINTF("X1 = %12.8e,\t Y1 = %12.8e,\t Z1 = %12.8e,\n",X1,Y1,Z1);
  DEBUG_PRINTF("q10 = %12.8e,\t q11 = %12.8e,\t q12 = %12.8e,\t q13 = %12.8e,\n",q10,q11,q12,q13);
  double X2 = 0;
  double Y2 = 0;
  double Z2 = 0;
  double q20 = 1;
  double q21 = 0;
  double q22 = 0;
  double q23 = 0;
  if(q0.getNumberOfBlocks()>1)
  {
    // SP::SiconosVector x2 = _d2->q();
    // DEBUG_EXPR( _d2->q()->display(););
    X2 = q0.getValue(7);
    Y2 = q0.getValue(8);
    Z2 = q0.getValue(9);
    q20 = q0.getValue(10);
    q21 = q0.getValue(11);
    q22 = q0.getValue(12);
    q23 = q0.getValue(13);
  }
  y.setValue(0, Hx(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23));
  y.setValue(1, Hy(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23));
  y.setValue(2, Hz(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23));
  DEBUG_EXPR(y.display());
  DEBUG_END("KneeJointR::computeh(double time, BlockVector& q0, SiconosVector& y)\n");
    
}
예제 #11
0
void KneeJointR::computeJachq(double time, Interaction& inter, SP::BlockVector q0)
{
  DEBUG_BEGIN("KneeJointR::computeJachq(double time, Interaction& inter,  SP::BlockVector q0) \n");
  
  _jachq->zero();
  SP::SiconosVector q1 = (q0->getAllVect())[0];

  
  double X1 = q1->getValue(0);
  double Y1 = q1->getValue(1);
  double Z1 = q1->getValue(2);
  double q10 = q1->getValue(3);
  double q11 = q1->getValue(4);
  double q12 = q1->getValue(5);
  double q13 = q1->getValue(6);

  double X2 = 0;
  double Y2 = 0;
  double Z2 = 0;
  double q20 = 1;
  double q21 = 0;
  double q22 = 0;
  double q23 = 0;
  if(q0->getNumberOfBlocks()>1)
  {
    SP::SiconosVector q2 = (q0->getAllVect())[1];
    X2 = q2->getValue(0);
    Y2 = q2->getValue(1);
    Z2 = q2->getValue(2);
    q20 = q2->getValue(3);
    q21 = q2->getValue(4);
    q22 = q2->getValue(5);
    q23 = q2->getValue(6);
    Jd1d2(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23);
  }
  else
    Jd1(X1, Y1, Z1, q10, q11, q12, q13);
  DEBUG_END("KneeJointR::computeJachq(double time, Interaction& inter,  SP::BlockVector q0 ) \n");

}
예제 #12
0
// ======================
// Setup Function
// ======================
void do_setup() {
  /* Clear WDT reset flag. */
  MCUSR &= ~(1<<WDRF);

  DEBUG_BEGIN(9600);
  DEBUG_PRINTLN("started.");
  os.begin();          // OpenSprinkler init
  os.options_setup();  // Setup options

  pd.init();            // ProgramData init

  setSyncInterval(RTC_SYNC_INTERVAL);  // RTC sync interval
  // if rtc exists, sets it as time sync source
  setSyncProvider(RTC.get);
  os.lcd_print_time(os.now_tz());  // display time to LCD

  // enable WDT
  /* In order to change WDE or the prescaler, we need to
   * set WDCE (This will allow updates for 4 clock cycles).
   */
  WDTCSR |= (1<<WDCE) | (1<<WDE);
  /* set new watchdog timeout prescaler value */
  WDTCSR = 1<<WDP3 | 1<<WDP0;  // 8.0 seconds
  /* Enable the WD interrupt (note no reset). */
  WDTCSR |= _BV(WDIE);

  if (os.start_network()) {  // initialize network
    os.status.network_fails = 0;
  } else {
    os.status.network_fails = 1;
  }
  os.status.req_network = 0;
  os.status.req_ntpsync = 1;

  os.apply_all_station_bits(); // reset station bits

  os.button_timeout = LCD_BACKLIGHT_TIMEOUT;
}
예제 #13
0
void KneeJointR::computeDotJachq(double time, SP::SiconosVector qdot1, SP::SiconosVector qdot2 )
{
  DEBUG_BEGIN("KneeJointR::computeDotJachq(double time, SP::SiconosVector qdot1, SP::SiconosVector qdot2) \n");
  _dotjachq->zero();
  
  double Xdot1 = qdot1->getValue(0);
  double Ydot1 = qdot1->getValue(1);
  double Zdot1 = qdot1->getValue(2);
  double qdot10 = qdot1->getValue(3);
  double qdot11 = qdot1->getValue(4);
  double qdot12 = qdot1->getValue(5);
  double qdot13 = qdot1->getValue(6);

  double Xdot2 = 0;
  double Ydot2 = 0;
  double Zdot2 = 0;
  double qdot20 = 1;
  double qdot21 = 0;
  double qdot22 = 0;
  double qdot23 = 0;

  if(qdot2)
  {
    Xdot2 = qdot2->getValue(0);
    Ydot2 = qdot2->getValue(1);
    Zdot2 = qdot2->getValue(2);
    qdot20 = qdot2->getValue(3);
    qdot21 = qdot2->getValue(4);
    qdot22 = qdot2->getValue(5);
    qdot23 = qdot2->getValue(6);
    DotJd1d2(Xdot1, Ydot1, Zdot1, qdot10, qdot11, qdot12, qdot13, Xdot2, Ydot2, Zdot2, qdot20, qdot21, qdot22, qdot23);
  }
  else
    DotJd1(Xdot1, Ydot1, Zdot1, qdot10, qdot11, qdot12, qdot13);

  DEBUG_END("KneeJointR::computeDotJachq(double time, SP::SiconosVector qdot1, SP::SiconosVector qdot2 ) \n");
}
void SlidingReducedOrderObserver::process()
{
  DEBUG_BEGIN("void SlidingReducedOrderObserver::process()\n");
  if (!_pass)
  {
    DEBUG_PRINT("First pass \n ");
    _pass = true;
    //update the estimate using the first value of y, such that C\hat{x}_0 = y_0
    const SiconosVector& y = _sensor->y();
    _e->zero();
    prod(*_C, *_xHat, *_e);
    *_e -= y;

    SiconosVector tmpV(_DS->n());
    SimpleMatrix tmpC(*_C);
    for (unsigned int i = 0; i < _e->size(); ++i)
      tmpV(i) = (*_e)(i);

    tmpC.SolveByLeastSquares(tmpV);
    *(_xHat) -= tmpV;
    *(_DS->x()) -= tmpV;
    _DS->initMemory(1);
    _DS->swapInMemory();
    DEBUG_EXPR(_DS->display(););
예제 #15
0
/*
 * Select on the list of fds.
 * Returns: -1 = error
 *           0 = timeout or nothing to select
 *          >0 = number of signaled fds
 */
int
_gpgme_io_select(struct io_select_fd_s *fds, size_t nfds, int nonblock)
{
    fd_set readfds;
    fd_set writefds;
    unsigned int i;
    int any, max_fd, n, count;
    struct timeval timeout = { 1, 0 }; /* Use a 1s timeout.  */
    void *dbg_help = NULL;

    FD_ZERO(&readfds);
    FD_ZERO(&writefds);
    max_fd = 0;
    if(nonblock)
        timeout.tv_sec = 0;

    DEBUG_BEGIN(dbg_help, 3, "gpgme:select on [ ");
    any = 0;
    for(i = 0; i < nfds; i++)
    {
        if(fds[i].fd == -1)
            continue;
        if(fds[i].frozen)
            DEBUG_ADD1(dbg_help, "f%d ", fds[i].fd);
        else if(fds[i].for_read)
        {
            assert(!FD_ISSET(fds[i].fd, &readfds));
            FD_SET(fds[i].fd, &readfds);
            if(fds[i].fd > max_fd)
                max_fd = fds[i].fd;
            DEBUG_ADD1(dbg_help, "r%d ", fds[i].fd);
            any = 1;
        }
        else if(fds[i].for_write)
        {
            assert(!FD_ISSET(fds[i].fd, &writefds));
            FD_SET(fds[i].fd, &writefds);
            if(fds[i].fd > max_fd)
                max_fd = fds[i].fd;
            DEBUG_ADD1(dbg_help, "w%d ", fds[i].fd);
            any = 1;
        }
        fds[i].signaled = 0;
    }
    DEBUG_END(dbg_help, "]");
    if(!any)
        return 0;

    do
    {
        count = _gpgme_ath_select(max_fd + 1, &readfds, &writefds, NULL,
                                  &timeout);
    }
    while(count < 0 && errno == EINTR);
    if(count < 0)
    {
        int saved_errno = errno;
        DEBUG1("_gpgme_io_select failed: %s\n", strerror(errno));
        errno = saved_errno;
        return -1; /* error */
    }

    DEBUG_BEGIN(dbg_help, 3, "select OK [ ");
    if(DEBUG_ENABLED(dbg_help))
    {
        for(i = 0; i <= max_fd; i++)
        {
            if(FD_ISSET(i, &readfds))
                DEBUG_ADD1(dbg_help, "r%d ", i);
            if(FD_ISSET(i, &writefds))
                DEBUG_ADD1(dbg_help, "w%d ", i);
        }
        DEBUG_END(dbg_help, "]");
    }

    /* n is used to optimize it a little bit.  */
    for(n = count, i = 0; i < nfds && n; i++)
    {
        if(fds[i].fd == -1)
            ;
        else if(fds[i].for_read)
        {
            if(FD_ISSET(fds[i].fd, &readfds))
            {
                fds[i].signaled = 1;
                n--;
            }
        }
        else if(fds[i].for_write)
        {
            if(FD_ISSET(fds[i].fd, &writefds))
            {
                fds[i].signaled = 1;
                n--;
            }
        }
    }
    return count;
}
예제 #16
0
int main(int argc, char *argv[])
  {
    int Rtnval;
    int Done;
    struct sigaction sig;
    DPT_RTN_T i;
 
   //
   // Set up our argument pointers for others to use
   //
   Argc = argc;
   Argv = argv;


#	if (defined(_DPT_SOLARIS) || defined(_DPT_ROOTONLY))
		// Only root or sys are allowed to run dptutil.
		if ( ( getuid () != 0 ) && ( geteuid () != 0 )
		 &&  ( getuid () != 3 ) && ( geteuid () != 3 )
		 &&  ( getgid () != 0 ) && ( getegid () != 0 )
		 &&  ( getgid () != 3 ) && ( getegid () != 3 ) )
			{
			printf ("You must be root to run this utility\n");
			return (0);
			}
#	endif

   /* Parse The Command Line Parameters, And If An Error Exit */

    if(ParseCommandLine(argc,argv)) {
       exit(ExitBadParameter);
    }

#ifdef _SINIX_ADDON
    DEBUG_SETOUTPUT(est);
    if (DebugToFile) {
	DEBUG_BEGIN(5, "DPT Engine");
	DEBUG_SETLEVEL(DebugToFile);
    } else
	DEBUG_OFF;
    SNI_set_compile_date(&engineSig);
    if (Signature) {
	printf("%s: V.%d.%c%c %.2d.%.2d.%d\n", engineSig.dsDescription, engineSig.dsVersion,
	engineSig.dsRevision, engineSig.dsSubRevision, engineSig.dsDay,
	engineSig.dsMonth, 1980 + engineSig.dsYear);
      	exit(ExitGoodStatus);
    }
#else
	DEBUG_SETOUTPUT(cerr);
	DEBUG_OFF;
#endif

  /* Check To See If There Is Already An Engine Out There */

	if (MkLock(NULL) == 1) {
#ifndef _SINIX_ADDON
        if(Verbose)
#endif
           printf(
"\nThere Appears To Be Another Copy Of dpteng Already Running!\n");
           exit(ExitDuplicateEngine);
	}

  /* If The Engine Cannot Be Opened, Print A Message And Exit */

    i = DPT_StartEngine();
    if(i != MSG_RTN_COMPLETED)
      {

        if(Verbose)  {
            printf("\nStarting the Engine: ");
		if (i == ERR_OSD_OPEN_ENGINE)
			printf("Open HBA(s) ");
		else if (i == ERR_SEMAPHORE_ALLOC)
			printf("Alloc Semaphore ");
		else if (i == ERR_CONN_LIST_ALLOC)
			printf("Alloc Connection List ");
		else
			printf(" With Unknown Error %lx", (unsigned long)i);
		printf("Failed\n");
	}

        RmLock(NULL);
        exit(ExitEngOpenFail);
      }

  /* Initialize The Signaling */

    memset((void *)&sig,0,sizeof(sig));


#if defined  ( _DPT_SCO ) || defined ( _DPT_AIX ) || defined( _DPT_BSDI ) || defined(_DPT_FREE_BSD ) || defined(_DPT_LINUX)
    sig.sa_handler = (void (*)(int))AlarmHandler;

#elif defined ( _DPT_SOLARIS )
    sig.sa_handler = (void (*)(int))AlarmHandler;

 /* M0001 - Added UnixWare Support */

#elif defined ( _DPT_UNIXWARE )
    sig.sa_handler = (void (*)(int))AlarmHandler;

 /* M0001 - Added UnixWare DGUX */
#elif defined ( _DPT_DGUX )
    sig.sa_handler = (void (*)_PROTO_ARGS((int)))AlarmHandler;

#elif defined ( SNI_MIPS )
    sig.sa_handler = (void (*)())AlarmHandler;        
#else
#error Define this for your OS
#endif

#ifdef NEW_LOGGER

#if defined(_DPT_SCO) || defined(_DPT_BSDI) || defined(_DPT_FREE_BSD) || defined(_DPT_LINUX) || defined (_DPT_SOLARIS)
	// ignore sighup, sigterm will come later after a few seconds
	signal(SIGHUP, (void (*) (int)) DptSignalIgnore);
	signal(SIGTERM, (void (*) (int)) DptSignalIgnore);
	signal(SIGCHLD, (void (*) (int)) DptSignalIgnore);
#else
#error Define me.  These are shutdown signals to ignore so the SOC logger
// can write its final heartbeat on a shutdown
#endif

#endif //#ifdef NEW_LOGGER

  /* If The Signaling Could Not Be Set Up, Print An Error And Exit */

     if(sigaction(SIGALRM,&sig,0) == -1)
       {
         if(Verbose)
             printf("\nSignaling Could Not Be Set Up\n");
         RmLock(NULL);
         exit(ExitSignalFail);
       }

  /* if no debug mode selected, start engine as daemon (like inetd) */
  /* i.e. fork son proc and create new session id. - michiz         */
  if (!Verbose && !EataHex && !EataInfo && !DebugEngMsg) {
	int i;
	if ((i = fork()) != 0) {
		ChLock(NULL, i);
		exit(0);
	}
	setsid();
  }


#ifdef _SINIX_ADDON

  /* New option -stop to kill a hanged up engine */
  if (ExitEngine) {
      int i;
      struct msqid_ds CtlBuf;
      MsqID = msgget(DPT_EngineKey, MSG_ALLRD | MSG_ALLWR);
      if(MsqID != -1) {
	  msgctl(MsqID, IPC_STAT, &CtlBuf);
	  // Stop engine only, if no dptmgr still running
	  if ( CtlBuf.msg_lspid && CtlBuf.msg_lrpid &&
	      (getpgid(CtlBuf.msg_lspid) != -1) &&
	      (getpgid(CtlBuf.msg_lrpid) != -1))
		printf("You must stop dptmgr first\n");
	  else {
		i = MessageDPTEngine(DPT_EngineKey, MsqID, ENGMSG_CLOSE, 2,1);
		if (i)
		  msgctl(MsqID, IPC_RMID, &CtlBuf);
		if(Verbose)
		    printf("dpteng successfully stopped\n");
	  }
      }
      RmLock(NULL);
      exit(ExitGoodStatus);
  }
#endif
  /* Check To See If There Is Already An Engine Out There */

    MsqID = CheckForEngine(DPT_EngineKey,1,2);
    if(MsqID != -1)
      {
#ifndef _SINIX_ADDON
        if(Verbose)
#endif
           printf(
"\nThere Appears To Be Another Copy Of dpteng Already Running!\n");
        RmLock(NULL);
        exit(ExitDuplicateEngine);
      }

  /* Get The Process ID To Use As The Unique Caller ID */

    P_ID = getpid();

  /* Try To Create The Unique Message Que Of This ID */

    MsqID = msgget(DPT_EngineKey,IPC_CREAT | IPC_EXCL | MSG_ALLRD | MSG_ALLWR);

  /* If We Could Not Allocate The Message Que, Print A Message And Exit */

    if(MsqID == -1)
      {
        if(Verbose)
            printf("\nThe Message Queue Could Not Be Allocated\n");
        RmLock(NULL);
        exit(ExitMsqAllocFail);
      }

  /* Set Up The Function To Call When The Program Terminates Normally */

    atexit(ProgramUnload);

  /* Loop Forever Waiting For A Header Message To Come In. Once A Header */
  /* Message Is Received, We Will Attach To The Shared Memory Segment    */
  /* That Is Passed In The Header. This Memory Is The In And Out Buffers */

    Done = 0;
    if(Verbose)
        printf("\ndpteng Is Ready.\n");
    while(1)
      {
        //
        // At the top of the loop, we will pull off and process all turnaround messages on the queue since
        // they don't have to be sent down to the engine
        //
        while((Rtnval = (msgrcv(MsqID,(struct msgbuf *)&HdrBuff, MsgDataSize, DPT_TurnAroundKey,IPC_NOWAIT) != -1)))
         {
           Done = ProcessTurnAroundMessage(&HdrBuff);
         }

        //
        // If Done is set, there are no more clients, so let's
        // check for any messages in the queue before we exit. It
        // is possible that someone is trying to load and we don't
        // want to pull the rug out from under him
        //
        if(Done)
         {
           Rtnval = msgrcv(MsqID,(struct msgbuf *)&HdrBuff,
                           MsgDataSize, -DPT_EngineKey,IPC_NOWAIT);
           //
           // If the call failed, were OUT-O-HERE
           // 
           if(Rtnval == -1)
            {
              if(Verbose)
               {
                 printf("\n               : No Clients, Engine Unloads");
               }
               break;
            }

           //
           // There was a message on the queue so continue processing
           //
           Done = 0;
         }

         //
         // Done is not set, so wait for a message to come in
         //
         else {
                while((Rtnval = (msgrcv(MsqID,(struct msgbuf *)&HdrBuff,
                                      MsgDataSize, -DPT_EngineKey,0) == -1)))
                 {

  /* If The Message Failed, And The Reason Was Not An Interrupt, Exit */

                   if(errno != EINTR)
                      break;
                 }
         }

  /* If The Message Received OK Go Process It */

        if(Rtnval == 0)
          {
            //
            // Check to make sure this guy is still out there before processing the message
            // if not, throw away the message
            //
            if(kill(HdrBuff.callerID,0) != 0)
             {
              if(Verbose)
               {
                 printf("\n               : Message received for PID %ld : no process found, discarding",
                         HdrBuff.callerID);
               }
               continue;
             }

  /* If This Is A Turnarround Message, Process it */

            if((HdrBuff.engineTag == HdrBuff.callerID)&&
               (HdrBuff.engineTag == HdrBuff.targetTag))
              {
                 Done = ProcessTurnAroundMessage(&HdrBuff);
                 continue;
              }

  /* Attach To The Shared Memory */

            toEng_P = (uCHAR *)shmat((int)HdrBuff.BufferID,0,0);

  /* Make Sure That We Could Attach */

            if((long)toEng_P != -1)
              {
                 fromEng_P = toEng_P + HdrBuff.FromEngBuffOffset;

                                           /* M0000 : New Verbose Statements */
                 if(Verbose)
                   {
                     FormatTimeString(TimeString,time(0));
                     printf("\nEngine Calls   : %s DPT_CallEngine",TimeString);
                     printf( 
                   "\n                 EngTag = %lx, Event = %lx, Target = %lx",
                       (unsigned long)HdrBuff.engineTag,
                       (unsigned long)HdrBuff.engEvent,
                       (unsigned long)HdrBuff.targetTag);
                     printf( 
                   "\n                 Offset = %lx fromEng_P = %lx toEng_P = %lx",
		       HdrBuff.FromEngBuffOffset, (unsigned long)fromEng_P, (unsigned long)toEng_P);
/*#else
                      "\n                 EngTag = %x, Event = %x, Target = %x",
                       HdrBuff.engineTag,HdrBuff.engEvent,HdrBuff.targetTag);
#endif */
                     fflush(stdout);
                   }

 /* All Went Well With The Receive, So Call The Engine */

                  HdrBuff.result = DPT_Engine( HdrBuff.engineTag,
                                               HdrBuff.engEvent,
                                               HdrBuff.targetTag,
                                               fromEng_P,toEng_P,
                                               HdrBuff.timeOut);

                 //
                 // Detach from the memory segment
                 //
                 shmdt((char *)toEng_P);

                 //
                 // Check to make sure this guy is still out there before returning the message
                 // if not, throw away the message
                 //
                 if(kill(HdrBuff.callerID,0) != 0)
                  {
                   if(Verbose)
                    {
                      printf("\n               : Returning message for PID %ld : no process found, discarding",
                             HdrBuff.callerID);
                    }
                   continue;
                  }

  /* Set Up The Proper Message Type In The Buffer Headers */

                 HdrBuff.MsgID = HdrBuff.callerID;
                 HdrBuff.callerID = DPT_EngineKey;
                 Rtnval = msgsnd(MsqID,(struct msgbuf *)&HdrBuff,MsgDataSize,0);

  /* If We Had An Error Sending, Print A Message And Exit */

                 if(Rtnval == -1)
                   {
                     if(Verbose)
                       {
                         FormatTimeString(TimeString,time(0));
                         printf(
                   "\n%s Error Sending A Message In The Engine : %d\n",
                         TimeString,errno);
                         fflush(stdout);
                       }
                     RmLock(NULL);
                     exit(ExitMsgSendFail);
                   }
              }
/*
else printf("\nError Trying To Attach To The Memory");
*/
          }

  /* If We Had An Error Receiving, Print A Message And Exit */

        else  {
                if(Verbose)
                  {
                    FormatTimeString(TimeString,time(0));
                    printf(
                        "\n%s Error Receiving A Message In The Engine : %d\n",
                        TimeString,errno);
                    fflush(stdout);
                  }
                RmLock(NULL);
                exit(ExitMsgReceiveFail);
              }
      }

  } //main(int argc, char *argv[])
예제 #17
0
/*
 * Select on the list of fds.
 * Returns: -1 = error
 *           0 = timeout or nothing to select
 *          >0 = number of signaled fds
 */
int
_gpgme_io_select ( struct io_select_fd_s *fds, size_t nfds, int nonblock )
{
    HANDLE waitbuf[MAXIMUM_WAIT_OBJECTS];
    int    waitidx[MAXIMUM_WAIT_OBJECTS];
    int code, nwait;
    int i, any;
    int count;
    void *dbg_help;

 restart:
    DEBUG_BEGIN (dbg_help, 3, "select on [ ");
    any = 0;
    nwait = 0;
    count = 0;
    for ( i=0; i < nfds; i++ ) {
        if ( fds[i].fd == -1 ) 
            continue;
        fds[i].signaled = 0;
        if ( fds[i].for_read || fds[i].for_write ) {
            if ( fds[i].frozen ) {
                DEBUG_ADD1 (dbg_help, "f%d ", fds[i].fd );
            }
            else if ( fds[i].for_read ) {
                struct reader_context_s *c = find_reader (fds[i].fd,1);
                
                if (!c) { 
                    DEBUG1 ("oops: no reader thread for fd %d", fds[i].fd);
                }
                else {
                    if ( nwait >= DIM (waitbuf) ) {
                        DEBUG_END (dbg_help, "oops ]");
                        DEBUG0 ("Too many objects for WFMO!" );
                        return -1;
                    }
                    waitidx[nwait]   = i;
                    waitbuf[nwait++] = c->have_data_ev;
                }
                DEBUG_ADD1 (dbg_help, "r%d ", fds[i].fd );
                any = 1;
            }
            else if ( fds[i].for_write ) {
                struct writer_context_s *c = find_writer (fds[i].fd,1);
                
                if (!c) { 
                    DEBUG1 ("oops: no writer thread for fd %d", fds[i].fd);
                }
                else {
                    if ( nwait >= DIM (waitbuf) ) {
                        DEBUG_END (dbg_help, "oops ]");
                        DEBUG0 ("Too many objects for WFMO!" );
                        return -1;
                    }
		    waitidx[nwait]   = i;
		    waitbuf[nwait++] = c->is_empty;
                }
		DEBUG_ADD1 (dbg_help, "w%d ", fds[i].fd );
		any = 1;
            }
        }
    }
    DEBUG_END (dbg_help, "]");
    if (!any) 
        return 0;

    code = WaitForMultipleObjects ( nwait, waitbuf, 0, nonblock ? 0 : 1000);
    if ( code >= WAIT_OBJECT_0 && code < WAIT_OBJECT_0 + nwait ) {
        /* This WFMO is a really silly function:  It does return either
         * the index of the signaled object or if 2 objects have been
         * signalled at the same time, the index of the object with the
         * lowest object is returned - so and how do we find out
         * how many objects have been signaled???.
         * The only solution I can imagine is to test each object starting
         * with the returned index individually - how dull.
         */
        any = 0;
        for (i=code - WAIT_OBJECT_0; i < nwait; i++ ) {
            if (WaitForSingleObject (waitbuf[i], 0) == WAIT_OBJECT_0) {
                assert (waitidx[i] >=0 && waitidx[i] < nfds);
                fds[waitidx[i]].signaled = 1;
                any = 1;
                count++;
            }
        }
        if (!any) {
            DEBUG0 ("Oops: No signaled objects found after WFMO");
            count = -1;
        }
    }
    else if ( code == WAIT_TIMEOUT ) {
        DEBUG0 ("WFMO timed out\n" );
    }  
    else if (code == WAIT_FAILED ) {
        int le = (int)GetLastError ();
        if ( le == ERROR_INVALID_HANDLE ) {
            int k, j = handle_to_fd (waitbuf[i]);
                    
            DEBUG1 ("WFMO invalid handle %d removed\n", j);
            for (k=0 ; k < nfds; k++ ) {
                if ( fds[k].fd == j ) {
                    fds[k].for_read = fds[k].for_write = 0;
                    goto restart;
                }
            }
            DEBUG0 (" oops, or not???\n");
        }
        DEBUG1 ("WFMO failed: %d\n", le );
        count = -1;
    }
    else {
        DEBUG1 ("WFMO returned %d\n", code );
        count = -1;
    }

    if ( count ) {
        DEBUG_BEGIN (dbg_help, 3, " signaled [ ");
        for ( i=0; i < nfds; i++ ) {
            if ( fds[i].fd == -1 ) 
                continue;
            if ( (fds[i].for_read || fds[i].for_write) && fds[i].signaled ) {
                DEBUG_ADD2 (dbg_help, "%c%d ",
                            fds[i].for_read? 'r':'w',fds[i].fd );
            }
        }
        DEBUG_END (dbg_help, "]");
    }
    
    return count;
}
void SlidingReducedOrderObserver::initialize(const NonSmoothDynamicalSystem& nsds,
                                             const Simulation& s)
{
  DEBUG_BEGIN(" SlidingReducedOrderObserver::initialize(const NonSmoothDynamicalSystem& nsds, const Simulation& s)\n");
  if (!_C)
  {
    RuntimeException::selfThrow("SlidingReducedOrderObserver::initialize - you have to set C before initializing the Observer");
  }
  else
  {
    Observer::initialize(nsds, s);
  }
  bool isDSinDSG0 = true;
  DynamicalSystemsGraph& originalDSG0 = *nsds.topology()->dSG(0);
  DynamicalSystemsGraph::VDescriptor originaldsgVD;
  if (!_DS) // No DynamicalSystem was given
  {
    // We can only work with FirstOrderNonLinearDS, FirstOrderLinearDS and FirstOrderLinearTIDS
    // We can use the Visitor mighty power to check if we have the right type
    DynamicalSystem& observedDS = *_sensor->getDS();
    Type::Siconos dsType;
    dsType = Type::value(observedDS);
    // create the DS for the controller
    // if the DS we use is different from the DS we are controlling
    // when we want for instant to see how well the controller behaves
    // if the plant model is not exact, we can use the setSimulatedDS
    // method
    if (dsType == Type::FirstOrderLinearDS)
    {
      DEBUG_PRINT("dsType == Type::FirstOrderLinearDS\n");
      _DS.reset(new FirstOrderLinearDS(static_cast<FirstOrderLinearDS&>(observedDS)));
    }
    else if (dsType == Type::FirstOrderLinearTIDS)
    {
      DEBUG_PRINT("dsType == Type::FirstOrderLinearTIDS\n");
      _DS.reset(new FirstOrderLinearTIDS(static_cast<FirstOrderLinearTIDS&>(observedDS)));
    }
    else
      RuntimeException::selfThrow("SlidingReducedOrderObserver is not yet implemented for system of type" + dsType);

    // is it controlled ?
    originaldsgVD = originalDSG0.descriptor(_sensor->getDS());
  }
  else
  {
    // is it controlled ?
    if (originalDSG0.is_vertex(_DS))
      originaldsgVD = originalDSG0.descriptor(_DS);
    else
      isDSinDSG0 = false;
  }

  // Initialize with the guessed state
  _DS->setX0Ptr(_xHat);
  _DS->resetToInitialState();
  _e.reset(new SiconosVector(_C->size(0)));
  _y.reset(new SiconosVector(_C->size(0)));

  double t0 = nsds.t0();
  double h = s.currentTimeStep();
  double T = nsds.finalT() + h;
  _nsds.reset(new NonSmoothDynamicalSystem(t0, T));
  _integrator.reset(new ZeroOrderHoldOSI());
  std11::static_pointer_cast<ZeroOrderHoldOSI>(_integrator)->setExtraAdditionalTerms(
      std11::shared_ptr<ControlZOHAdditionalTerms>(new ControlZOHAdditionalTerms()));
  _nsds->insertDynamicalSystem(_DS);

  // Add the necessary properties
  DynamicalSystemsGraph& DSG0 = *_nsds->topology()->dSG(0);
  DynamicalSystemsGraph::VDescriptor dsgVD = DSG0.descriptor(_DS);
  // Observer part
  DSG0.L[dsgVD] = _L;
  DSG0.e[dsgVD] = _e;

  // Was the original DynamicalSystem controlled ?
  if (isDSinDSG0 && originalDSG0.B.hasKey(originaldsgVD))
  {
    DSG0.B[dsgVD] = originalDSG0.B[originaldsgVD];
    assert(originalDSG0.u[originaldsgVD] && "A DynamicalSystem is controlled but its control input has not been initialized yet");
    DSG0.u[dsgVD] = originalDSG0.u[originaldsgVD];
  }

  // all necessary things for simulation
  _simulation.reset(new TimeStepping(_nsds, _td, 0));
  _simulation->setName("Internal simultion of SlidingReducedOrderObserver");
  _simulation->associate(_integrator, _DS);

  // initialize error
  *_y = _sensor->y();
  DEBUG_END(" SlidingReducedOrderObserver::initialize(const NonSmoothDynamicalSystem& nsds, const Simulation& s)\n");
}
예제 #19
0
// Fill the matrix
void OSNSMatrix::fill(SP::InteractionsGraph indexSet, bool update)
{
  DEBUG_BEGIN("void OSNSMatrix::fill(SP::InteractionsGraph indexSet, bool update)\n");
  assert(indexSet);

  if (update)
  {
    // Computes _dimRow and interactionBlocksPositions according to indexSet
    _dimColumn = updateSizeAndPositions(indexSet);
    _dimRow = _dimColumn;
  }

  if (_storageType == NM_DENSE)
  {

    // === Memory allocation, if required ===
    // Mem. is allocate only if !M or if its size has changed.
    if (update)
    {
      if (! _M1)
        _M1.reset(new SimpleMatrix(_dimRow, _dimColumn));
      else
      {
        if (_M1->size(0) != _dimRow || _M1->size(1) != _dimColumn)
          _M1->resize(_dimRow, _dimColumn);
        _M1->zero();
      }
    }

    // ======> Aim: find inter1 and inter2 both in indexSet and which have
    // common DynamicalSystems.  Then get the corresponding matrix
    // from map interactionBlocks, and copy it into M

    unsigned int pos = 0, col = 0; // index position used for
    // interactionBlock copy into M, see
    // below.
    // === Loop through "active" Interactions (ie present in
    // indexSets[level]) ===
    InteractionsGraph::VIterator vi, viend;
    for (std11::tie(vi, viend) = indexSet->vertices();
         vi != viend; ++vi)
    {
      SP::Interaction inter = indexSet->bundle(*vi);
      pos = inter->absolutePosition();

      std11::static_pointer_cast<SimpleMatrix>(_M1)
      ->setBlock(pos, pos, *indexSet->properties(*vi).block);
      DEBUG_PRINTF("OSNSMatrix _M1: %i %i\n", _M1->size(0), _M1->size(1));
      DEBUG_PRINTF("OSNSMatrix block: %i %i\n", indexSet->properties(*vi).block->size(0), indexSet->properties(*vi).block->size(1));
    }

    InteractionsGraph::EIterator ei, eiend;
    for (std11::tie(ei, eiend) = indexSet->edges();
         ei != eiend; ++ei)
    {
      InteractionsGraph::VDescriptor vd1 = indexSet->source(*ei);
      InteractionsGraph::VDescriptor vd2 = indexSet->target(*ei);

      SP::Interaction inter1 = indexSet->bundle(vd1);
      SP::Interaction inter2 = indexSet->bundle(vd2);

      pos = inter1->absolutePosition();

      assert(indexSet->is_vertex(inter2));

      col = inter2->absolutePosition();


      assert(pos < _dimRow);
      assert(col < _dimColumn);

      DEBUG_PRINTF("OSNSMatrix _M1: %i %i\n", _M1->size(0), _M1->size(1));
      DEBUG_PRINTF("OSNSMatrix upper: %i %i\n", indexSet->properties(*ei).upper_block->size(0), indexSet->properties(*ei).upper_block->size(1));
      DEBUG_PRINTF("OSNSMatrix lower: %i %i\n", indexSet->properties(*ei).lower_block->size(0), indexSet->properties(*ei).lower_block->size(1));

      assert(indexSet->properties(*ei).lower_block);
      assert(indexSet->properties(*ei).upper_block);
      std11::static_pointer_cast<SimpleMatrix>(_M1)
      ->setBlock(std::min(pos, col), std::max(pos, col),
                 *indexSet->properties(*ei).upper_block);

      std11::static_pointer_cast<SimpleMatrix>(_M1)
      ->setBlock(std::max(pos, col), std::min(pos, col),
                 *indexSet->properties(*ei).lower_block);
    }

  }
  else if (_storageType == NM_SPARSE_BLOCK)
  {
    if (! _M2)
    {
      DEBUG_PRINT("Reset _M2 shared pointer using new BlockCSRMatrix(indexSet) \n ");
      _M2.reset(new BlockCSRMatrix(indexSet));

    }
    else
    {
      DEBUG_PRINT("fill existing _M2\n");
      _M2->fill(indexSet);
    }
  }

  if (update)
    convert();
  DEBUG_END("void OSNSMatrix::fill(SP::InteractionsGraph indexSet, bool update)\n");
}
void NewtonEulerFrom1DLocalFrameR::computeJachq(double time, Interaction& inter, SP::BlockVector q0)
{

  DEBUG_BEGIN("NewtonEulerFrom1DLocalFrameR::computeJachq(double time, Interaction& inter, SP::BlockVector q0 ) \n");
  DEBUG_PRINTF("with time =  %f\n",time);
  DEBUG_PRINTF("with inter =  %p\n",&inter);


  _jachq->setValue(0, 0, _Nc->getValue(0));
  _jachq->setValue(0, 1, _Nc->getValue(1));
  _jachq->setValue(0, 2, _Nc->getValue(2));
  if (inter.has2Bodies())
  {
    _jachq->setValue(0, 7, -_Nc->getValue(0));
    _jachq->setValue(0, 8, -_Nc->getValue(1));
    _jachq->setValue(0, 9, -_Nc->getValue(2));
  }

  for (unsigned int iDS =0 ; iDS < q0->getNumberOfBlocks()  ; iDS++)
  {
    SP::SiconosVector q = (q0->getAllVect())[iDS];
    double sign = 1.0;
    DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq : ds%d->q :", iDS);
    DEBUG_EXPR_WE(q->display(););

    ::boost::math::quaternion<double>    quatGP;
    if (iDS == 0)
    {
      ::boost::math::quaternion<double>    quatAux(0, _Pc1->getValue(0) - q->getValue(0), _Pc1->getValue(1) - q->getValue(1),
                                                   _Pc1->getValue(2) - q->getValue(2));
      quatGP = quatAux;
    }
    else
    {
      sign = -1.0;
      //cout<<"NewtonEulerFrom1DLocalFrameR::computeJachq sign is -1 \n";
      ::boost::math::quaternion<double>    quatAux(0, _Pc2->getValue(0) - q->getValue(0), _Pc2->getValue(1) - q->getValue(1),
                                                   _Pc2->getValue(2) - q->getValue(2));
      quatGP = quatAux;
    }
    DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq :GP :%lf, %lf, %lf\n", quatGP.R_component_2(), quatGP.R_component_3(), quatGP.R_component_4());
    DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq :Q :%e,%e, %e, %e\n", q->getValue(3), q->getValue(4), q->getValue(5), q->getValue(6));
    ::boost::math::quaternion<double>    quatQ(q->getValue(3), q->getValue(4), q->getValue(5), q->getValue(6));
    ::boost::math::quaternion<double>    quatcQ(q->getValue(3), -q->getValue(4), -q->getValue(5), -q->getValue(6));
    ::boost::math::quaternion<double>    quat0(1, 0, 0, 0);
    ::boost::math::quaternion<double>    quatBuff;
    ::boost::math::quaternion<double>    _2qiquatGP;
    _2qiquatGP = quatGP;
    _2qiquatGP *= 2 * (q->getValue(3));
    quatBuff = (quatGP * quatQ) + (quatcQ * quatGP) - _2qiquatGP;

    DEBUG_PRINTF("NewtonEulerFrom1DLocalFrameR::computeJachq :quattBuuf : %e,%e,%e \n", quatBuff.R_component_2(), quatBuff.R_component_3(), quatBuff.R_component_4());

    _jachq->setValue(0, 7 * iDS + 3, sign * (quatBuff.R_component_2()*_Nc->getValue(0) +
                                             quatBuff.R_component_3()*_Nc->getValue(1) + quatBuff.R_component_4()*_Nc->getValue(2)));
    //cout<<"WARNING NewtonEulerFrom1DLocalFrameR set jachq \n";
    //_jachq->setValue(0,7*iDS+3,0);
    for (unsigned int i = 1; i < 4; i++)
    {
      ::boost::math::quaternion<double>    quatei(0, (i == 1) ? 1 : 0, (i == 2) ? 1 : 0, (i == 3) ? 1 : 0);
      _2qiquatGP = quatGP;
      _2qiquatGP *= 2 * (q->getValue(3 + i));
      quatBuff = quatei * quatcQ * quatGP - quatGP * quatQ * quatei - _2qiquatGP;
      _jachq->setValue(0, 7 * iDS + 3 + i, sign * (quatBuff.R_component_2()*_Nc->getValue(0) +
                                                   quatBuff.R_component_3()*_Nc->getValue(1) + quatBuff.R_component_4()*_Nc->getValue(2)));
    }
  }
예제 #21
0
파일: graph.c 프로젝트: Forilan/fflp
void show_plugin_graph() {
	DEBUG_BEGIN();
	plugin_graph.debug(print_plugin_info);
	DEBUG_END();
}
예제 #22
0
void rotateAbsToBody(double q0, double q1, double q2, double q3, SP::SiconosVector v )
{
  DEBUG_BEGIN("::rotateAbsToBody(double q0, double q1, double q2, double q3, SP::SiconosVector v )\n");
  DEBUG_EXPR(v->display(););
예제 #23
0
void standWatch() {
  DEBUG_PRINT(F("Sailing for "));
  DEBUG_PRINT(sailingOrders.loggingInterval);
  DEBUG_PRINTLN(F(" seconds."));

  trimResult = {.success = true,
                .sailStuck = false,
                .trimRoutineExceededMax = false,
                .sailBatteryTooLow = false};

  if (battery2.GetVoltage() < MINIMUM_BATTERY_VOLTAGE) {
    trimResult.success = false;
    trimResult.sailBatteryTooLow = true;
  }

  uint32_t elapsedTime = 0; // used to track seconds during sail operation
  int16_t currentOrderedSailPosition;

  while (elapsedTime < sailingOrders.loggingInterval) {
    if ((tackIsA && currentTackTime >= sailingOrders.orderedTackTimeA) ||
        (!tackIsA && currentTackTime >= sailingOrders.orderedTackTimeB)) {
      tackIsA = !tackIsA; // change tacks
      currentTackTime = 0;
      DEBUG_PRINTLN(F("Changing tacks"));
    }

    if (tackIsA) {
      currentOrderedSailPosition = sailingOrders.orderedSailPositionA;
      DEBUG_PRINTLN(F("Sailing on tack A"));
    } else {
      currentOrderedSailPosition = sailingOrders.orderedSailPositionB;
      DEBUG_PRINTLN(F("Sailing on tack B"));
    }

    if (trimResult.success) {
      if (abs(sail.GetSailPosition() - currentOrderedSailPosition) > 5) {
        trimResult = sail.Trim(currentOrderedSailPosition);
      } else {
        DEBUG_PRINTLN(F("Close enough, no need to trim."));
      }
    } else {
      DEBUG_PRINTLN(F("Unsuccessful sail trim."));
    }

    delay(DELAY_FOR_SERIAL);
    blinker.Blink(2);
    sleeper.sleepDelay(6000);
    currentTackTime += 6;
    elapsedTime += 6;

    DEBUG_PRINT(F("elapsedTime = "));
    DEBUG_PRINT(elapsedTime);
    DEBUG_PRINT(F(" | currentTackTime = "));
    DEBUG_PRINTLN(currentTackTime);
  }
}

void setup() {
  initPins();
  sleeper.pwrDownMode(); // lowest power setting for sleeping

  DEBUG_BEGIN(CONSOLE_BAUD);
  DEBUG_PRINTLN(F("Satcom test starting..."));

  runNum = GbUtility::IncrementRunNum();
  DEBUG_PRINT(F("Starting runNum "));
  DEBUG_PRINTLN(runNum);

  // uncomment for real program
  // gb_satcom.SetUpSat(SAT_CHARGE_TIME, ISBD_TIMEOUT);

  delay(500);
  sleeper.sleepDelay(1000); // so the subsequent sleep works
  delay(500);
}