void tickDrawerOsc(DrawerOsc *o)
{
if (o->uPeriod > 0)
{
unsigned long pos = us % o->uPeriod;
if(pos > o->halfPeriod != !o->out)
{
// tick
o->out ^= 1;
// then step
digitalWrite(o->stepPin, o->out);
if (o->dir == !DRAWER_START_DIR)
{
o->counter--;
if (o->counter == 0) drumEnd(o);
}
}
unsigned long checkPos = us % LIMIT_CHECK;
if (checkPos > HALF_LIMIT_CHECK != !o->checked)
{
o->checked ^= 1;
if (o->checked) checkLimits(o);
}
}
}
void FakeCursor::update()
{
if (!wii) return;
if (state != ACTIVE) return;
if (wii->dataReady())
{
if (!click)
Filter::init();
Point p = Filter::process(wii->getPos());
if ((!click) && (checkLimits(p) == false))
return;
move(p);
if (!click)
click = new Click(clickType);
else
click->refresh(true);
}
else
{
if ((click) && (!click->refresh(false)))
{
clickType = Click::LEFT;
delete click;
click = 0;
// Filter::init();
}
}
}
void printLine(char parameters[])
{
/* Prints a given line in format: value1 value2 ... valuen */
unsigned long inLine, nColumns;
int i, empty = 1;
double values[MAXELEMENTS];
for (i = 0; i < MAXELEMENTS; i++) {
values[i] = matrix.zero;
}
sscanf(parameters, "%lu", &inLine);
checkLimits();
/* Gather the lines of the elements on that line */
for (i = 0; i < matrix.length; i++) {
if (mElements[i].line == inLine) {
values[mElements[i].column - matrix.minC] = mElements[i].value;
empty = 0;
}
}
if (empty) {
printf("empty line\n");
} else {
nColumns = matrix.maxC-matrix.minC + 1;
for (i = 0; i < nColumns; i++) {
printf(" %.3f", values[i]);
}
printf("\n");
}
}
void printColumn(char parameters[])
{
/* Prints a given column vertically in format [line;column]=value */
unsigned long inColumn, nLines;
int i, empty = 1;
double values[MAXELEMENTS];
for (i = 0; i < MAXELEMENTS; i++) {
values[i] = matrix.zero;
}
sscanf(parameters, "%lu", &inColumn);
checkLimits();
/* Gather the columns of the elements on that line */
for (i = 0; i < matrix.length; i++) {
if (mElements[i].column == inColumn) {
values[mElements[i].line - matrix.minL] = mElements[i].value;
empty = 0;
}
}
if (empty) {
printf("empty column\n");
} else {
nLines = matrix.maxL-matrix.minL + 1;
for (i = 0; i < nLines; i++) {
printf("[%lu;%lu]=%.3f\n", i+matrix.minL, inColumn, values[i]);
}
}
}
void MqsSurfaceSettings::HeightChanged(int height)
{
if(!checkLimits(surface->xPos(), surface->yPos(), surface->xScale(), height *0.01f))
surface->yScale() = height *0.01f;
else
ui.spinBox_Height->setValue(surface->yScale()*100);
}
void MqsSurfaceSettings::WidthChanged(int width)
{
if(!checkLimits(surface->xPos(), surface->yPos(), width * 0.01f, surface->yScale()))
surface->xScale() = width *0.01f;
else
ui.spinBox_Width->setValue(surface->xScale()*100);
}
bool FileWriter::writeMessage(const Message &AMessage, const QString &ASaveMode, bool ADirectionIn)
{
if (isOpened() && ASaveMode!=ARCHIVE_SAVE_FALSE)
{
Jid contactJid = AMessage.from();
FGroupchat |= AMessage.type()==Message::GroupChat;
if (!FGroupchat || contactJid.hasResource())
{
FMessagesCount++;
FXmlWriter->writeStartElement(ADirectionIn ? "from" : "to");
int secs = FHeader.start.secsTo(AMessage.dateTime());
if (secs >= 0)
FXmlWriter->writeAttribute("secs",QString::number(secs));
else
FXmlWriter->writeAttribute("utc",DateTime(AMessage.dateTime()).toX85UTC());
if (FGroupchat)
FXmlWriter->writeAttribute("name",contactJid.resource());
if (ASaveMode == ARCHIVE_SAVE_BODY)
FXmlWriter->writeTextElement("body",AMessage.body());
else
writeElementChilds(AMessage.stanza().element());
FXmlWriter->writeEndElement();
FXmlFile->flush();
checkLimits();
return true;
}
}
return false;
}
// retrieves samples from each active channel; returns true if values in limits
boolean appBase::getSamples()
{
int32_t v,vt;
float tempC;
boolean isOK = true;
uint16_t dly = amb.getConvTime(); // use delay based on slowest conversion
uint16_t dADC = adc.getConvTime();
dly = dly > dADC ? dly : dADC;
for( uint8_t j = 0; j < _NCHAN; j++ ) { // one-shot conversions on both chips
uint8_t k = chan.getChan( j );
if( k != 0 ) {
--k; // now k = physical ADC channel number
adc.nextConversion( k ); // start ADC conversion on channel k
amb.nextConversion(); // start ambient sensor conversion
activeDelay( dly ); // give the chips time to perform the conversions
ftimes[k] = millis(); // record timestamp for RoR calculations
amb.readSensor(); // retrieve value from ambient temp register
v = adc.readuV(); // retrieve microvolt sample from MCP3424
isOK = checkLimits( v, j ); // v and j are available to calling program
if( isOK ) {
tempC = TC->Temp_C( 0.001 * v, amb.getAmbC() ); // convert to Celsius
if( celsius )
vt = round( tempC / _D_MULT ); // store results as integers
else
vt = round( C_TO_F( tempC ) / _D_MULT ); // store results as integers
temps[k] = fT[k].doFilter( vt ); // apply digital filtering for display/logging
ftemps[k] =fRise[k].doFilter( vt ); // filtering for RoR
}
else break; // abort remaining samples if one is bad
}
}
return isOK;
}
void FormantFilter_drawFilterFunctions (FormantFilter me, Graphics g, double bandwidth,
int toFreqScale, int fromFilter, int toFilter, double zmin, double zmax,
int dbScale, double ymin, double ymax, int garnish) {
if (! checkLimits (me, FilterBank_HERTZ, toFreqScale, & fromFilter, & toFilter,
& zmin, & zmax, dbScale, & ymin, & ymax)) {
return;
}
if (bandwidth <= 0) {
Melder_warning (U"Bandwidth must be greater than zero.");
}
long n = 1000;
autoNUMvector<double>a (1, n);
Graphics_setInner (g);
Graphics_setWindow (g, zmin, zmax, ymin, ymax);
for (long j = fromFilter; j <= toFilter; j++) {
double df = (zmax - zmin) / (n - 1);
double fc = my y1 + (j - 1) * my dy;
long ibegin, iend;
for (long i = 1; i <= n; i++) {
double f = zmin + (i - 1) * df;
double z = scaleFrequency (f, toFreqScale, FilterBank_HERTZ);
if (z == NUMundefined) {
a[i] = NUMundefined;
} else {
a[i] = NUMformantfilter_amplitude (fc, bandwidth, z);
if (dbScale) {
a[i] = to_dB (a[i], 10, ymin);
}
}
}
setDrawingLimits (a.peek(), n, ymin, ymax, &ibegin, &iend);
if (ibegin <= iend) {
double fmin = zmin + (ibegin - 1) * df;
double fmax = zmax - (n - iend) * df;
Graphics_function (g, a.peek(), ibegin, iend, fmin, fmax);
}
}
Graphics_unsetInner (g);
if (garnish) {
double distance = dbScale ? 10 : 1;
char32 const *ytext = dbScale ? U"Amplitude (dB)" : U"Amplitude";
Graphics_drawInnerBox (g);
Graphics_marksBottom (g, 2, 1, 1, 0);
Graphics_marksLeftEvery (g, 1, distance, 1, 1, 0);
Graphics_textLeft (g, 1, ytext);
Graphics_textBottom (g, 1, GetFreqScaleText (toFreqScale));
}
}
void MelFilter_drawFilterFunctions (MelFilter me, Graphics g,
int toFreqScale, int fromFilter, int toFilter, double zmin, double zmax,
int dbScale, double ymin, double ymax, int garnish) {
if (! checkLimits (me, FilterBank_MEL, toFreqScale, & fromFilter, & toFilter,
& zmin, & zmax, dbScale, & ymin, & ymax)) {
return;
}
long n = 1000;
autoNUMvector<double> a (1, n);
Graphics_setInner (g);
Graphics_setWindow (g, zmin, zmax, ymin, ymax);
for (long j = fromFilter; j <= toFilter; j++) {
double df = (zmax - zmin) / (n - 1);
double fc_mel = my y1 + (j - 1) * my dy;
double fc_hz = MELTOHZ (fc_mel);
double fl_hz = MELTOHZ (fc_mel - my dy);
double fh_hz = MELTOHZ (fc_mel + my dy);
long ibegin, iend;
for (long i = 1; i <= n; i++) {
// Filterfunction: triangular on a linear frequency scale AND a linear amplitude scale.
double f = zmin + (i - 1) * df;
double z = scaleFrequency (f, toFreqScale, FilterBank_HERTZ);
if (z == NUMundefined) {
a[i] = NUMundefined;
} else {
a[i] = NUMtriangularfilter_amplitude (fl_hz, fc_hz, fh_hz, z);
if (dbScale) {
a[i] = to_dB (a[i], 10, ymin);
}
}
}
setDrawingLimits (a.peek(), n, ymin, ymax, &ibegin, &iend);
if (ibegin <= iend) {
double fmin = zmin + (ibegin - 1) * df;
double fmax = zmax - (n - iend) * df;
Graphics_function (g, a.peek(), ibegin, iend, fmin, fmax);
}
}
Graphics_unsetInner (g);
if (garnish) {
double distance = dbScale ? 10 : 1;
char32 const *ytext = dbScale ? U"Amplitude (dB)" : U"Amplitude";
Graphics_drawInnerBox (g);
Graphics_marksBottom (g, 2, 1, 1, 0);
Graphics_marksLeftEvery (g, 1, distance, 1, 1, 0);
Graphics_textLeft (g, 1, ytext);
Graphics_textBottom (g, 1, GetFreqScaleText (toFreqScale));
}
}
void Arm_Cartesian_Control::process(
double dt,
KDL::JntArray& joint_positions,
KDL::Twist& targetVelocity, KDL::JntArrayVel& out_jnt_velocities) {
out_jnt_velocities.q.data = joint_positions.data;
double max_lin_frame_velocitiy = 0.1; // m/s
double max_joint_vel = 0.1; // radian/s
double eps_velocity = 0.0001;
// calc Jacobian
//KDL::Jacobian jacobian(arm.getNrOfJoints());
//jnt2jac->JntToJac(joint_positions, jacobian);
// calc absolute relative velocity; used for joint velocitiy reduction
double linear_frame_vel = targetVelocity.vel.Norm();
double angular_frame_vel = targetVelocity.rot.Norm();
if (linear_frame_vel < eps_velocity && angular_frame_vel < eps_velocity) {
out_jnt_velocities.qdot.data.setZero();
return;
}
if (linear_frame_vel > max_lin_frame_velocitiy) {
//reduce target velocity to maximum limit
targetVelocity.vel.data[0] *= (max_lin_frame_velocitiy / linear_frame_vel) ;
targetVelocity.vel.data[1] *= (max_lin_frame_velocitiy / linear_frame_vel) ;
targetVelocity.vel.data[2] *= (max_lin_frame_velocitiy / linear_frame_vel) ;
}
// calc joint velocitites
KDL::JntArray jntVel(arm_chain->getNrOfJoints());
ik_solver->CartToJnt(joint_positions, targetVelocity, jntVel);
// limit joint velocities
if (jntVel.data.norm() > 0.01) {
double max_velocity = jntVel.data.cwiseAbs().maxCoeff();
if (max_velocity > max_joint_vel) {
jntVel.data /= max_velocity;
jntVel.data *= max_joint_vel;
}
} else {
jntVel.data.Zero(arm_chain->getNrOfJoints());
}
//check for joint limits
checkLimits(dt, joint_positions, jntVel);
out_jnt_velocities.qdot.data = jntVel.data;
}
void BarkFilter_drawSekeyHansonFilterFunctions (BarkFilter me, Graphics g,
int toFreqScale, int fromFilter, int toFilter, double zmin, double zmax,
int dbScale, double ymin, double ymax, int garnish) {
if (! checkLimits (me, FilterBank_BARK, toFreqScale, & fromFilter, & toFilter,
& zmin, & zmax, dbScale, & ymin, & ymax)) {
return;
}
long n = 1000;
autoNUMvector<double> a (1, n);
Graphics_setInner (g);
Graphics_setWindow (g, zmin, zmax, ymin, ymax);
for (long j = fromFilter; j <= toFilter; j++) {
double df = (zmax - zmin) / (n - 1);
double zMid = Matrix_rowToY (me, j);
long ibegin, iend;
for (long i = 1; i <= n; i++) {
double f = zmin + (i - 1) * df;
double z = scaleFrequency (f, toFreqScale, FilterBank_BARK);
if (z == NUMundefined) {
a[i] = NUMundefined;
} else {
z -= zMid + 0.215;
a[i] = 7 - 7.5 * z - 17.5 * sqrt (0.196 + z * z);
if (! dbScale) {
a[i] = pow (10, a[i]);
}
}
}
setDrawingLimits (a.peek(), n, ymin, ymax, &ibegin, &iend);
if (ibegin <= iend) {
double fmin = zmin + (ibegin - 1) * df;
double fmax = zmax - (n - iend) * df;
Graphics_function (g, a.peek(), ibegin, iend, fmin, fmax);
}
}
Graphics_unsetInner (g);
if (garnish) {
double distance = dbScale ? 10 : 1;
const char32 *ytext = dbScale ? U"Amplitude (dB)" : U"Amplitude";
Graphics_drawInnerBox (g);
Graphics_marksBottom (g, 2, 1, 1, 0);
Graphics_marksLeftEvery (g, 1, distance, 1, 1, 0);
Graphics_textLeft (g, 1, ytext);
Graphics_textBottom (g, 1, GetFreqScaleText (toFreqScale));
}
}
void compressMatrix()
{
/* Calculates the compression of the global matrix
with the given algorithm. I'm probably declaring way
too big */
int i, j, largestIndexInLine;
double values[MAXELEMENTS*2];
unsigned long indexes[MAXELEMENTS*2] = {0};
int offset, maxOffset = 0, offsets[MAXELEMENTS] = {0};
int nElsLine, nElsLines[MAXELEMENTS] = {0};
MatrixElement elements[MAXELEMENTS];
checkLimits();
if ((float) matrix.length/matrix.size > 0.5) {
printf("dense matrix\n");
return;
}
for (i = 0; i < matrix.length; i++) {
elements[i] = mElements[i];
nElsLines[mElements[i].line - matrix.minL]++;
values[i] = matrix.zero;
}
sortByLineAndDensity(elements, nElsLines);
/* The main for goes from line to line, lines are all
grouped on the array. */
for (i = 0; i < matrix.length; i += nElsLine) {
nElsLine = nElsLines[elements[i].line - matrix.minL];
offset = 0;
/* Iterates over each element of the line.
Restarts if it encounters an occupied slot. */
largestIndexInLine = i+nElsLine;
for (j = i; j < largestIndexInLine; j++) {
if (values[elements[j].column+offset - matrix.minC] != matrix.zero) {
j = i-1;
offset++;
}
}
/* Add the correct values to the arrays. */
for (j = i; j < largestIndexInLine; j++) {
values[elements[j].column+offset - matrix.minC] = elements[j].value;
indexes[elements[j].column+offset - matrix.minC] = elements[i].line;
}
offsets[elements[i].line - matrix.minL] = offset;
if (offset > maxOffset) {
maxOffset = offset;
}
}
printCompressedMatrix(values, indexes, offsets, maxOffset);
}
void FileWriter::startCollection()
{
FXmlWriter->setAutoFormatting(true);
FXmlWriter->writeStartElement("chat");
FXmlWriter->writeAttribute("with",FHeader.with.full());
FXmlWriter->writeAttribute("start",DateTime(FHeader.start).toX85UTC());
FXmlWriter->writeAttribute("version",QString::number(FHeader.version));
if (!FHeader.subject.isEmpty())
FXmlWriter->writeAttribute("subject",FHeader.subject);
if (!FHeader.threadId.isEmpty())
FXmlWriter->writeAttribute("thread",FHeader.threadId);
FXmlWriter->writeAttribute("secsFromLast","false");
checkLimits();
}
bool FileWriter::writeNote(const QString &ANote)
{
if (isOpened() && !ANote.isEmpty())
{
FNotesCount++;
FXmlWriter->writeStartElement("note");
FXmlWriter->writeAttribute("utc",DateTime(QDateTime::currentDateTime()).toX85UTC());
FXmlWriter->writeCharacters(ANote);
FXmlWriter->writeEndElement();
FXmlFile->flush();
checkLimits();
return true;
}
return false;
}
static void appendToUTarget(IntlUConverter *data,
Variant val,
UConverterToUnicodeArgs *args) {
if (val.isNull()) {
// Ignore
return;
}
if (val.isInteger()) {
int64_t lval = val.toInt64();
if (lval < 0 || lval > 0x10FFFF) {
data->failure(U_ILLEGAL_ARGUMENT_ERROR, "appendToUTarger");
return;
}
if (lval > 0xFFFF) {
if (checkLimits(data, args, 2)) {
*(args->target++) = (UChar)(((lval - 0x10000) >> 10) | 0xD800);
*(args->target++) = (UChar)(((lval - 0x10000) & 0x3FF) | 0xDC00);
}
return;
}
void printMatrixInfo()
{
/* Prints the "boundaries" of the matrix and
its density inside that range.*/
float dens;
if (matrix.length == 0) {
printf("empty matrix\n");
return;
}
checkLimits();
dens = (float) matrix.length/matrix.size;
printf("[%lu %lu] [%lu %lu] %d / %ld = %.3f%%\n", matrix.minL, matrix.minC,
matrix.maxL, matrix.maxC,
matrix.length, matrix.size,
dens * 100);
}
void tickLimitStepOsc(LimitStepOsc *o)
{
if(o->uPeriod>0)
{
unsigned long pos = us % o->uPeriod;
if(pos > o->halfPeriod != !o->out)
{
// tick
o->out ^= 1;
// then step
digitalWrite(o->stepPin, o->out);
}
unsigned long checkPos = us % LIMIT_CHECK;
if (checkPos > HALF_LIMIT_CHECK != !o->checked)
{
o->checked ^= 1;
if (o->checked) checkLimits(o);
}
}
}
bool DietWizardDialog::checkConstraints( Recipe &rec, int meal, int dish )
{
// Check if the properties are within the constraints
ConstraintList constraints;
loadConstraints( meal, dish, &constraints ); //load the constraints
bool any_enabled = false;
for ( ConstraintList::const_iterator ct_it = constraints.constBegin(); ct_it != constraints.constEnd(); ++ct_it ) {
if ( (*ct_it).enabled ) {
any_enabled = true;
break;
}
}
if ( !any_enabled )
return true;
// Calculate properties of the recipe
calculateProperties( rec, database );
bool withinLimits = checkLimits( rec.properties, constraints );
return ( withinLimits );
}
Block DistinctSortedBlockInputStream::readImpl()
{
/// Execute until end of stream or until
/// a block with some new records will be gotten.
for (;;)
{
/// Stop reading if we already reached the limit.
if (limit_hint && data.getTotalRowCount() >= limit_hint)
return Block();
Block block = children.back()->read();
if (!block)
return Block();
const ColumnRawPtrs column_ptrs(getKeyColumns(block));
if (column_ptrs.empty())
return block;
const ColumnRawPtrs clearing_hint_columns(getClearingColumns(block, column_ptrs));
if (data.type == ClearableSetVariants::Type::EMPTY)
data.init(ClearableSetVariants::chooseMethod(column_ptrs, key_sizes));
const size_t rows = block.rows();
IColumn::Filter filter(rows);
bool has_new_data = false;
switch (data.type)
{
case ClearableSetVariants::Type::EMPTY:
break;
#define M(NAME) \
case ClearableSetVariants::Type::NAME: \
has_new_data = buildFilter(*data.NAME, column_ptrs, clearing_hint_columns, filter, rows, data); \
break;
APPLY_FOR_SET_VARIANTS(M)
#undef M
}
/// Just go to the next block if there isn't any new record in the current one.
if (!has_new_data)
continue;
if (!checkLimits())
{
switch (overflow_mode)
{
case OverflowMode::THROW:
throw Exception("DISTINCT-Set size limit exceeded."
" Rows: " + toString(data.getTotalRowCount()) +
", limit: " + toString(max_rows) +
". Bytes: " + toString(data.getTotalByteCount()) +
", limit: " + toString(max_bytes) + ".",
ErrorCodes::SET_SIZE_LIMIT_EXCEEDED);
case OverflowMode::BREAK:
return Block();
default:
throw Exception("Logical error: unknown overflow mode", ErrorCodes::LOGICAL_ERROR);
}
}
prev_block.block = block;
prev_block.clearing_hint_columns = std::move(clearing_hint_columns);
size_t all_columns = block.columns();
for (size_t i = 0; i < all_columns; ++i)
block.safeGetByPosition(i).column = block.safeGetByPosition(i).column->filter(filter, -1);
return block;
}
}
Block DistinctBlockInputStream::readImpl()
{
/// Execute until end of stream or until
/// a block with some new records will be gotten.
while (1)
{
/// Stop reading if we already reach the limit.
if (limit_hint && data.getTotalRowCount() >= limit_hint)
return Block();
Block block = children[0]->read();
if (!block)
return Block();
const ConstColumnPlainPtrs column_ptrs(getKeyColumns(block));
if (column_ptrs.empty())
return block;
if (data.empty())
data.init(SetVariants::chooseMethod(column_ptrs, key_sizes));
const size_t old_set_size = data.getTotalRowCount();
const size_t rows = block.rows();
IColumn::Filter filter(rows);
switch (data.type)
{
case SetVariants::Type::EMPTY:
break;
#define M(NAME) \
case SetVariants::Type::NAME: \
buildFilter(*data.NAME, column_ptrs, filter, rows, data); \
break;
APPLY_FOR_SET_VARIANTS(M)
#undef M
}
/// Just go to the next block if there isn't any new record in the current one.
if (data.getTotalRowCount() == old_set_size)
continue;
if (!checkLimits())
{
if (overflow_mode == OverflowMode::THROW)
throw Exception("DISTINCT-Set size limit exceeded."
" Rows: " + toString(data.getTotalRowCount()) +
", limit: " + toString(max_rows) +
". Bytes: " + toString(data.getTotalByteCount()) +
", limit: " + toString(max_bytes) + ".",
ErrorCodes::SET_SIZE_LIMIT_EXCEEDED);
if (overflow_mode == OverflowMode::BREAK)
return Block();
throw Exception("Logical error: unknown overflow mode", ErrorCodes::LOGICAL_ERROR);
}
size_t all_columns = block.columns();
for (size_t i = 0; i < all_columns; ++i)
block.safeGetByPosition(i).column = block.safeGetByPosition(i).column->filter(filter, -1);
return block;
}
}
bool FGTrim::DoTrim(void) {
trim_failed=false;
int i;
for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(false);
}
fdmex->DisableOutput();
fdmex->SetTrimStatus(true);
fdmex->SuspendIntegration();
fgic->SetPRadpsIC(0.0);
fgic->SetQRadpsIC(0.0);
fgic->SetRRadpsIC(0.0);
//clear the sub iterations counts & zero out the controls
for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
//cout << current_axis << " " << TrimAxes[current_axis]->GetStateName()
//<< " " << TrimAxes[current_axis]->GetControlName()<< endl;
if(TrimAxes[current_axis]->GetStateType() == tQdot) {
if(mode == tGround) {
TrimAxes[current_axis]->initTheta();
}
}
xlo=TrimAxes[current_axis]->GetControlMin();
xhi=TrimAxes[current_axis]->GetControlMax();
TrimAxes[current_axis]->SetControl((xlo+xhi)/2);
TrimAxes[current_axis]->Run();
//TrimAxes[current_axis]->AxisReport();
sub_iterations[current_axis]=0;
successful[current_axis]=0;
solution[current_axis]=false;
}
if(mode == tPullup ) {
cout << "Setting pitch rate and nlf... " << endl;
setupPullup();
cout << "pitch rate done ... " << endl;
TrimAxes[0]->SetStateTarget(targetNlf);
cout << "nlf done" << endl;
} else if (mode == tTurn) {
setupTurn();
//TrimAxes[0]->SetStateTarget(targetNlf);
}
do {
axis_count=0;
for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
setDebug();
updateRates();
Nsub=0;
if(!solution[current_axis]) {
if(checkLimits()) {
solution[current_axis]=true;
solve();
}
} else if(findInterval()) {
solve();
} else {
solution[current_axis]=false;
}
sub_iterations[current_axis]+=Nsub;
}
for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
//these checks need to be done after all the axes have run
if(Debug > 0) TrimAxes[current_axis]->AxisReport();
if(TrimAxes[current_axis]->InTolerance()) {
axis_count++;
successful[current_axis]++;
}
}
if((axis_count == TrimAxes.size()-1) && (TrimAxes.size() > 1)) {
//cout << TrimAxes.size()-1 << " out of " << TrimAxes.size() << "!" << endl;
//At this point we can check the input limits of the failed axis
//and declare the trim failed if there is no sign change. If there
//is, keep going until success or max iteration count
//Oh, well: two out of three ain't bad
for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
//these checks need to be done after all the axes have run
if(!TrimAxes[current_axis]->InTolerance()) {
if(!checkLimits()) {
// special case this for now -- if other cases arise proper
// support can be added to FGTrimAxis
if( (gamma_fallback) &&
(TrimAxes[current_axis]->GetStateType() == tUdot) &&
(TrimAxes[current_axis]->GetControlType() == tThrottle)) {
cout << " Can't trim udot with throttle, trying flight"
<< " path angle. (" << N << ")" << endl;
if(TrimAxes[current_axis]->GetState() > 0)
TrimAxes[current_axis]->SetControlToMin();
else
TrimAxes[current_axis]->SetControlToMax();
TrimAxes[current_axis]->Run();
delete TrimAxes[current_axis];
TrimAxes[current_axis]=new FGTrimAxis(fdmex,fgic,tUdot,
tGamma );
} else {
cout << " Sorry, " << TrimAxes[current_axis]->GetStateName()
<< " doesn't appear to be trimmable" << endl;
//total_its=k;
trim_failed=true; //force the trim to fail
} //gamma_fallback
}
} //solution check
} //for loop
} //all-but-one check
N++;
if(N > max_iterations)
trim_failed=true;
} while((axis_count < TrimAxes.size()) && (!trim_failed));
if((!trim_failed) && (axis_count >= TrimAxes.size())) {
total_its=N;
if (debug_lvl > 0)
cout << endl << " Trim successful" << endl;
} else {
total_its=N;
if (debug_lvl > 0)
cout << endl << " Trim failed" << endl;
}
for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
}
fdmex->SetTrimStatus(false);
fdmex->ResumeIntegration();
fdmex->EnableOutput();
return !trim_failed;
}
/* Reprocess the data stored during a previous 'Process()' call.
*
* This method will reprocess data trimming satellites whose postfit
* residual is bigger than the limits indicated by limitsCodeList and
* limitsPhaseList.
*/
void SolverPPPFB::ReProcess( void )
throw(ProcessingException)
{
// Let's use a copy of the lists
std::list<double> codeList( limitsCodeList );
std::list<double> phaseList( limitsPhaseList );
// Get maximum size
size_t maxSize( codeList.size() );
if( maxSize < phaseList.size() ) maxSize = phaseList.size();
// This will prevent further storage of input data when calling
// method 'Process()'
firstIteration = false;
try
{
std::list<gnssRinex>::iterator pos;
std::list<gnssRinex>::reverse_iterator rpos;
// Backwards iteration. We must do this at least once
for (rpos = ObsData.rbegin(); rpos != ObsData.rend(); ++rpos)
{
SolverPPP::Process( (*rpos) );
}
// If both sizes are '0', let's return
if( maxSize == 0 )
{
return;
}
// We will store the limits here. By default we use very big values
double codeLimit( 1000000.0 );
double phaseLimit( 1000000.0 );
// If 'maxSize > 0', let's do the other iterations
for (size_t i = 0; i < maxSize; i++)
{
// Update current limits, if available
if( codeList.size() > 0 )
{
// Get the first element from the list
codeLimit = codeList.front();
// Delete the first element from the list
codeList.pop_front();
}
if( phaseList.size() > 0 )
{
// Get the first element from the list
phaseLimit = phaseList.front();
// Delete the first element from the list
phaseList.pop_front();
}
// Forwards iteration
for (pos = ObsData.begin(); pos != ObsData.end(); ++pos)
{
// Let's check limits
checkLimits( (*pos), codeLimit, phaseLimit );
// Process data
SolverPPP::Process( (*pos) );
}
// Backwards iteration.
for (rpos = ObsData.rbegin(); rpos != ObsData.rend(); ++rpos)
{
// Let's check limits
checkLimits( (*rpos), codeLimit, phaseLimit );
// Process data
SolverPPP::Process( (*rpos) );
}
} // End of 'for (int i=0; i<(cycles-1), i++)'
return;
}
catch(Exception& u)
{
// Throw an exception if something unexpected happens
ProcessingException e( getClassName() + ":"
+ u.what() );
GPSTK_THROW(e);
}
} // End of method 'SolverPPPFB::ReProcess()'
static std::tuple<ASTPtr, BlockIO> executeQueryImpl(
IParser::Pos begin,
IParser::Pos end,
Context & context,
bool internal,
QueryProcessingStage::Enum stage)
{
ProfileEvents::increment(ProfileEvents::Query);
time_t current_time = time(0);
const Settings & settings = context.getSettingsRef();
ParserQuery parser;
ASTPtr ast;
size_t query_size;
size_t max_query_size = settings.max_query_size;
try
{
ast = parseQuery(parser, begin, end, "");
/// Copy query into string. It will be written to log and presented in processlist. If an INSERT query, string will not include data to insertion.
query_size = ast->range.second - ast->range.first;
if (max_query_size && query_size > max_query_size)
throw Exception("Query is too large (" + toString(query_size) + ")."
" max_query_size = " + toString(max_query_size), ErrorCodes::QUERY_IS_TOO_LARGE);
}
catch (...)
{
/// Anyway log query.
if (!internal)
{
String query = String(begin, begin + std::min(end - begin, static_cast<ptrdiff_t>(max_query_size)));
logQuery(query.substr(0, settings.log_queries_cut_to_length), context);
onExceptionBeforeStart(query, context, current_time);
}
throw;
}
String query(begin, query_size);
BlockIO res;
try
{
if (!internal)
logQuery(query.substr(0, settings.log_queries_cut_to_length), context);
/// Check the limits.
checkLimits(*ast, settings.limits);
QuotaForIntervals & quota = context.getQuota();
quota.addQuery(current_time);
quota.checkExceeded(current_time);
/// Put query to process list. But don't put SHOW PROCESSLIST query itself.
ProcessList::EntryPtr process_list_entry;
if (!internal && nullptr == typeid_cast<const ASTShowProcesslistQuery *>(&*ast))
{
process_list_entry = context.getProcessList().insert(
query,
context.getUser(),
context.getCurrentQueryId(),
context.getIPAddress(),
settings);
context.setProcessListElement(&process_list_entry->get());
}
auto interpreter = InterpreterFactory::get(ast, context, stage);
res = interpreter->execute();
/// Hold element of process list till end of query execution.
res.process_list_entry = process_list_entry;
if (res.in)
{
if (IProfilingBlockInputStream * stream = dynamic_cast<IProfilingBlockInputStream *>(res.in.get()))
{
stream->setProgressCallback(context.getProgressCallback());
stream->setProcessListElement(context.getProcessListElement());
}
}
/// Everything related to query log.
{
QueryLogElement elem;
elem.type = QueryLogElement::QUERY_START;
elem.event_time = current_time;
elem.query_start_time = current_time;
elem.query = query.substr(0, settings.log_queries_cut_to_length);
setClientInfo(elem, context);
bool log_queries = settings.log_queries && !internal;
/// Log into system table start of query execution, if need.
if (log_queries)
context.getQueryLog().add(elem);
/// Also make possible for caller to log successful query finish and exception during execution.
res.finish_callback = [elem, &context, log_queries] (IBlockInputStream * stream) mutable
{
ProcessListElement * process_list_elem = context.getProcessListElement();
if (!process_list_elem)
return;
double elapsed_seconds = process_list_elem->watch.elapsedSeconds();
elem.type = QueryLogElement::QUERY_FINISH;
elem.event_time = time(0);
elem.query_duration_ms = elapsed_seconds * 1000;
elem.read_rows = process_list_elem->progress.rows;
elem.read_bytes = process_list_elem->progress.bytes;
auto memory_usage = process_list_elem->memory_tracker.getPeak();
elem.memory_usage = memory_usage > 0 ? memory_usage : 0;
if (stream)
{
if (IProfilingBlockInputStream * profiling_stream = dynamic_cast<IProfilingBlockInputStream *>(stream))
{
const BlockStreamProfileInfo & info = profiling_stream->getProfileInfo();
elem.result_rows = info.rows;
elem.result_bytes = info.bytes;
}
}
if (elem.read_rows != 0)
{
LOG_INFO(&Logger::get("executeQuery"), std::fixed << std::setprecision(3)
<< "Read " << elem.read_rows << " rows, "
<< formatReadableSizeWithBinarySuffix(elem.read_bytes) << " in " << elapsed_seconds << " sec., "
<< static_cast<size_t>(elem.read_rows / elapsed_seconds) << " rows/sec., "
<< formatReadableSizeWithBinarySuffix(elem.read_bytes / elapsed_seconds) << "/sec.");
}
if (log_queries)
context.getQueryLog().add(elem);
};
res.exception_callback = [elem, &context, log_queries, current_time] () mutable
{
context.getQuota().addError(current_time);
elem.type = QueryLogElement::EXCEPTION_WHILE_PROCESSING;
elem.event_time = time(0);
elem.query_duration_ms = 1000 * (elem.event_time - elem.query_start_time);
elem.exception = getCurrentExceptionMessage(false);
ProcessListElement * process_list_elem = context.getProcessListElement();
if (process_list_elem)
{
double elapsed_seconds = process_list_elem->watch.elapsedSeconds();
elem.query_duration_ms = elapsed_seconds * 1000;
elem.read_rows = process_list_elem->progress.rows;
elem.read_bytes = process_list_elem->progress.bytes;
auto memory_usage = process_list_elem->memory_tracker.getPeak();
elem.memory_usage = memory_usage > 0 ? memory_usage : 0;
}
setExceptionStackTrace(elem);
logException(context, elem);
if (log_queries)
context.getQueryLog().add(elem);
};
if (!internal && res.in)
{
std::stringstream log_str;
log_str << "Query pipeline:\n";
res.in->dumpTree(log_str);
LOG_DEBUG(&Logger::get("executeQuery"), log_str.str());
}
}
}
catch (...)
{
if (!internal)
onExceptionBeforeStart(query, context, current_time);
throw;
}
return std::make_tuple(ast, res);
}
void BTSTO(int reg){
checkLimits(reg, 0, 15)
baseTable[reg]=1;
curBaseRegister = reg;
}
void BTDROP(int reg){
checkLimits(reg, 0, 15)
baseTable[reg]=0;
}
bool FGTrim::DoTrim(void) {
bool trim_failed=false;
unsigned int N = 0;
unsigned int axis_count = 0;
FGFCS *FCS = fdmex->GetFCS();
vector<double> throttle0 = FCS->GetThrottleCmd();
double elevator0 = FCS->GetDeCmd();
double aileron0 = FCS->GetDaCmd();
double rudder0 = FCS->GetDrCmd();
double PitchTrim0 = FCS->GetPitchTrimCmd();
fgic = *fdmex->GetIC();
for(int i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(false);
}
fdmex->SetTrimStatus(true);
fdmex->SuspendIntegration();
fgic.SetPRadpsIC(0.0);
fgic.SetQRadpsIC(0.0);
fgic.SetRRadpsIC(0.0);
if (mode == tGround) {
trimOnGround();
double theta = fgic.GetThetaRadIC();
double phi = fgic.GetPhiRadIC();
// Take opportunity of the first approx. found by trimOnGround() to
// refine the control limits.
TrimAxes[0].SetControlLimits(0., fgic.GetAltitudeAGLFtIC());
TrimAxes[1].SetControlLimits(theta - 5.0 * degtorad, theta + 5.0 * degtorad);
TrimAxes[2].SetControlLimits(phi - 30.0 * degtorad, phi + 30.0 * degtorad);
}
//clear the sub iterations counts & zero out the controls
for(unsigned int current_axis=0;current_axis<TrimAxes.size();current_axis++) {
//cout << current_axis << " " << TrimAxes[current_axis]->GetStateName()
//<< " " << TrimAxes[current_axis]->GetControlName()<< endl;
xlo=TrimAxes[current_axis].GetControlMin();
xhi=TrimAxes[current_axis].GetControlMax();
TrimAxes[current_axis].SetControl((xlo+xhi)/2);
TrimAxes[current_axis].Run();
//TrimAxes[current_axis].AxisReport();
sub_iterations[current_axis]=0;
successful[current_axis]=0;
solution[current_axis]=false;
}
if(mode == tPullup ) {
cout << "Setting pitch rate and nlf... " << endl;
setupPullup();
cout << "pitch rate done ... " << endl;
TrimAxes[0].SetStateTarget(targetNlf);
cout << "nlf done" << endl;
} else if (mode == tTurn) {
setupTurn();
//TrimAxes[0].SetStateTarget(targetNlf);
}
do {
axis_count=0;
for(unsigned int current_axis=0;current_axis<TrimAxes.size();current_axis++) {
setDebug(TrimAxes[current_axis]);
updateRates();
Nsub=0;
if(!solution[current_axis]) {
if(checkLimits(TrimAxes[current_axis])) {
solution[current_axis]=true;
solve(TrimAxes[current_axis]);
}
} else if(findInterval(TrimAxes[current_axis])) {
solve(TrimAxes[current_axis]);
} else {
solution[current_axis]=false;
}
sub_iterations[current_axis]+=Nsub;
}
for(unsigned int current_axis=0;current_axis<TrimAxes.size();current_axis++) {
//these checks need to be done after all the axes have run
if(Debug > 0) TrimAxes[current_axis].AxisReport();
if(TrimAxes[current_axis].InTolerance()) {
axis_count++;
successful[current_axis]++;
}
}
if((axis_count == TrimAxes.size()-1) && (TrimAxes.size() > 1)) {
//cout << TrimAxes.size()-1 << " out of " << TrimAxes.size() << "!" << endl;
//At this point we can check the input limits of the failed axis
//and declare the trim failed if there is no sign change. If there
//is, keep going until success or max iteration count
//Oh, well: two out of three ain't bad
for(unsigned int current_axis=0;current_axis<TrimAxes.size();current_axis++) {
//these checks need to be done after all the axes have run
if(!TrimAxes[current_axis].InTolerance()) {
if(!checkLimits(TrimAxes[current_axis])) {
// special case this for now -- if other cases arise proper
// support can be added to FGTrimAxis
if( (gamma_fallback) &&
(TrimAxes[current_axis].GetStateType() == tUdot) &&
(TrimAxes[current_axis].GetControlType() == tThrottle)) {
cout << " Can't trim udot with throttle, trying flight"
<< " path angle. (" << N << ")" << endl;
if(TrimAxes[current_axis].GetState() > 0)
TrimAxes[current_axis].SetControlToMin();
else
TrimAxes[current_axis].SetControlToMax();
TrimAxes[current_axis].Run();
TrimAxes[current_axis]=FGTrimAxis(fdmex,&fgic,tUdot,tGamma);
} else {
cout << " Sorry, " << TrimAxes[current_axis].GetStateName()
<< " doesn't appear to be trimmable" << endl;
//total_its=k;
trim_failed=true; //force the trim to fail
} //gamma_fallback
}
} //solution check
} //for loop
} //all-but-one check
N++;
if(N > max_iterations)
trim_failed=true;
} while((axis_count < TrimAxes.size()) && (!trim_failed));
if((!trim_failed) && (axis_count >= TrimAxes.size())) {
total_its=N;
if (debug_lvl > 0)
cout << endl << " Trim successful" << endl;
} else { // The trim has failed
total_its=N;
// Restore the aircraft parameters to their initial values
fgic = *fdmex->GetIC();
FCS->SetDeCmd(elevator0);
FCS->SetDaCmd(aileron0);
FCS->SetDrCmd(rudder0);
FCS->SetPitchTrimCmd(PitchTrim0);
for (unsigned int i=0; i < throttle0.size(); i++)
FCS->SetThrottleCmd(i, throttle0[i]);
fdmex->Initialize(&fgic);
fdmex->Run();
// If WOW is true we must make sure there are no gears into the ground.
if (fdmex->GetGroundReactions()->GetWOW())
trimOnGround();
if (debug_lvl > 0)
cout << endl << " Trim failed" << endl;
}
fdmex->ResumeIntegration();
fdmex->SetTrimStatus(false);
for(int i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
}
return !trim_failed;
}
KicadModule2Svg::PadLayer KicadModule2Svg::convertPad(QTextStream & stream, QString & pad, QList<int> & numbers) {
PadLayer padLayer = UnableToTranslate;
QStringList padStrings;
while (true) {
QString line = stream.readLine();
if (line.isNull()) {
throw QObject::tr("unexpected end of file");
}
if (line.contains("$EndPAD")) {
break;
}
padStrings.append(line);
}
QString shape;
QString drill;
QString attributes;
QString position;
foreach (QString string, padStrings) {
if (string.startsWith("Sh")) {
shape = string;
}
else if (string.startsWith("Po")) {
position = string;
}
else if (string.startsWith("At")) {
attributes = string;
}
else if (string.startsWith("Dr")) {
drill = string;
}
}
if (drill.isEmpty()) {
throw QObject::tr("pad missing drill");
}
if (attributes.isEmpty()) {
throw QObject::tr("pad missing attributes");
}
if (position.isEmpty()) {
throw QObject::tr("pad missing position");
}
if (shape.isEmpty()) {
throw QObject::tr("pad missing shape");
}
QStringList positionStrings = position.split(" ");
if (positionStrings.count() < 3) {
throw QObject::tr("position missing params");
}
int posX = positionStrings.at(1).toInt();
int posY = positionStrings.at(2).toInt();
QStringList drillStrings = drill.split(" ");
if (drillStrings.count() < 4) {
throw QObject::tr("drill missing params");
}
int drillX = drillStrings.at(1).toInt();
int drillXOffset = drillStrings.at(2).toInt();
int drillYOffset = drillStrings.at(3).toInt();
int drillY = drillX;
if (drillXOffset != 0 || drillYOffset != 0) {
throw QObject::tr("drill offset not implemented");
}
if (drillStrings.count() > 4) {
if (drillStrings.at(4) == "O") {
if (drillStrings.count() < 7) {
throw QObject::tr("drill missing ellipse params");
}
drillY = drillStrings.at(6).toInt();
}
}
QStringList attributeStrings = attributes.split(" ");
if (attributeStrings.count() < 4) {
throw QObject::tr("attributes missing params");
}
bool ok;
int layerMask = attributeStrings.at(3).toInt(&ok, 16);
if (!ok) {
throw QObject::tr("bad layer mask parameter");
}
QString padType = attributeStrings.at(1);
if (padType == "MECA") {
// seems to be the same thing
padType = "STD";
}
if (padType == "STD") {
padLayer = ToCopper0;
}
else if (padType == "SMD") {
padLayer = ToCopper1;
}
else if (padType == "CONN") {
if (layerMask & 1) {
padLayer = ToCopper0;
}
else {
padLayer = ToCopper1;
}
}
else if (padType == "HOLE") {
padLayer = ToCopper0;
}
else {
throw QObject::tr("Sorry, can't handle pad type %1").arg(padType);
}
QStringList shapeStrings = shape.split(" ");
if (shapeStrings.count() < 8) {
throw QObject::tr("pad shape missing params");
}
QString padName = unquote(shapeStrings.at(1));
int padNumber = padName.toInt(&ok) - 1;
if (!ok) {
padNumber = padName.isEmpty() ? -1 : numbers.takeFirst();
//DebugDialog::debug(QString("name:%1 padnumber %2").arg(padName).arg(padNumber));
}
else {
numbers.removeOne(padNumber);
}
QString shapeIdentifier = shapeStrings.at(2);
int xSize = shapeStrings.at(3).toInt();
int ySize = shapeStrings.at(4).toInt();
if (ySize <= 0) {
DebugDialog::debug(QString("ySize is zero %1").arg(padName));
ySize = xSize;
}
if (xSize <= 0) {
throw QObject::tr("pad shape size is invalid");
}
int xDelta = shapeStrings.at(5).toInt();
int yDelta = shapeStrings.at(6).toInt();
int orientation = shapeStrings.at(7).toInt();
if (shapeIdentifier == "T") {
throw QObject::tr("trapezoidal pads not implemented");
// eventually polygon?
}
if (xDelta != 0 || yDelta != 0) {
// note: so far, all cases of non-zero delta go with shape "T"
throw QObject::tr("shape delta not implemented");
}
if (padType == "HOLE") {
if (shapeIdentifier != "C") {
throw QObject::tr("non-circular holes not implemented");
}
if (drillX == xSize) {
throw QObject::tr("non-copper holes not implemented");
}
}
if (shapeIdentifier == "C") {
checkLimits(posX, xSize, posY, ySize);
pad += drawCPad(posX, posY, xSize, ySize, drillX, drillY, padName, padNumber, padType, padLayer);
}
else if (shapeIdentifier == "R") {
checkLimits(posX, xSize, posY, ySize);
pad += drawRPad(posX, posY, xSize, ySize, drillX, drillY, padName, padNumber, padType, padLayer);
}
else if (shapeIdentifier == "O") {
checkLimits(posX, xSize, posY, ySize);
QString id = getID(padNumber, padLayer);
pad += QString("<g %1 connectorname='%2'>")
.arg(id).arg(padName)
+ drawOblong(posX, posY, xSize, ySize, drillX, drillY, padType, padLayer)
+ "</g>";
}
else {
throw QObject::tr("unable to handle pad shape %1").arg(shapeIdentifier);
}
if (orientation != 0) {
if (orientation < 0) {
orientation = (orientation % 3600) + 3600;
}
orientation = 3600 - (orientation % 3600);
QTransform t = QTransform().translate(-posX, -posY) *
QTransform().rotate(orientation / 10.0) *
QTransform().translate(posX, posY);
pad = TextUtils::svgTransform(pad, t, true, QString("_x='%1' _y='%2' _r='%3'").arg(posX).arg(posY).arg(orientation / 10.0));
}
return padLayer;
}
