// 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);
}