From 4b5ee1c2fb209220c39c72fcf9474bd75cff0ebf Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 29 Dec 2013 18:58:45 +0100 Subject: [PATCH] OP-1149 Changes to GCS to use AccelGyroSettings for bias calculations --- .../plugins/config/configccattitudewidget.cpp | 24 ++++++---- .../src/plugins/config/configrevowidget.cpp | 16 +++---- .../setupwizard/biascalibrationutil.cpp | 23 ++++------ .../vehicleconfigurationhelper.cpp | 46 +++++++------------ 4 files changed, 46 insertions(+), 63 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp index 17a4269ff..180905898 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp @@ -34,6 +34,7 @@ #include #include #include "accelstate.h" +#include "accelgyrosettings.h" #include "gyrostate.h" #include #include @@ -54,6 +55,7 @@ ConfigCCAttitudeWidget::ConfigCCAttitudeWidget(QWidget *parent) : addApplySaveButtons(ui->applyButton, ui->saveButton); addUAVObject("AttitudeSettings"); + addUAVObject("AccelGyroSettings"); // Connect the help button connect(ui->ccAttitudeHelp, SIGNAL(clicked()), this, SLOT(openHelp())); @@ -115,22 +117,24 @@ void ConfigCCAttitudeWidget::sensorsUpdated(UAVObject *obj) float y_bias = listMean(y_accum) / ACCEL_SCALE; float z_bias = (listMean(z_accum) + 9.81) / ACCEL_SCALE; - float x_gyro_bias = listMean(x_gyro_accum) * 100.0f; - float y_gyro_bias = listMean(y_gyro_accum) * 100.0f; - float z_gyro_bias = listMean(z_gyro_accum) * 100.0f; + float x_gyro_bias = listMean(x_gyro_accum); + float y_gyro_bias = listMean(y_gyro_accum); + float z_gyro_bias = listMean(z_gyro_accum); accelState->setMetadata(initialAccelStateMdata); gyroState->setMetadata(initialGyroStateMdata); - AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(getObjectManager())->getData(); + AccelGyroSettings::DataFields accelGyroSettingsData = AccelGyroSettings::GetInstance(getObjectManager())->getData(); + AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(getObjectManager())->getData(); // We offset the gyro bias by current bias to help precision - attitudeSettingsData.AccelBias[0] += x_bias; - attitudeSettingsData.AccelBias[1] += y_bias; - attitudeSettingsData.AccelBias[2] += z_bias; - attitudeSettingsData.GyroBias[0] = -x_gyro_bias; - attitudeSettingsData.GyroBias[1] = -y_gyro_bias; - attitudeSettingsData.GyroBias[2] = -z_gyro_bias; + accelGyroSettingsData.accel_bias[0] += x_bias; + accelGyroSettingsData.accel_bias[1] += y_bias; + accelGyroSettingsData.accel_bias[2] += z_bias; + accelGyroSettingsData.gyro_bias[0] = -x_gyro_bias; + accelGyroSettingsData.gyro_bias[1] = -y_gyro_bias; + accelGyroSettingsData.gyro_bias[2] = -z_gyro_bias; attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE; AttitudeSettings::GetInstance(getObjectManager())->setData(attitudeSettingsData); + AccelGyroSettings::GetInstance(getObjectManager())->setData(accelGyroSettingsData); this->setDirty(true); // reenable controls diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp index ee02a476c..081c16874 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp @@ -375,13 +375,13 @@ void ConfigRevoWidget::doGetAccelGyroBiasData(UAVObject *obj) m_ui->accelBiasStart->setEnabled(true); - RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager()); + RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager()); Q_ASSERT(revoCalibration); AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager()); Q_ASSERT(accelGyroSettings); - RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); + RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData(); revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_TRUE; @@ -524,13 +524,13 @@ void ConfigRevoWidget::doStartSixPointCalibration() isBoardRotationStored = false; storeAndClearBoardRotation(); - RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager()); + RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager()); HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager()); AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager()); Q_ASSERT(revoCalibration); Q_ASSERT(homeLocation); - RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); + RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); HomeLocation::DataFields homeLocationData = homeLocation->getData(); AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData(); @@ -752,13 +752,13 @@ void ConfigRevoWidget::computeScaleBias() double S[3], b[3]; double Be_length; AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager()); - RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager()); + RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager()); HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager()); Q_ASSERT(revoCalibration); Q_ASSERT(homeLocation); AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData(); - RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); + RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); HomeLocation::DataFields homeLocationData = homeLocation->getData(); #ifdef SIX_POINT_CAL_ACCEL @@ -822,7 +822,7 @@ void ConfigRevoWidget::computeScaleBias() accelGyroSettings->setData(accelGyroSettingsData); m_ui->sixPointCalibInstructions->append("Computed accel and mag scale and bias..."); } else { - revoCalibrationData = revoCalibration->getData(); + revoCalibrationData = revoCalibration->getData(); accelGyroSettingsData = accelGyroSettings->getData(); m_ui->sixPointCalibInstructions->append("Bad calibration. Please repeat."); } @@ -845,7 +845,7 @@ void ConfigRevoWidget::computeScaleBias() accelGyroSettings->setData(accelGyroSettingsData); m_ui->sixPointCalibInstructions->append("Computed mag scale and bias..."); } else { - revoCalibrationData = revoCalibration->getData(); + revoCalibrationData = revoCalibration->getData(); accelGyroSettingsData = accelGyroSettings->getData(); m_ui->sixPointCalibInstructions->append("Bad calibration. Please repeat."); } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp b/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp index 36411013e..20136dcec 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp @@ -141,11 +141,16 @@ void BiasCalibrationUtil::startMeasurement() RevoCalibration *revolutionCalibration = RevoCalibration::GetInstance(uavObjectManager); Q_ASSERT(revolutionCalibration); - AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(uavObjectManager); + AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(uavObjectManager); Q_ASSERT(accelGyroSettings); - RevoCalibration::DataFields revoCalibrationData = revolutionCalibration->getData(); + RevoCalibration::DataFields revoCalibrationData = revolutionCalibration->getData(); AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData(); + // Disable gyro bias correction to see raw data + AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(uavObjectManager)->getData(); + + attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE; + revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_FALSE; accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] = 0; accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] = 0; @@ -153,22 +158,10 @@ void BiasCalibrationUtil::startMeasurement() accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] = 0; accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] = 0; accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] = 0; - revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_FALSE; int i; for (i = 0; i < 5; i++) { RevoCalibration::GetInstance(uavObjectManager)->setData(revoCalibrationData); - } - - // Disable gyro bias correction to see raw data - AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(uavObjectManager)->getData(); - attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE; - attitudeSettingsData.AccelBias[AttitudeSettings::ACCELBIAS_X] = 0; - attitudeSettingsData.AccelBias[AttitudeSettings::ACCELBIAS_Y] = 0; - attitudeSettingsData.AccelBias[AttitudeSettings::ACCELBIAS_Z] = 0; - attitudeSettingsData.GyroBias[AttitudeSettings::GYROBIAS_X] = 0; - attitudeSettingsData.GyroBias[AttitudeSettings::GYROBIAS_Y] = 0; - attitudeSettingsData.GyroBias[AttitudeSettings::GYROBIAS_Z] = 0; - for (i = 0; i < 5; i++) { + AccelGyroSettings::GetInstance(uavObjectManager)->setData(accelGyroSettingsData); AttitudeSettings::GetInstance(uavObjectManager)->setData(attitudeSettingsData); } // Set up to receive updates for accels diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index 693dc61a3..7c8eb3aa7 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -337,31 +337,33 @@ void VehicleConfigurationHelper::applySensorBiasConfiguration() accelGyroBias bias = m_configSource->getCalibrationBias(); float G = 9.81f; + AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(m_uavoManager); + Q_ASSERT(accelGyroSettings); + AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData(); + + accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] += bias.m_accelerometerXBias; + accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] += bias.m_accelerometerYBias; + accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] += bias.m_accelerometerZBias + G; + accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] = bias.m_gyroXBias; + accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] = bias.m_gyroYBias; + accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] = bias.m_gyroZBias; + + accelGyroSettings->setData(accelGyroSettingsData); + addModifiedObject(accelGyroSettings, tr("Writing gyro and accelerometer bias settings")); + switch (m_configSource->getControllerType()) { case VehicleConfigurationSource::CONTROLLER_CC: case VehicleConfigurationSource::CONTROLLER_CC3D: { - const float ACCELERATION_SCALE = 0.004f * G; - const float GYRO_SCALE = 100.0f; - AttitudeSettings *copterControlCalibration = AttitudeSettings::GetInstance(m_uavoManager); Q_ASSERT(copterControlCalibration); AttitudeSettings::DataFields data = copterControlCalibration->getData(); data.AccelTau = DEFAULT_ENABLED_ACCEL_TAU; - - data.AccelBias[AttitudeSettings::ACCELBIAS_X] += bias.m_accelerometerXBias / ACCELERATION_SCALE; - data.AccelBias[AttitudeSettings::ACCELBIAS_Y] += bias.m_accelerometerYBias / ACCELERATION_SCALE; - data.AccelBias[AttitudeSettings::ACCELBIAS_Z] += (bias.m_accelerometerZBias + G) / ACCELERATION_SCALE; - - data.GyroBias[AttitudeSettings::GYROBIAS_X] = -(bias.m_gyroXBias * GYRO_SCALE); - data.GyroBias[AttitudeSettings::GYROBIAS_Y] = -(bias.m_gyroYBias * GYRO_SCALE); - data.GyroBias[AttitudeSettings::GYROBIAS_Z] = -(bias.m_gyroZBias * GYRO_SCALE); - data.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE; copterControlCalibration->setData(data); - addModifiedObject(copterControlCalibration, tr("Writing gyro and accelerometer bias settings")); + addModifiedObject(copterControlCalibration, tr("Writing board settings")); break; } case VehicleConfigurationSource::CONTROLLER_REVO: @@ -369,24 +371,11 @@ void VehicleConfigurationHelper::applySensorBiasConfiguration() RevoCalibration *revolutionCalibration = RevoCalibration::GetInstance(m_uavoManager); Q_ASSERT(revolutionCalibration); RevoCalibration::DataFields data = revolutionCalibration->getData(); - AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(m_uavoManager); - Q_ASSERT(accelGyroSettings); - AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData(); data.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_TRUE; - accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] += bias.m_accelerometerXBias; - accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] += bias.m_accelerometerYBias; - accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] += bias.m_accelerometerZBias + G; - - accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] = bias.m_gyroXBias; - accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] = bias.m_gyroYBias; - accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] = bias.m_gyroZBias; - revolutionCalibration->setData(data); - accelGyroSettings->setData(accelGyroSettingsData); - addModifiedObject(revolutionCalibration, tr("Writing gyro and accelerometer bias settings")); - addModifiedObject(accelGyroSettings, tr("Writing gyro and accelerometer bias settings")); + addModifiedObject(revolutionCalibration, tr("Writing board settings")); break; } default: @@ -399,10 +388,7 @@ void VehicleConfigurationHelper::applySensorBiasConfiguration() void VehicleConfigurationHelper::applyStabilizationConfiguration() { StabilizationSettings *stabSettings = StabilizationSettings::GetInstance(m_uavoManager); - Q_ASSERT(stabSettings); - StabilizationSettings::DataFields data = stabSettings->getData(); - StabilizationSettings defaultSettings; stabSettings->setData(defaultSettings.getData()); addModifiedObject(stabSettings, tr("Writing stabilization settings"));