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

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

@ -33,6 +33,7 @@
#include "gyrostate.h" #include "gyrostate.h"
#include "qdebug.h" #include "qdebug.h"
#include "revocalibration.h" #include "revocalibration.h"
#include "accelgyrosettings.h"
BiasCalibrationUtil::BiasCalibrationUtil(long measurementCount, long measurementRate) : QObject(), BiasCalibrationUtil::BiasCalibrationUtil(long measurementCount, long measurementRate) : QObject(),
@ -140,13 +141,18 @@ void BiasCalibrationUtil::startMeasurement()
RevoCalibration *revolutionCalibration = RevoCalibration::GetInstance(uavObjectManager); RevoCalibration *revolutionCalibration = RevoCalibration::GetInstance(uavObjectManager);
Q_ASSERT(revolutionCalibration); Q_ASSERT(revolutionCalibration);
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(uavObjectManager);
Q_ASSERT(accelGyroSettings);
RevoCalibration::DataFields revoCalibrationData = revolutionCalibration->getData(); RevoCalibration::DataFields revoCalibrationData = revolutionCalibration->getData();
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X] = 0; AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y] = 0;
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] = 0; accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] = 0;
revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_X] = 0; accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] = 0;
revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_Y] = 0; accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] = 0;
revoCalibrationData.gyro_bias[RevoCalibration::GYRO_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; revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_FALSE;
int i; int i;
for (i = 0; i < 5; 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 "manualcontrolsettings.h"
#include "stabilizationsettings.h" #include "stabilizationsettings.h"
#include "revocalibration.h" #include "revocalibration.h"
#include "accelgyrosettings.h"
const qint16 VehicleConfigurationHelper::LEGACY_ESC_FREQUENCE = 50; const qint16 VehicleConfigurationHelper::LEGACY_ESC_FREQUENCE = 50;
const qint16 VehicleConfigurationHelper::RAPID_ESC_FREQUENCE = 400; const qint16 VehicleConfigurationHelper::RAPID_ESC_FREQUENCE = 400;
@ -368,19 +369,24 @@ 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;
data.accel_bias[RevoCalibration::ACCEL_BIAS_X] += bias.m_accelerometerXBias; accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] += bias.m_accelerometerXBias;
data.accel_bias[RevoCalibration::ACCEL_BIAS_Y] += bias.m_accelerometerYBias; accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] += bias.m_accelerometerYBias;
data.accel_bias[RevoCalibration::ACCEL_BIAS_Z] += bias.m_accelerometerZBias + G; accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] += bias.m_accelerometerZBias + G;
data.gyro_bias[RevoCalibration::GYRO_BIAS_X] = bias.m_gyroXBias; accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] = bias.m_gyroXBias;
data.gyro_bias[RevoCalibration::GYRO_BIAS_Y] = bias.m_gyroYBias; accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] = bias.m_gyroYBias;
data.gyro_bias[RevoCalibration::GYRO_BIAS_Z] = bias.m_gyroZBias; accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] = bias.m_gyroZBias;
revolutionCalibration->setData(data); revolutionCalibration->setData(data);
accelGyroSettings->setData(accelGyroSettingsData);
addModifiedObject(revolutionCalibration, tr("Writing gyro and accelerometer bias settings")); addModifiedObject(revolutionCalibration, tr("Writing gyro and accelerometer bias settings"));
addModifiedObject(accelGyroSettings, tr("Writing gyro and accelerometer bias settings"));
break; break;
} }
default: 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 # Add in all of the synthetic/generated uavobject files
HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/accelgyrosettings.h \
$$UAVOBJECT_SYNTHETICS/barosensor.h \ $$UAVOBJECT_SYNTHETICS/barosensor.h \
$$UAVOBJECT_SYNTHETICS/airspeedsensor.h \ $$UAVOBJECT_SYNTHETICS/airspeedsensor.h \
$$UAVOBJECT_SYNTHETICS/airspeedsettings.h \ $$UAVOBJECT_SYNTHETICS/airspeedsettings.h \
@ -117,6 +118,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/mpu6000settings.h $$UAVOBJECT_SYNTHETICS/mpu6000settings.h
SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/accelgyrosettings.cpp \
$$UAVOBJECT_SYNTHETICS/barosensor.cpp \ $$UAVOBJECT_SYNTHETICS/barosensor.cpp \
$$UAVOBJECT_SYNTHETICS/airspeedsensor.cpp \ $$UAVOBJECT_SYNTHETICS/airspeedsensor.cpp \
$$UAVOBJECT_SYNTHETICS/airspeedsettings.cpp \ $$UAVOBJECT_SYNTHETICS/airspeedsettings.cpp \