// 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"); }
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"); }
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(););
//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); }
// 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"); }
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"); }
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"); }
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"); }
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"); }
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"); }
// ====================== // 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; }
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(););
/* * 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; }
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[])
/* * 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"); }
// 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))); } }
void show_plugin_graph() { DEBUG_BEGIN(); plugin_graph.debug(print_plugin_info); DEBUG_END(); }
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(););
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); }