float PIDController::update(float measuredValue, float dt, bool resetAccumulator) {
const float error = getMeasuredValueSetpoint() - measuredValue; // Sign is the direction we want measuredValue to go. Positive means go higher.
const float p = getKP() * error; // term is Proportional to error
const float accumulatedError = glm::clamp(error * dt + (resetAccumulator ? 0 : _lastAccumulation), // integrate error
getAccumulatedValueLowLimit(), // but clamp by anti-windup limits
getAccumulatedValueHighLimit());
const float i = getKI() * accumulatedError; // term is Integral of error
const float changeInError = (error - _lastError) / dt; // positive value denotes increasing deficit
const float d = getKD() * changeInError; // term is Derivative of Error
const float computedValue = glm::clamp(p + i + d,
getControlledValueLowLimit(),
getControlledValueHighLimit());
if (getIsLogging()) { // if logging/reporting
updateHistory(measuredValue, dt, error, accumulatedError, changeInError, p, i, d, computedValue);
}
Q_ASSERT(!glm::isnan(computedValue));
// update state for next time
_lastError = error;
_lastAccumulation = accumulatedError;
return computedValue;
}
/**
* This slot is called, if a new task has been selected.
*/
void IgcLogger::slotNewTaskSelected()
{
if( ! _logfile.isOpen() )
{
// Logger does not run, ignore this call.
return;
}
QMessageBox mb( QMessageBox::Warning,
tr( "Restart Logging?" ),
tr("<html>A new flight task was selected.<br>Restart logging?</html>"),
QMessageBox::Yes | QMessageBox::No,
MainWindow::mainWindow() );
mb.setDefaultButton( QMessageBox::No );
#ifdef ANDROID
mb.show();
QPoint pos = MainWindow::mainWindow()->mapToGlobal( QPoint( MainWindow::mainWindow()->width()/2 - mb.width()/2,
MainWindow::mainWindow()->height()/2 - mb.height()/2 ) );
mb.move( pos );
#endif
if( mb.exec() == QMessageBox::Yes )
{
// qDebug("Restarting logging...");
Stop();
_logMode = on;
emit logging(getIsLogging());
}
}
/**
* Switches on the standby mode. If we are currently logging, the logfile will
* be closed.
*/
void IgcLogger::Standby()
{
if( _logMode == on )
{
CloseFile();
}
_logMode = standby;
_backtrack.clear();
// Reset time classes to initial state
delete lastLoggedBRecord;
delete lastLoggedFRecord;
delete lastLoggedKRecord;
lastLoggedBRecord = new QTime();
lastLoggedFRecord = new QTime();
lastLoggedKRecord = new QTime();
emit logging( getIsLogging() );
}
/** This slot is called to start or end a log. */
void IgcLogger::slotToggleLogging()
{
// qDebug("toggle logging!");
if ( _logMode == on )
{
QMessageBox mb( QMessageBox::Question,
tr( "Stop Logging?" ),
tr("<html>Are you sure you want<br>stop logging?</html>"),
QMessageBox::Yes | QMessageBox::No,
MainWindow::mainWindow() );
mb.setDefaultButton( QMessageBox::No );
#ifdef ANDROID
mb.show();
QPoint pos = MainWindow::mainWindow()->mapToGlobal( QPoint( MainWindow::mainWindow()->width()/2 - mb.width()/2,
MainWindow::mainWindow()->height()/2 - mb.height()/2 ) );
mb.move( pos );
#endif
if( mb.exec() == QMessageBox::Yes )
{
// qDebug("Stopping logging...");
Stop();
}
}
else
{
// Logger is in mode standby or off
int answer = QMessageBox::Yes;
if( ! calculator->glider() )
{
QMessageBox mb( QMessageBox::Warning,
tr( "Start Logging?" ),
tr("<html>You should select a glider<br>before start logging.<br>Continue start logging?</html>"),
QMessageBox::Yes | QMessageBox::No,
MainWindow::mainWindow() );
mb.setDefaultButton( QMessageBox::No );
#ifdef ANDROID
mb.show();
QPoint pos = MainWindow::mainWindow()->mapToGlobal( QPoint( MainWindow::mainWindow()->width()/2 - mb.width()/2,
MainWindow::mainWindow()->height()/2 - mb.height()/2 ) );
mb.move( pos );
#endif
answer = mb.exec();
}
if( answer == QMessageBox::Yes )
{
_logMode = on;
}
}
// emit the logging state in all cases to allow update of actions in MainWindow
emit logging(getIsLogging());
}
