TrikGyroscopeAdapter::TrikGyroscopeAdapter(kitBase::robotModel::robotParts::GyroscopeSensor *gyro
, const QSharedPointer<trik::robotModel::twoD::TrikTwoDRobotModel> &model)
: mTimeInited(false)
, mGyro(gyro)
, mModel(model)
, mResult(7, 0)
, mQuaternion(1, 0, 0, 0)
, mStartTime(getTimeValue(model.data()))
, mLastUpdateTimeStamp(getTimeValue(model.data()))
{
connect(gyro, SIGNAL(newData(QVector<int>)), this, SLOT(countTilt(QVector<int>)));
}
void CTimelineControl::_OnLButtonDown( uiMouseEvent mouseEvent, int x, int y )
{
this->setCapture();
m_lbuttonDown = true;
if ( m_changeTimeAndValue )
{
m_currentTime = getTimeValue( x - OFFSET_X + m_crollX );
// check new select time
int oldSelect = m_selectTimeID;
bool isSelectTime = checkSelectTime( x, y );
if ( oldSelect != -1 && isSelectTime && oldSelect == m_selectTimeID )
{
m_mouseActionState = k_mouseActionChangeTime;
}
else if ( checkSelectXYZ( x, y ) == true )
{
m_mouseActionState = k_mouseActionChangeValue;
}
update();
updateCursor( x, y);
}
}
/** Checks the given value is a valid date
* @param value :: input date property to validate
* @return a string which describes the error else ""
*/
std::string DateValidator::checkValidity(const std::string &value) const {
// empty strings are allowed
if (value.empty()) {
return "";
}
std::string formaterror;
struct tm timeinfo = getTimeValue(value, formaterror);
if (!formaterror.empty()) {
return formaterror;
}
if (timeinfo.tm_mday < 1 || timeinfo.tm_mday > 31) {
return "Invalid Date:Day part of the Date parameter must be between 1 and "
"31";
}
if (timeinfo.tm_mon < 0 || timeinfo.tm_mon > 11) {
return "Invalid Date:Month part of the Date parameter must be between 1 "
"and 12";
}
// get current time
time_t rawtime;
time(&rawtime);
struct tm *currenttime = localtime(&rawtime);
if (timeinfo.tm_year > currenttime->tm_year) {
return "Invalid Date:Year part of the Date parameter can not be greater "
"than the current year";
}
return "";
}
void
QSS_PAR_initialize (SIM_simulator simulate)
{
QSS_simulator simulator = (QSS_simulator) simulate->state->sim;
QSS_PAR_allocRootSimulatorData (simulator);
getTime (simulator->iTime);
PRT_partition partition = PRT_Partition (simulator->data,
simulator->output->name);
getTime (simulator->sdTime);
subTime (simulator->sdTime, simulator->iTime);
simulator->stats->partitioningTime = getTimeValue (simulator->sdTime);
getTime (simulator->iTime);
LP_initializeDataStructs (simulator, partition);
getTime (simulator->sdTime);
subTime (simulator->sdTime, simulator->iTime);
PRT_freePartition (partition);
QSS_PAR_logMemory (simulator);
simulator->stats->initializeLPS = getTimeValue (simulator->sdTime);
}
void CTimelineControl::_OnMouseMove( uiMouseEvent mouseEvent, int x, int y )
{
if ( m_changeTimeAndValue )
{
if ( m_lbuttonDown )
{
if ( m_mouseActionState == k_mouseActionChangeTime )
{
// drag to change time
updateChangeTime( x, y );
}
else if ( m_mouseActionState == k_mouseActionChangeValue )
{
// drag to change value
updateChangeValue( x,y );
}
else
{
// drag
m_currentTime = getTimeValue( x - OFFSET_X + m_crollX );
checkSelectTime( x, y );
}
update();
}
}
if ( m_rbuttonDown )
{
int nWidth = getClientWidth();
float timeWidth = m_lengthPixel*m_timeLength;
int limitW = (int)timeWidth + OFFSET_X * 2;
if ( nWidth < limitW )
{
int dx = x - m_x;
m_crollX -= dx;
if ( m_crollX < 0 )
m_crollX = 0;
else if ( nWidth + m_crollX > limitW )
{
m_crollX = limitW - nWidth;
}
}
update();
}
m_x = x;
m_y = y;
// update cursor icon
updateCursor( x, y);
}
void
QSS_PAR_saveLog (QSS_simulator simulator)
{
int outputs = simulator->data->lp->outputs;
if (outputs)
{
getTime (simulator->sTime);
OUT_save (simulator->log);
getTime (simulator->sdTime);
subTime (simulator->sdTime, simulator->sTime);
simulator->saveTime = getTimeValue (simulator->sdTime);
}
}
void TrikGyroscopeAdapter::countTilt(const QVector<int> &oldFormat)
{
const quint64 timeStamp = getTimeValue(mModel.data());
if (!mTimeInited) {
mTimeInited = true;
mLastUpdateTimeStamp = timeStamp;
} else {
mResult[0] = oldFormat[0];
mResult[1] = oldFormat[1];
mResult[2] = oldFormat[2];
mResult[3] = convertToTrikRuntimeTime(timeStamp);
const qreal scale = static_cast<qreal>(timeStamp - mLastUpdateTimeStamp) / twoDModel::timeQuant;
const qreal x = static_cast<qreal>(mResult[0]) / twoDModel::gyroscopeConstant * scale;
const qreal y = static_cast<qreal>(mResult[1]) / twoDModel::gyroscopeConstant * scale;
const qreal z = static_cast<qreal>(mResult[2]) / twoDModel::gyroscopeConstant * scale;
mLastUpdateTimeStamp = timeStamp;
const float c1 = static_cast<float>(qCos(x / 2));
const float s1 = static_cast<float>(qSin(x / 2));
const float c2 = static_cast<float>(qCos(y / 2));
const float s2 = static_cast<float>(qSin(y / 2));
const float c3 = static_cast<float>(qCos(z / 2));
const float s3 = static_cast<float>(qSin(z / 2));
QQuaternion deltaQuaternion;
deltaQuaternion.setScalar(c1 * c2 * c3 + s1 * s2 * s3);
deltaQuaternion.setX(s1 * c2 * c3 - c1 * s2 * s3);
deltaQuaternion.setY(c1 * s2 * c3 + s1 * c2 * s3);
deltaQuaternion.setZ(c1 * c2 * s3 - s1 * s2 * c3);
mQuaternion *= deltaQuaternion;
mQuaternion.normalize();
mResult[4] = static_cast<int>(degreeToMilidegree(qRadiansToDegrees(getPitch<qreal>(mQuaternion))));
mResult[5] = static_cast<int>(degreeToMilidegree(qRadiansToDegrees(getRoll<qreal>(mQuaternion))));
mResult[6] = static_cast<int>(degreeToMilidegree(qRadiansToDegrees(getYaw<qreal>(mQuaternion))));
}
}
int SMO_getTimeList(SMOutputAPI* smoapi, double* array)
//
// Purpose: Return list of all times corresponding to computed results in decimal days since 12/13/1899.
// Note that the initial conditions (time 0) are not included in the file.
{
int k;
if (smoapi->isOpened)
{
if (array == NULL) return 411;
// loop over and build time series
for (k = 0; k < smoapi->Nperiods; k++)
array[k] = getTimeValue(smoapi, k);
return 0;
}
// Error no results to report on binary file not opened
return 412;
}
void CTimelineControl::updateChangeTime( int x, int y )
{
if ( m_selectTimeID == -1 )
return;
float deltaFrame = 0.4f;
if ( m_selectTimeID > 0 && m_selectTimeID != m_value.size() - 1 )
{
float left = m_value[m_selectTimeID-1].time;
float right = m_value[m_selectTimeID+1].time;
float dragTime = getTimeValue( x - OFFSET_X + m_crollX );
if ( dragTime < left + deltaFrame )
dragTime = left + deltaFrame;
if ( dragTime > right - deltaFrame )
dragTime = right - deltaFrame;
m_value[m_selectTimeID].time = dragTime;
}
}
void
DASSL_integrate (SIM_simulator simulate)
{
CLC_simulator simulator = (CLC_simulator) simulate->state->sim;
clcData = simulator->data;
clcModel = simulator->model;
simOutput = simulator->output;
int i;
double t = clcData->it;
double tout;
const double _ft = clcData->ft;
double dQRel = clcData->dQRel[0];
double dQMin = clcData->dQMin[0];
double *_x = clcData->x;
double *rwork;
int is_sampled = simOutput->commInterval != CI_Step;
double step_size;
if (is_sampled)
{
step_size = simOutput->sampled->period[0];
}
const int num_steps = (
is_sampled ? ceil (_ft / step_size) + 2 : MAX_OUTPUT_POINTS);
double **solution = checkedMalloc (sizeof(double*) * simOutput->outputs);
double *solution_time = checkedMalloc (sizeof(double) * num_steps);
double **outvar = checkedMalloc (sizeof(double) * simOutput->outputs);
int info[20], lrw, liw, *iwork;
double *x, *dx, rel_tol = dQRel, abs_tol = dQMin;
int numofconstr = clcData->events, method_info = 0;
int *root_output;
int size = clcData->states;
int event_detected = 0;
x = checkedMalloc (sizeof(double) * clcData->states);
dx = checkedMalloc (sizeof(double) * clcData->states);
root_output = checkedMalloc (sizeof(int) * clcData->events);
lrw = 5000 + 15000 * clcData->states
+ /*clcData->states * clcData->states +*/8 * clcData->events;
rwork = checkedMalloc (sizeof(double) * lrw);
CLC_compute_outputs (simOutput, solution, num_steps);
for (i = 0; i < clcData->states; i++)
x[i] = _x[i];
cleanDoubleVector (dx, 0, clcData->states);
cleanVector (root_output, 0, clcData->events);
cleanVector (info, 0, 20);
if (!is_sampled)
{
info[2] = 1;
}
liw = 60040;
iwork = checkedMalloc (sizeof(int) * liw);
int percentage = 0;
// Save first step
CLC_save_step (simOutput, solution, solution_time, t,
clcData->totalOutputSteps, x, clcData->d, clcData->alg);
clcData->totalOutputSteps++;
getTime (simulator->sTime);
#ifdef SYNC_RT
setInitRealTime();
#endif
while (t < _ft)
{
if (!is_sampled)
{
tout = _ft;
}
else
{
if (!event_detected)
{
tout = t + step_size;
}
else
{
if (fabs (tout - t) < 1e-12)
{
CLC_save_step (simOutput, solution, solution_time, tout,
clcData->totalOutputSteps, x, clcData->d,
clcData->alg);
clcData->totalOutputSteps++;
tout = t + step_size;
}
}
event_detected = 0;
}
if (tout > _ft)
tout = _ft;
ddaskr_ (DASSL_model, &size, &t, x, dx, &tout, info, &rel_tol, &abs_tol,
&method_info, rwork, &lrw, iwork, &liw,
NULL,
NULL, NULL, NULL, DASSL_events, &numofconstr, root_output);
if (method_info < 0)
{
printf (
"Error: DASSL returned IDID = %d. Check DASSL documentation\n",
method_info);
exit (-1);
}
#ifdef SYNC_RT
/* Sync */
waitUntil(t);
#endif
if (method_info == 5)
{
CLC_handle_event (clcData, clcModel, x, root_output, t, iwork);
if (is_sampled)
event_detected = 1;
info[0] = 0;
}
if (!is_sampled)
{
CLC_save_step (simOutput, solution, solution_time, t,
clcData->totalOutputSteps, x, clcData->d,
clcData->alg);
clcData->totalOutputSteps++;
}
else
{
if (!event_detected)
{
if (fabs (tout - solution_time[clcData->totalOutputSteps - 1])
> step_size / 10)
{
CLC_save_step (simOutput, solution, solution_time, tout,
clcData->totalOutputSteps, x, clcData->d,
clcData->alg);
clcData->totalOutputSteps++;
}
}
else
{
}
}
if ((int) (t * 100 / _ft) > percentage)
{
percentage = 100 * t / _ft;
fprintf (stderr, "*%g", t);
fflush (stderr);
}
}
/*
if (!event_detected && is_sampled) {
if (solution_time[totalOutputSteps]<t) {
CLC_save_step(simOutput,solution,solution_time,t,totalOutputSteps,x, clcData->d);
totalOutputSteps++;
}
}
*/
clcData->totalSteps += iwork[10];
clcData->totalStepsDASSL += iwork[11];
clcData->totalJacobians += iwork[12];
clcData->totalCrossingEvaluations += iwork[35];
getTime (simulator->sTime);
subTime (simulator->sTime, simulator->iTime);
if (simulator->settings->debug == 0 || simulator->settings->debug > 1)
{
SD_print (simulator->simulationLog, "Simulation time (DASSL):");
SD_print (simulator->simulationLog, "----------------");
SD_print (simulator->simulationLog, "Miliseconds: %g",
getTimeValue (simulator->sTime));
SD_print (simulator->simulationLog, "Function evaluations: %llu",
clcData->funEvaluations);
//SD_print (simulator->simulationLog, "Scalar function evaluations: %d", clcData->scalarEvaluations);
//SD_print (simulator->simulationLog, "Zero Crossings : %d", clcData->zeroCrossings);
SD_print (simulator->simulationLog,
"Function evaluations (reported by DASSL): %d",
clcData->totalStepsDASSL);
SD_print (simulator->simulationLog, "Jacobian evaluations : %d",
clcData->totalJacobians);
SD_print (simulator->simulationLog, "Zero crossing evaluations : %d",
clcData->totalCrossingEvaluations);
SD_print (simulator->simulationLog, "Output steps: %d",
clcData->totalOutputSteps);
SD_print (simulator->simulationLog, "Simulation steps: %d",
clcData->totalSteps);
SD_print (simulator->simulationLog, "Events detected : %d",
clcData->totalEvents);
}
CLC_write_output (simOutput, solution, solution_time,
clcData->totalOutputSteps);
// To avoid QSS output
free (x);
free (dx);
free (outvar);
free (root_output);
free (solution_time);
free (rwork);
free (iwork);
for (i = 0; i < simOutput->outputs; i++)
{
free (solution[i]);
}
free (solution);
}
void
QSS_SEQ_initialize (SIM_simulator simulate)
{
QSS_simulator simulator = (QSS_simulator) simulate->state->sim;
getTime (simulator->iTime);
int i, j, k, s, forUL;
double e, zc[4];
simulator->frw = FRW_Framework (simulator->data);
double t = simulator->time->time;
char logFile[128];
strcpy (logFile, simulator->output->name);
simulator->simulationLog = SD_SimulationLog (logFile);
// Local mappings.
QSS_data qssData = simulator->data;
QSS_time qssTime = simulator->time;
FRW_framework frw = simulator->frw;
double *q = qssData->q;
double *x = qssData->x;
const int order = qssData->order;
const int coeffs = order + 1;
#ifdef DEBUG
SD_simulationSettings settings = simulator->settings;
SD_simulationLog simulationLog = simulator->simulationLog;
#endif
QSS_model qssModel = simulator->model;
QA_quantizer quantizer;
SD_output output = simulator->output;
#ifdef DEBUG
if (settings->debug & SD_DBG_InitValues)
{
SD_print (simulationLog, "Initialize simulation\n");
}
#endif
forUL = qssData->states;
for (i = 0; i < forUL; i++)
{
qssData->lqu[i] = qssData->dQRel[i] * fabs (x[i * coeffs]);
if (qssData->lqu[i] < qssData->dQMin[i])
{
qssData->lqu[i] = qssData->dQMin[i];
}
#ifdef DEBUG
if (settings->debug & SD_DBG_InitValues)
{
SD_print (simulationLog, "Initial value: x[%d][0] = %g", i, x[i * coeffs]);
}
#endif
}
#ifdef DEBUG
if (settings->debug & SD_DBG_InitValues)
{
SD_print (simulationLog, "Initialize solver...");
}
#endif
simulator->quantizer = QA_Quantizer (qssData, qssTime);
quantizer = simulator->quantizer;
#ifdef DEBUG
if (settings->debug & SD_DBG_InitValues)
{
SD_print (simulationLog, "done.");
SD_print (simulationLog, "Initialize state derivatives...");
}
#endif
forUL = qssData->states;
for (i = 0; i < forUL; i++)
{
FRW_recomputeDerivative (frw, qssModel, qssData, qssTime, i);
QA_nextTime (quantizer,i,t,qssTime->nextStateTime,x,qssData->lqu);
#ifdef DEBUG
if (settings->debug & SD_DBG_InitValues)
{
SD_print (simulationLog, "Initial derivative: x[%d][1] = %g", i,
x[i * coeffs + 1]);
}
#endif
}
#ifdef DEBUG
if (settings->debug & SD_DBG_InitValues)
{
SD_print (simulationLog, "done.");
SD_print (simulationLog, "Initialize input...");
}
#endif
forUL = qssData->inputs;
for (i = 0; i < forUL; i++)
{
j = qssData->TD[i];
FRW_nextInputTime (frw, qssModel, qssData, qssTime, 0, j, i);
}
#ifdef DEBUG
if (settings->debug & SD_DBG_InitValues)
{
SD_print (simulationLog, "done.");
SD_print (simulationLog, "Initialize events...");
}
#endif
forUL = qssData->events;
for (i = 0; i < forUL; i++)
{
if (qssData->nZS[i])
{
int nZS = qssData->nZS[i];
e = INF;
for (j = 0; j < nZS; j++)
{
k = qssData->ZS[i][j];
if (qssData->dQMin[k] < e)
{
e = qssData->dQMin[k];
}
}
}
else
{
e = qssData->ft * qssData->params->zcHyst;
}
qssModel->events->zeroCrossing (i, q, qssData->d, qssData->alg,
qssTime->time, zc);
s = sign (zc[0]);
qssData->event[i].zcSign = s;
qssData->event[i].zcHyst = e / 10.0;
if (qssData->event[i].direction == 0 || qssData->event[i].direction == s
|| zc[0] == 0)
{
if (zc[0] == 0 && qssData->event[i].relation == 3)
{
qssModel->events->handlerPos (i, q, qssData->d, qssData->alg, t);
}
else if (zc[0] == 0 && qssData->event[i].relation == 1)
{
qssModel->events->handlerNeg (i, q, qssData->d, qssData->alg, t);
}
else if (s >= 0)
{
qssModel->events->handlerPos (i, q, qssData->d, qssData->alg, t);
}
else
{
qssModel->events->handlerNeg (i, q, qssData->d, qssData->alg, t);
}
qssModel->events->zeroCrossing (i, q, qssData->d, qssData->alg, t,
zc);
qssData->event[i].zcSign = sign (zc[0]);
int nHZ = qssData->nHZ[i];
for (k = 0; k < nHZ; k++)
{
j = qssData->HZ[i][k];
FRW_nextEventTime (frw, qssModel, qssData, qssTime, j);
}
int nHD = qssData->nHD[i];
for (k = 0; k < nHD; k++)
{
j = qssData->HD[i][k];
e = t - qssTime->tx[j];
if (e > 0)
{
int cf0 = j * coeffs;
x[cf0] = evaluatePoly (cf0, e, x, order);
}
qssTime->tx[j] = t;
FRW_recomputeDerivative (frw, qssModel, qssData, qssTime, j);
}
if (nHD)
{
QA_recomputeNextTimes (quantizer, nHD, qssData->HD[i], t,
qssTime->nextStateTime, x, qssData->lqu,
q);
}
}
FRW_nextEventTime (frw, qssModel, qssData, qssTime, i);
}
#ifdef DEBUG
if (settings->debug & SD_DBG_InitValues)
{
SD_print (simulationLog, "done.");
SD_print (simulationLog, "Initialize state variables time...");
}
#endif
forUL = qssData->states;
for (i = 0; i < forUL; i++)
{
QA_recomputeNextTime (quantizer, i, qssTime->time, qssTime->nextStateTime,
x, qssData->lqu, q);
}
#ifdef DEBUG
if (settings->debug & SD_DBG_InitValues)
{
SD_print (simulationLog, "done.");
SD_print (simulationLog, "Initialize output...");
}
#endif
simulator->log = OUT_Output (qssData, qssTime, output);
#ifdef DEBUG
if (settings->debug & SD_DBG_InitValues)
{
SD_print (simulationLog, "done.");
SD_print (simulationLog, "Initialize time...");
}
#endif
simulator->scheduler = SC_Scheduler (qssData, qssTime);
#ifdef DEBUG
if (settings->debug & SD_DBG_InitValues)
{
SD_print (simulationLog, "done.");
}
if (settings->debug & SD_DBG_VarChanges)
{
SD_setSimulationLogVariables(simulationLog, qssData->states, qssData->events);
}
#endif
QSS_SEQ_logMemory (simulator);
getTime (simulator->sTime);
subTime (simulator->sTime, simulator->iTime);
simulator->initTime += getTimeValue (simulator->sTime);
}
QVector<int> TrikGyroscopeAdapter::readRawData() const
{
QVector<int> result = mModel->engine()->readGyroscopeSensor();
result.append(convertToTrikRuntimeTime(getTimeValue(mModel.data())));
return result;
}