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

OP-1149 Changes to GCS to accounts for UAVO Changes from previous commit

Browse Source
This commit is contained in:
Alessio Morale 2013-12-26 16:56:54 +01:00
parent 7ac2f9f87f
commit a340c06b01
4 changed files with 74 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -43,6 +43,7 @@
#include <attitudesettings.h>
#include <ekfconfiguration.h>
#include <revocalibration.h>
#include <accelgyrosettings.h>
#include <homelocation.h>
#include <accelstate.h>
#include <gyrostate.h>
@ -376,19 +377,27 @@ void ConfigRevoWidget::doGetAccelGyroBiasData(UAVObject *obj)
RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration);
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
Q_ASSERT(accelGyroSettings);
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_TRUE;
// Update the biases based on collected data
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X] += listMean(accel_accum_x);
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y] += listMean(accel_accum_y);
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] += (listMean(accel_accum_z) + GRAVITY);
revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_X] += listMean(gyro_accum_x);
revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_Y] += listMean(gyro_accum_y);
revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_Z] += listMean(gyro_accum_z);
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] += listMean(accel_accum_x);
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] += listMean(accel_accum_y);
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] += (listMean(accel_accum_z) + GRAVITY);
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] += listMean(gyro_accum_x);
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] += listMean(gyro_accum_y);
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] += listMean(gyro_accum_z);
revoCalibration->setData(revoCalibrationData);
revoCalibration->updated();
accelGyroSettings->setData(accelGyroSettingsData);
accelGyroSettings->updated();
AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
Q_ASSERT(attitudeSettings);
@ -517,11 +526,13 @@ void ConfigRevoWidget::doStartSixPointCalibration()
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();
HomeLocation::DataFields homeLocationData = homeLocation->getData();
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
// check if Homelocation is set
if (!homeLocationData.Set) {
@ -536,12 +547,12 @@ void ConfigRevoWidget::doStartSixPointCalibration()
#ifdef SIX_POINT_CAL_ACCEL
// Calibration accel
revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_X] = 1;
revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Y] = 1;
revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Z] = 1;
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X] = 0;
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y] = 0;
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] = 0;
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X] = 1;
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y] = 1;
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z] = 1;
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] = 0;
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] = 0;
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] = 0;
accel_accum_x.clear();
accel_accum_y.clear();
@ -561,6 +572,7 @@ void ConfigRevoWidget::doStartSixPointCalibration()
revoCalibrationData.MagBiasNullingRate = 0;
revoCalibration->setData(revoCalibrationData);
accelGyroSettings->setData(accelGyroSettingsData);
Thread::usleep(100000);
@ -739,24 +751,26 @@ void ConfigRevoWidget::computeScaleBias()
{
double S[3], b[3];
double Be_length;
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::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();
HomeLocation::DataFields homeLocationData = homeLocation->getData();
#ifdef SIX_POINT_CAL_ACCEL
// Calibration accel
SixPointInConstFieldCal(homeLocationData.g_e, accel_data_x, accel_data_y, accel_data_z, S, b);
revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_X] = fabs(S[0]);
revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Y] = fabs(S[1]);
revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Z] = fabs(S[2]);
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X] = fabs(S[0]);
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y] = fabs(S[1]);
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z] = fabs(S[2]);
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X] = -sign(S[0]) * b[0];
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y] = -sign(S[1]) * b[1];
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] = -sign(S[2]) * b[2];
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] = -sign(S[0]) * b[0];
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] = -sign(S[1]) * b[1];
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] = -sign(S[2]) * b[2];
#endif
// Calibration mag
@ -791,23 +805,25 @@ void ConfigRevoWidget::computeScaleBias()
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z];
// Check the accel calibration is good
good_calibration &= revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_X] ==
revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_X];
good_calibration &= revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Y] ==
revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Y];
good_calibration &= revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Z] ==
revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Z];
good_calibration &= revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X] ==
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X];
good_calibration &= revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y] ==
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y];
good_calibration &= revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] ==
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z];
good_calibration &= accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X] ==
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X];
good_calibration &= accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y] ==
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y];
good_calibration &= accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z] ==
accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z];
good_calibration &= accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] ==
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X];
good_calibration &= accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] ==
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y];
good_calibration &= accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] ==
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z];
if (good_calibration) {
revoCalibration->setData(revoCalibrationData);
accelGyroSettings->setData(accelGyroSettingsData);
m_ui->sixPointCalibInstructions->append("Computed accel and mag scale and bias...");
} else {
revoCalibrationData = revoCalibration->getData();
accelGyroSettingsData = accelGyroSettings->getData();
m_ui->sixPointCalibInstructions->append("Bad calibration. Please repeat.");
}
#else // ifdef SIX_POINT_CAL_ACCEL
@ -826,9 +842,11 @@ void ConfigRevoWidget::computeScaleBias()
revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z];
if (good_calibration) {
revoCalibration->setData(revoCalibrationData);
accelGyroSettings->setData(accelGyroSettingsData);
m_ui->sixPointCalibInstructions->append("Computed mag scale and bias...");
} else {
revoCalibrationData = revoCalibration->getData();
accelGyroSettingsData = accelGyroSettings->getData();
m_ui->sixPointCalibInstructions->append("Bad calibration. Please repeat.");
}
#endif // ifdef SIX_POINT_CAL_ACCEL

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

@ -33,6 +33,7 @@
#include "gyrostate.h"
#include "qdebug.h"
#include "revocalibration.h"
#include "accelgyrosettings.h"
BiasCalibrationUtil::BiasCalibrationUtil(long measurementCount, long measurementRate) : QObject(),
@ -140,13 +141,18 @@ void BiasCalibrationUtil::startMeasurement()
RevoCalibration *revolutionCalibration = RevoCalibration::GetInstance(uavObjectManager);
Q_ASSERT(revolutionCalibration);
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(uavObjectManager);
Q_ASSERT(accelGyroSettings);
RevoCalibration::DataFields revoCalibrationData = revolutionCalibration->getData();
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X] = 0;
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y] = 0;
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] = 0;
revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_X] = 0;
revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_Y] = 0;
revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_Z] = 0;
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] = 0;
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] = 0;
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] = 0;
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++) {

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

@ -35,6 +35,7 @@
#include "manualcontrolsettings.h"
#include "stabilizationsettings.h"
#include "revocalibration.h"
#include "accelgyrosettings.h"
const qint16 VehicleConfigurationHelper::LEGACY_ESC_FREQUENCE = 50;
const qint16 VehicleConfigurationHelper::RAPID_ESC_FREQUENCE = 400;
@ -368,19 +369,24 @@ 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;
data.accel_bias[RevoCalibration::ACCEL_BIAS_X] += bias.m_accelerometerXBias;
data.accel_bias[RevoCalibration::ACCEL_BIAS_Y] += bias.m_accelerometerYBias;
data.accel_bias[RevoCalibration::ACCEL_BIAS_Z] += bias.m_accelerometerZBias + G;
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;
data.gyro_bias[RevoCalibration::GYRO_BIAS_X] = bias.m_gyroXBias;
data.gyro_bias[RevoCalibration::GYRO_BIAS_Y] = bias.m_gyroYBias;
data.gyro_bias[RevoCalibration::GYRO_BIAS_Z] = bias.m_gyroZBias;
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"));
break;
}
default:

2
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -26,6 +26,7 @@ OTHER_FILES += UAVObjects.pluginspec
# Add in all of the synthetic/generated uavobject files
HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/accelgyrosettings.h \
$$UAVOBJECT_SYNTHETICS/barosensor.h \
$$UAVOBJECT_SYNTHETICS/airspeedsensor.h \
$$UAVOBJECT_SYNTHETICS/airspeedsettings.h \
@ -117,6 +118,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/mpu6000settings.h
SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/accelgyrosettings.cpp \
$$UAVOBJECT_SYNTHETICS/barosensor.cpp \
$$UAVOBJECT_SYNTHETICS/airspeedsensor.cpp \
$$UAVOBJECT_SYNTHETICS/airspeedsettings.cpp \