mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +01:00
OP-1149/OP-1150: uncrustify/Cleanup
This commit is contained in:
Alessio Morale
parent
e599b01806
commit
27c19d7cb1
@ -350,19 +350,19 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
|
||||
}
|
||||
|
||||
// Scale the accels
|
||||
float accels[3] = { (float)accel_accum[0] / accel_samples,
|
||||
(float)accel_accum[1] / accel_samples,
|
||||
(float)accel_accum[2] / accel_samples };
|
||||
float accels[3] = { (float)accel_accum[0] / accel_samples,
|
||||
(float)accel_accum[1] / accel_samples,
|
||||
(float)accel_accum[2] / accel_samples };
|
||||
|
||||
|
||||
float accels_out[3] = { accels[0] * accel_scaling * accel_scale[0] - accel_bias[0],
|
||||
accels[1] * accel_scaling * accel_scale[1] - accel_bias[1],
|
||||
accels[2] * accel_scaling * accel_scale[2] - accel_bias[2] };
|
||||
|
||||
if(accel_temp_calibrated){
|
||||
if (accel_temp_calibrated) {
|
||||
float ctemp = accelSensorData.temperature > calibrated_temp_max ? calibrated_temp_max :
|
||||
(accelSensorData.temperature < calibrated_temp_min ? calibrated_temp_min
|
||||
: accelSensorData.temperature );
|
||||
: accelSensorData.temperature);
|
||||
accels_out[0] -= accel_temp_coeff[0] * ctemp;
|
||||
accels_out[1] -= accel_temp_coeff[1] * ctemp;
|
||||
accels_out[2] -= accel_temp_coeff[2] * ctemp;
|
||||
@ -381,18 +381,18 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
|
||||
AccelSensorSet(&accelSensorData);
|
||||
|
||||
// Scale the gyros
|
||||
float gyros[3] = { (float)gyro_accum[0] / gyro_samples,
|
||||
(float)gyro_accum[1] / gyro_samples,
|
||||
(float)gyro_accum[2] / gyro_samples };
|
||||
float gyros[3] = { (float)gyro_accum[0] / gyro_samples,
|
||||
(float)gyro_accum[1] / gyro_samples,
|
||||
(float)gyro_accum[2] / gyro_samples };
|
||||
|
||||
float gyros_out[3] = { gyros[0] * gyro_scaling * gyro_scale[0] - gyro_staticbias[0],
|
||||
gyros[1] * gyro_scaling * gyro_scale[1] - gyro_staticbias[1],
|
||||
gyros[2] * gyro_scaling * gyro_scale[2] - gyro_staticbias[2]};
|
||||
gyros[2] * gyro_scaling * gyro_scale[2] - gyro_staticbias[2] };
|
||||
|
||||
if(gyro_temp_calibrated){
|
||||
if (gyro_temp_calibrated) {
|
||||
float ctemp = gyroSensorData.temperature > calibrated_temp_max ? calibrated_temp_max :
|
||||
(gyroSensorData.temperature < calibrated_temp_min ? calibrated_temp_min
|
||||
: gyroSensorData.temperature );
|
||||
: gyroSensorData.temperature);
|
||||
gyros_out[0] -= gyro_temp_coeff[0] * ctemp;
|
||||
gyros_out[1] -= gyro_temp_coeff[1] * ctemp;
|
||||
gyros_out[2] -= (gyro_temp_coeff[2] + gyro_temp_coeff[3] * ctemp) * ctemp;
|
||||
@ -454,41 +454,40 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
|
||||
{
|
||||
RevoCalibrationGet(&cal);
|
||||
AccelGyroSettingsGet(&agcal);
|
||||
mag_bias[0] = cal.mag_bias.X;
|
||||
mag_bias[1] = cal.mag_bias.Y;
|
||||
mag_bias[2] = cal.mag_bias.Z;
|
||||
mag_scale[0] = cal.mag_scale.X;
|
||||
mag_scale[1] = cal.mag_scale.Y;
|
||||
mag_scale[2] = cal.mag_scale.Z;
|
||||
accel_bias[0] = agcal.accel_bias.X;
|
||||
accel_bias[1] = agcal.accel_bias.Y;
|
||||
accel_bias[2] = agcal.accel_bias.Z;
|
||||
accel_scale[0] = agcal.accel_scale.X;
|
||||
accel_scale[1] = agcal.accel_scale.Y;
|
||||
accel_scale[2] = agcal.accel_scale.Z;
|
||||
gyro_staticbias[0] = agcal.gyro_bias.X;
|
||||
gyro_staticbias[1] = agcal.gyro_bias.Y;
|
||||
gyro_staticbias[2] = agcal.gyro_bias.Z;
|
||||
gyro_scale[0] = agcal.gyro_scale.X;
|
||||
gyro_scale[1] = agcal.gyro_scale.Y;
|
||||
gyro_scale[2] = agcal.gyro_scale.Z;
|
||||
gyro_temp_coeff[0] = agcal.gyro_temp_coeff.X;
|
||||
gyro_temp_coeff[1] = agcal.gyro_temp_coeff.Y;
|
||||
gyro_temp_coeff[2] = agcal.gyro_temp_coeff.Z;
|
||||
gyro_temp_coeff[3] = agcal.gyro_temp_coeff.Z2;
|
||||
accel_temp_coeff[0] = agcal.accel_temp_coeff.X;
|
||||
accel_temp_coeff[1] = agcal.accel_temp_coeff.Y;
|
||||
accel_temp_coeff[2] = agcal.accel_temp_coeff.Z;
|
||||
calibrated_temp_max = agcal.temp_calibrated_extent.max;
|
||||
calibrated_temp_min = agcal.temp_calibrated_extent.min;
|
||||
mag_bias[0] = cal.mag_bias.X;
|
||||
mag_bias[1] = cal.mag_bias.Y;
|
||||
mag_bias[2] = cal.mag_bias.Z;
|
||||
mag_scale[0] = cal.mag_scale.X;
|
||||
mag_scale[1] = cal.mag_scale.Y;
|
||||
mag_scale[2] = cal.mag_scale.Z;
|
||||
accel_bias[0] = agcal.accel_bias.X;
|
||||
accel_bias[1] = agcal.accel_bias.Y;
|
||||
accel_bias[2] = agcal.accel_bias.Z;
|
||||
accel_scale[0] = agcal.accel_scale.X;
|
||||
accel_scale[1] = agcal.accel_scale.Y;
|
||||
accel_scale[2] = agcal.accel_scale.Z;
|
||||
gyro_staticbias[0] = agcal.gyro_bias.X;
|
||||
gyro_staticbias[1] = agcal.gyro_bias.Y;
|
||||
gyro_staticbias[2] = agcal.gyro_bias.Z;
|
||||
gyro_scale[0] = agcal.gyro_scale.X;
|
||||
gyro_scale[1] = agcal.gyro_scale.Y;
|
||||
gyro_scale[2] = agcal.gyro_scale.Z;
|
||||
gyro_temp_coeff[0] = agcal.gyro_temp_coeff.X;
|
||||
gyro_temp_coeff[1] = agcal.gyro_temp_coeff.Y;
|
||||
gyro_temp_coeff[2] = agcal.gyro_temp_coeff.Z;
|
||||
gyro_temp_coeff[3] = agcal.gyro_temp_coeff.Z2;
|
||||
accel_temp_coeff[0] = agcal.accel_temp_coeff.X;
|
||||
accel_temp_coeff[1] = agcal.accel_temp_coeff.Y;
|
||||
accel_temp_coeff[2] = agcal.accel_temp_coeff.Z;
|
||||
calibrated_temp_max = agcal.temp_calibrated_extent.max;
|
||||
calibrated_temp_min = agcal.temp_calibrated_extent.min;
|
||||
|
||||
accel_temp_calibrated = (agcal.temp_calibrated_extent.max - agcal.temp_calibrated_extent.min > .1f) &&
|
||||
agcal.accel_temp_coeff.X > 1e-9f && agcal.accel_temp_coeff.Y > 1e-9f && agcal.accel_temp_coeff.Z > 1e-9f;
|
||||
|
||||
gyro_temp_calibrated = (agcal.temp_calibrated_extent.max - agcal.temp_calibrated_extent.min > .1f) &&
|
||||
agcal.gyro_temp_coeff.X > 1e-9f && agcal.gyro_temp_coeff.Y > 1e-9f &&
|
||||
agcal.gyro_temp_coeff.Z > 1e-9f && agcal.gyro_temp_coeff.Z2 > 1e-9f;
|
||||
agcal.accel_temp_coeff.X > 1e-9f && agcal.accel_temp_coeff.Y > 1e-9f && agcal.accel_temp_coeff.Z > 1e-9f;
|
||||
|
||||
gyro_temp_calibrated = (agcal.temp_calibrated_extent.max - agcal.temp_calibrated_extent.min > .1f) &&
|
||||
agcal.gyro_temp_coeff.X > 1e-9f && agcal.gyro_temp_coeff.Y > 1e-9f &&
|
||||
agcal.gyro_temp_coeff.Z > 1e-9f && agcal.gyro_temp_coeff.Z2 > 1e-9f;
|
||||
|
||||
|
||||
AttitudeSettingsData attitudeSettings;
|
||||
|
||||
@ -263,10 +263,10 @@ void ThermalCalibrationHelper::collectSample(UAVObject *sample)
|
||||
Q_ASSERT(reading);
|
||||
m_accelSamples.append(reading->getData());
|
||||
m_debugStream << "ACCEL:: " << m_accelSamples.last().temperature <<
|
||||
"\t" << QDateTime::currentDateTime().toString("hh.mm.ss.zzz") <<
|
||||
"\t" << m_accelSamples.last().x <<
|
||||
"\t" << m_accelSamples.last().y <<
|
||||
"\t" << m_accelSamples.last().z << endl;
|
||||
"\t" << QDateTime::currentDateTime().toString("hh.mm.ss.zzz") <<
|
||||
"\t" << m_accelSamples.last().x <<
|
||||
"\t" << m_accelSamples.last().y <<
|
||||
"\t" << m_accelSamples.last().z << endl;
|
||||
|
||||
break;
|
||||
}
|
||||
@ -276,10 +276,10 @@ void ThermalCalibrationHelper::collectSample(UAVObject *sample)
|
||||
Q_ASSERT(reading);
|
||||
m_gyroSamples.append(reading->getData());
|
||||
m_debugStream << "GYRO:: " << m_gyroSamples.last().temperature <<
|
||||
"\t" << QDateTime::currentDateTime().toString("hh.mm.ss.zzz") <<
|
||||
"\t" << m_gyroSamples.last().x <<
|
||||
"\t" << m_gyroSamples.last().y <<
|
||||
"\t" << m_gyroSamples.last().z << endl;
|
||||
"\t" << QDateTime::currentDateTime().toString("hh.mm.ss.zzz") <<
|
||||
"\t" << m_gyroSamples.last().x <<
|
||||
"\t" << m_gyroSamples.last().y <<
|
||||
"\t" << m_gyroSamples.last().z << endl;
|
||||
break;
|
||||
}
|
||||
case BaroSensor::OBJID:
|
||||
@ -288,9 +288,9 @@ void ThermalCalibrationHelper::collectSample(UAVObject *sample)
|
||||
Q_ASSERT(reading);
|
||||
m_baroSamples.append(reading->getData());
|
||||
m_debugStream << "BARO:: " << m_baroSamples.last().Temperature <<
|
||||
"\t" << QDateTime::currentDateTime().toString("hh.mm.ss.zzz") <<
|
||||
"\t" << m_baroSamples.last().Pressure <<
|
||||
"\t" << m_baroSamples.last().Altitude << endl;
|
||||
"\t" << QDateTime::currentDateTime().toString("hh.mm.ss.zzz") <<
|
||||
"\t" << m_baroSamples.last().Pressure <<
|
||||
"\t" << m_baroSamples.last().Altitude << endl;
|
||||
// this is needed as temperature is low pass filtered
|
||||
m_temperature = reading->getTemperature();
|
||||
updateTemp(m_temperature);
|
||||
@ -302,10 +302,10 @@ void ThermalCalibrationHelper::collectSample(UAVObject *sample)
|
||||
Q_ASSERT(reading);
|
||||
m_magSamples.append(reading->getData());
|
||||
m_debugStream << "MAG:: " <<
|
||||
"\t" << QDateTime::currentDateTime().toString("hh.mm.ss.zzz") <<
|
||||
"\t" << m_magSamples.last().x <<
|
||||
"\t" << m_magSamples.last().y <<
|
||||
"\t" << m_magSamples.last().z << endl;
|
||||
"\t" << QDateTime::currentDateTime().toString("hh.mm.ss.zzz") <<
|
||||
"\t" << m_magSamples.last().x <<
|
||||
"\t" << m_magSamples.last().y <<
|
||||
"\t" << m_magSamples.last().z << endl;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
@ -355,7 +355,7 @@ void ThermalCalibrationHelper::calculate()
|
||||
datat[x] = m_gyroSamples[x].temperature;
|
||||
}
|
||||
|
||||
m_results.gyroCalibrated = ThermalCalibration::GyroscopeCalibration(datax, datay, dataz, datat, m_results.gyro, m_results.gyroInSigma, m_results.gyroOutSigma);
|
||||
m_results.gyroCalibrated = ThermalCalibration::GyroscopeCalibration(datax, datay, dataz, datat, m_results.gyro, m_results.gyroInSigma, m_results.gyroOutSigma);
|
||||
m_results.accelGyroTempMin = datat.array().minCoeff();
|
||||
m_results.accelGyroTempMax = datat.array().maxCoeff();
|
||||
// TODO: sanity checks needs to be enforced before accel calibration can be enabled and usable.
|
||||
@ -376,27 +376,26 @@ void ThermalCalibrationHelper::calculate()
|
||||
|
||||
m_results.accelCalibrated = ThermalCalibration::AccelerometerCalibration(datax, datay, dataz, datat, m_results.accel);
|
||||
*/
|
||||
m_results.accelCalibrated = false;
|
||||
m_results.accelCalibrated = false;
|
||||
QString str;
|
||||
str += QStringLiteral("info::Calibration results");
|
||||
|
||||
str += QStringLiteral("info::Baro cal {%1, %2, %3, %4}; initial variance: %5; Calibrated variance %6")
|
||||
.arg(m_results.baro[0]).arg(m_results.baro[1]).arg(m_results.baro[2]).arg(m_results.baro[3])
|
||||
.arg(m_results.baroInSigma).arg(m_results.baroOutSigma) + QChar::CarriageReturn;
|
||||
.arg(m_results.baro[0]).arg(m_results.baro[1]).arg(m_results.baro[2]).arg(m_results.baro[3])
|
||||
.arg(m_results.baroInSigma).arg(m_results.baroOutSigma) + QChar::CarriageReturn;
|
||||
str += QStringLiteral("info::Gyro cal x{%1} y{%2} z{%3, %4}; initial variance: {%5, %6, %7}; Calibrated variance {%8, %9, %10}")
|
||||
.arg(m_results.gyro[0]).arg(m_results.gyro[1]).arg(m_results.gyro[2]).arg(m_results.baro[3])
|
||||
.arg(m_results.gyroInSigma[0]).arg(m_results.gyroInSigma[1]).arg(m_results.gyroInSigma[2])
|
||||
.arg(m_results.gyroOutSigma[0]).arg(m_results.gyroOutSigma[1]).arg(m_results.gyroOutSigma[2]) + QChar::CarriageReturn;
|
||||
str += QStringLiteral("info::Accel cal x{%1} y{%2} z{%3}; initial variance: {%4, %5, %6}; Calibrated variance {%7, %8, %9}")
|
||||
.arg(m_results.accel[0]).arg(m_results.accel[1]).arg(m_results.accel[2])
|
||||
.arg(m_results.accelInSigma[0]).arg(m_results.accelInSigma[1]).arg(m_results.accelInSigma[2])
|
||||
.arg(m_results.accelOutSigma[0]).arg(m_results.accelOutSigma[1]).arg(m_results.accelOutSigma[2]) + QChar::CarriageReturn;
|
||||
.arg(m_results.gyro[0]).arg(m_results.gyro[1]).arg(m_results.gyro[2]).arg(m_results.baro[3])
|
||||
.arg(m_results.gyroInSigma[0]).arg(m_results.gyroInSigma[1]).arg(m_results.gyroInSigma[2])
|
||||
.arg(m_results.gyroOutSigma[0]).arg(m_results.gyroOutSigma[1]).arg(m_results.gyroOutSigma[2]) + QChar::CarriageReturn;
|
||||
str += QStringLiteral("info::Accel cal x{%1} y{%2} z{%3}; initial variance: {%4, %5, %6}; Calibrated variance {%7, %8, %9}")
|
||||
.arg(m_results.accel[0]).arg(m_results.accel[1]).arg(m_results.accel[2])
|
||||
.arg(m_results.accelInSigma[0]).arg(m_results.accelInSigma[1]).arg(m_results.accelInSigma[2])
|
||||
.arg(m_results.accelOutSigma[0]).arg(m_results.accelOutSigma[1]).arg(m_results.accelOutSigma[2]) + QChar::CarriageReturn;
|
||||
qDebug() << str;
|
||||
m_debugStream << str;
|
||||
m_debugStream << str;
|
||||
copyResultToSettings();
|
||||
emit calculationCompleted();
|
||||
closeDebugLog();
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -439,9 +438,9 @@ void ThermalCalibrationHelper::updateTemp(float temp)
|
||||
qDebug() << "M_gradient " << m_gradient << " Elapsed" << elapsed << " m_initialGradient" << m_initialGradient;
|
||||
// make a rough estimation of the time needed
|
||||
m_targetduration = elapsed * 8;
|
||||
if(m_debugFile.isOpen()){
|
||||
if (m_debugFile.isOpen()) {
|
||||
m_debugStream << "INFO::Trace gradient " << m_gradient << " Elapsed" << elapsed << " m_initialGradient" << m_initialGradient
|
||||
<< " target:" << m_targetduration << endl;
|
||||
<< " target:" << m_targetduration << endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -488,14 +487,14 @@ void ThermalCalibrationHelper::disconnectUAVOs()
|
||||
|
||||
void ThermalCalibrationHelper::createDebugLog()
|
||||
{
|
||||
if(m_debugFile.isOpen()){
|
||||
if (m_debugFile.isOpen()) {
|
||||
closeDebugLog();
|
||||
}
|
||||
if(m_tempdir->isValid()){
|
||||
if (m_tempdir->isValid()) {
|
||||
QString filename = QStringLiteral("thcaldebug_%1.txt").arg(QDateTime::currentDateTime().toString("dd.MM.yyyy-hh.mm.ss.zzz"));
|
||||
QDir dir = QDir(m_tempdir->path());
|
||||
m_debugFile.setFileName(dir.filePath(filename));
|
||||
if (!m_debugFile.open(QIODevice::WriteOnly | QIODevice::Text)){
|
||||
if (!m_debugFile.open(QIODevice::WriteOnly | QIODevice::Text)) {
|
||||
m_debugStream.setDevice(0);
|
||||
return;
|
||||
}
|
||||
@ -504,28 +503,27 @@ void ThermalCalibrationHelper::createDebugLog()
|
||||
m_debugStream.setDevice(&m_debugFile);
|
||||
|
||||
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
|
||||
UAVObjectUtilManager *utilMngr = pm->getObject<UAVObjectUtilManager>();
|
||||
deviceDescriptorStruct board = utilMngr->getBoardDescriptionStruct();
|
||||
UAVObjectUtilManager *utilMngr = pm->getObject<UAVObjectUtilManager>();
|
||||
deviceDescriptorStruct board = utilMngr->getBoardDescriptionStruct();
|
||||
|
||||
m_debugStream << "INFO::Hardware";
|
||||
m_debugStream << " type:" << QString().setNum(board.boardType, 16);
|
||||
m_debugStream << " revision:"<<QString().setNum(board.boardRevision, 16);
|
||||
m_debugStream << " revision:" << QString().setNum(board.boardRevision, 16);
|
||||
m_debugStream << " serial:" << QString(utilMngr->getBoardCPUSerial().toHex()) << endl;
|
||||
|
||||
QString uavo = board.uavoHash.toHex();
|
||||
QString uavo = board.uavoHash.toHex();
|
||||
m_debugStream << "INFO::firmware tag:" << board.gitTag << " date:" << board.gitDate << " hash:" << board.gitHash <<
|
||||
"uavo:" << uavo.left(8) << endl;
|
||||
"uavo:" << uavo.left(8) << endl;
|
||||
|
||||
|
||||
m_debugStream << "INFO::gcs tag:" << VersionInfo::tagOrBranch() + VersionInfo::dirty() << " date:" << VersionInfo::dateTime() <<
|
||||
" hash:" << VersionInfo::hash().left(8) << " uavo:" << VersionInfo::uavoHash().left(8) << endl;
|
||||
|
||||
" hash:" << VersionInfo::hash().left(8) << " uavo:" << VersionInfo::uavoHash().left(8) << endl;
|
||||
}
|
||||
}
|
||||
|
||||
void ThermalCalibrationHelper::closeDebugLog()
|
||||
{
|
||||
if(m_debugFile.isOpen()){
|
||||
if (m_debugFile.isOpen()) {
|
||||
m_debugStream.flush();
|
||||
m_debugStream.setDevice(0);
|
||||
m_debugFile.close();
|
||||
|
||||
@ -161,7 +161,7 @@ private:
|
||||
void connectUAVOs();
|
||||
void disconnectUAVOs();
|
||||
|
||||
QFile m_debugFile;
|
||||
QFile m_debugFile;
|
||||
QTextStream m_debugStream;
|
||||
QScopedPointer<QTemporaryDir> m_tempdir;
|
||||
void createDebugLog();
|
||||
@ -211,7 +211,6 @@ private:
|
||||
void setMetadataForCalibration(UAVDataObject *uavo);
|
||||
UAVObjectManager *getObjectManager();
|
||||
|
||||
//Q_DISABLE_COPY(ThermalCalibrationHelper)
|
||||
};
|
||||
}
|
||||
#endif // THERMALCALIBRATIONHELPER_H
|
||||
|
||||
Loading…
Reference in New Issue
Block a user