rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +01:00
Code Issues Releases Activity

OP-1149 Changes to GCS to use AccelGyroSettings for bias calculations

Browse Source
This commit is contained in:
Alessio Morale 2013-12-29 18:58:45 +01:00
parent e04cef2fa4
commit 4b5ee1c2fb
4 changed files with 46 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp
View File

@ -34,6 +34,7 @@
#include <QDesktopServices> #include <QDesktopServices>
#include <QUrl> #include <QUrl>
#include "accelstate.h" #include "accelstate.h"
#include "accelgyrosettings.h"
#include "gyrostate.h" #include "gyrostate.h"
#include <extensionsystem/pluginmanager.h> #include <extensionsystem/pluginmanager.h>
#include <coreplugin/generalsettings.h> #include <coreplugin/generalsettings.h>
@ -54,6 +55,7 @@ ConfigCCAttitudeWidget::ConfigCCAttitudeWidget(QWidget *parent) :
addApplySaveButtons(ui->applyButton, ui->saveButton); addApplySaveButtons(ui->applyButton, ui->saveButton);
addUAVObject("AttitudeSettings"); addUAVObject("AttitudeSettings");
addUAVObject("AccelGyroSettings");
// Connect the help button // Connect the help button
connect(ui->ccAttitudeHelp, SIGNAL(clicked()), this, SLOT(openHelp())); 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 y_bias = listMean(y_accum) / ACCEL_SCALE;
float z_bias = (listMean(z_accum) + 9.81) / ACCEL_SCALE; float z_bias = (listMean(z_accum) + 9.81) / ACCEL_SCALE;
float x_gyro_bias = listMean(x_gyro_accum) * 100.0f; float x_gyro_bias = listMean(x_gyro_accum);
float y_gyro_bias = listMean(y_gyro_accum) * 100.0f; float y_gyro_bias = listMean(y_gyro_accum);
float z_gyro_bias = listMean(z_gyro_accum) * 100.0f; float z_gyro_bias = listMean(z_gyro_accum);
accelState->setMetadata(initialAccelStateMdata); accelState->setMetadata(initialAccelStateMdata);
gyroState->setMetadata(initialGyroStateMdata); 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 // We offset the gyro bias by current bias to help precision
attitudeSettingsData.AccelBias[0] += x_bias; accelGyroSettingsData.accel_bias[0] += x_bias;
attitudeSettingsData.AccelBias[1] += y_bias; accelGyroSettingsData.accel_bias[1] += y_bias;
attitudeSettingsData.AccelBias[2] += z_bias; accelGyroSettingsData.accel_bias[2] += z_bias;
attitudeSettingsData.GyroBias[0] = -x_gyro_bias; accelGyroSettingsData.gyro_bias[0] = -x_gyro_bias;
attitudeSettingsData.GyroBias[1] = -y_gyro_bias; accelGyroSettingsData.gyro_bias[1] = -y_gyro_bias;
attitudeSettingsData.GyroBias[2] = -z_gyro_bias; accelGyroSettingsData.gyro_bias[2] = -z_gyro_bias;
attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE; attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE;
AttitudeSettings::GetInstance(getObjectManager())->setData(attitudeSettingsData); AttitudeSettings::GetInstance(getObjectManager())->setData(attitudeSettingsData);
AccelGyroSettings::GetInstance(getObjectManager())->setData(accelGyroSettingsData);
this->setDirty(true); this->setDirty(true);
// reenable controls // reenable controls

16
ground/openpilotgcs/src/plugins/config/configrevowidget.cpp
View File

@ -375,13 +375,13 @@ void ConfigRevoWidget::doGetAccelGyroBiasData(UAVObject *obj)
m_ui->accelBiasStart->setEnabled(true); m_ui->accelBiasStart->setEnabled(true);
RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager()); RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration); Q_ASSERT(revoCalibration);
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager()); AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
Q_ASSERT(accelGyroSettings); Q_ASSERT(accelGyroSettings);
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData(); AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_TRUE; revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_TRUE;
@ -524,13 +524,13 @@ void ConfigRevoWidget::doStartSixPointCalibration()
isBoardRotationStored = false; isBoardRotationStored = false;
storeAndClearBoardRotation(); storeAndClearBoardRotation();
RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager()); RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager()); HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager()); AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration); Q_ASSERT(revoCalibration);
Q_ASSERT(homeLocation); Q_ASSERT(homeLocation);
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
HomeLocation::DataFields homeLocationData = homeLocation->getData(); HomeLocation::DataFields homeLocationData = homeLocation->getData();
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData(); AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
@ -752,13 +752,13 @@ void ConfigRevoWidget::computeScaleBias()
double S[3], b[3]; double S[3], b[3];
double Be_length; double Be_length;
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager()); AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager()); RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager()); HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration); Q_ASSERT(revoCalibration);
Q_ASSERT(homeLocation); Q_ASSERT(homeLocation);
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData(); AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
HomeLocation::DataFields homeLocationData = homeLocation->getData(); HomeLocation::DataFields homeLocationData = homeLocation->getData();
#ifdef SIX_POINT_CAL_ACCEL #ifdef SIX_POINT_CAL_ACCEL
@ -822,7 +822,7 @@ void ConfigRevoWidget::computeScaleBias()
accelGyroSettings->setData(accelGyroSettingsData); accelGyroSettings->setData(accelGyroSettingsData);
m_ui->sixPointCalibInstructions->append("Computed accel and mag scale and bias..."); m_ui->sixPointCalibInstructions->append("Computed accel and mag scale and bias...");
} else { } else {
revoCalibrationData = revoCalibration->getData(); revoCalibrationData = revoCalibration->getData();
accelGyroSettingsData = accelGyroSettings->getData(); accelGyroSettingsData = accelGyroSettings->getData();
m_ui->sixPointCalibInstructions->append("Bad calibration. Please repeat."); m_ui->sixPointCalibInstructions->append("Bad calibration. Please repeat.");
} }
@ -845,7 +845,7 @@ void ConfigRevoWidget::computeScaleBias()
accelGyroSettings->setData(accelGyroSettingsData); accelGyroSettings->setData(accelGyroSettingsData);
m_ui->sixPointCalibInstructions->append("Computed mag scale and bias..."); m_ui->sixPointCalibInstructions->append("Computed mag scale and bias...");
} else { } else {
revoCalibrationData = revoCalibration->getData(); revoCalibrationData = revoCalibration->getData();
accelGyroSettingsData = accelGyroSettings->getData(); accelGyroSettingsData = accelGyroSettings->getData();
m_ui->sixPointCalibInstructions->append("Bad calibration. Please repeat."); m_ui->sixPointCalibInstructions->append("Bad calibration. Please repeat.");
} }

23
ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp
View File

@ -141,11 +141,16 @@ void BiasCalibrationUtil::startMeasurement()
RevoCalibration *revolutionCalibration = RevoCalibration::GetInstance(uavObjectManager); RevoCalibration *revolutionCalibration = RevoCalibration::GetInstance(uavObjectManager);
Q_ASSERT(revolutionCalibration); Q_ASSERT(revolutionCalibration);
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(uavObjectManager); AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(uavObjectManager);
Q_ASSERT(accelGyroSettings); Q_ASSERT(accelGyroSettings);
RevoCalibration::DataFields revoCalibrationData = revolutionCalibration->getData(); RevoCalibration::DataFields revoCalibrationData = revolutionCalibration->getData();
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->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_X] = 0;
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] = 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_X] = 0;
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] = 0; accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] = 0;
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] = 0; accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] = 0;
revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_FALSE;
int i; int i;
for (i = 0; i < 5; i++) { for (i = 0; i < 5; i++) {
RevoCalibration::GetInstance(uavObjectManager)->setData(revoCalibrationData); RevoCalibration::GetInstance(uavObjectManager)->setData(revoCalibrationData);
} AccelGyroSettings::GetInstance(uavObjectManager)->setData(accelGyroSettingsData);
// 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++) {
AttitudeSettings::GetInstance(uavObjectManager)->setData(attitudeSettingsData); AttitudeSettings::GetInstance(uavObjectManager)->setData(attitudeSettingsData);
} }
// Set up to receive updates for accels // Set up to receive updates for accels

46
ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp
View File

@ -337,31 +337,33 @@ void VehicleConfigurationHelper::applySensorBiasConfiguration()
accelGyroBias bias = m_configSource->getCalibrationBias(); accelGyroBias bias = m_configSource->getCalibrationBias();
float G = 9.81f; 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()) { switch (m_configSource->getControllerType()) {
case VehicleConfigurationSource::CONTROLLER_CC: case VehicleConfigurationSource::CONTROLLER_CC:
case VehicleConfigurationSource::CONTROLLER_CC3D: case VehicleConfigurationSource::CONTROLLER_CC3D:
{ {
const float ACCELERATION_SCALE = 0.004f * G;
const float GYRO_SCALE = 100.0f;
AttitudeSettings *copterControlCalibration = AttitudeSettings::GetInstance(m_uavoManager); AttitudeSettings *copterControlCalibration = AttitudeSettings::GetInstance(m_uavoManager);
Q_ASSERT(copterControlCalibration); Q_ASSERT(copterControlCalibration);
AttitudeSettings::DataFields data = copterControlCalibration->getData(); AttitudeSettings::DataFields data = copterControlCalibration->getData();
data.AccelTau = DEFAULT_ENABLED_ACCEL_TAU; 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; data.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE;
copterControlCalibration->setData(data); copterControlCalibration->setData(data);
addModifiedObject(copterControlCalibration, tr("Writing gyro and accelerometer bias settings")); addModifiedObject(copterControlCalibration, tr("Writing board settings"));
break; break;
} }
case VehicleConfigurationSource::CONTROLLER_REVO: case VehicleConfigurationSource::CONTROLLER_REVO:
@ -369,24 +371,11 @@ void VehicleConfigurationHelper::applySensorBiasConfiguration()
RevoCalibration *revolutionCalibration = RevoCalibration::GetInstance(m_uavoManager); RevoCalibration *revolutionCalibration = RevoCalibration::GetInstance(m_uavoManager);
Q_ASSERT(revolutionCalibration); Q_ASSERT(revolutionCalibration);
RevoCalibration::DataFields data = revolutionCalibration->getData(); RevoCalibration::DataFields data = revolutionCalibration->getData();
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(m_uavoManager);
Q_ASSERT(accelGyroSettings);
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
data.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_TRUE; 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); revolutionCalibration->setData(data);
accelGyroSettings->setData(accelGyroSettingsData); addModifiedObject(revolutionCalibration, tr("Writing board settings"));
addModifiedObject(revolutionCalibration, tr("Writing gyro and accelerometer bias settings"));
addModifiedObject(accelGyroSettings, tr("Writing gyro and accelerometer bias settings"));
break; break;
} }
default: default:
@ -399,10 +388,7 @@ void VehicleConfigurationHelper::applySensorBiasConfiguration()
void VehicleConfigurationHelper::applyStabilizationConfiguration() void VehicleConfigurationHelper::applyStabilizationConfiguration()
{ {
StabilizationSettings *stabSettings = StabilizationSettings::GetInstance(m_uavoManager); StabilizationSettings *stabSettings = StabilizationSettings::GetInstance(m_uavoManager);
Q_ASSERT(stabSettings); Q_ASSERT(stabSettings);
StabilizationSettings::DataFields data = stabSettings->getData();
StabilizationSettings defaultSettings; StabilizationSettings defaultSettings;
stabSettings->setData(defaultSettings.getData()); stabSettings->setData(defaultSettings.getData());
addModifiedObject(stabSettings, tr("Writing stabilization settings")); addModifiedObject(stabSettings, tr("Writing stabilization settings"));