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-975 Use GyroSensor UAVO when available for gyro calibration (on revo GyroState is biased by EKF gyrobias, so it reads almost always 0)

Browse Source
This commit is contained in:
Alessio Morale 2014-05-07 22:34:27 +02:00
parent bd7b1f8d57
commit 96904bdf50
4 changed files with 87 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
ground/openpilotgcs/src/plugins/config/calibration/gyrobiascalibrationmodel.cpp
View File

@ -29,6 +29,7 @@
#include <attitudesettings.h>
#include "extensionsystem/pluginmanager.h"
#include <gyrostate.h>
#include <gyrosensor.h>
#include <revocalibration.h>
#include <accelgyrosettings.h>
#include "calibration/gyrobiascalibrationmodel.h"
@ -67,6 +68,7 @@ void GyroBiasCalibrationModel::start()
attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE;
attitudeSettings->setData(attitudeSettingsData);
attitudeSettings->updated();
displayVisualHelp(CALIBRATION_HELPER_PLANE_PREFIX + CALIBRATION_HELPER_IMAGE_NED);
displayInstructions(tr("Calibrating the gyroscopes. Keep the copter/plane steady..."), true);
@ -74,19 +76,33 @@ void GyroBiasCalibrationModel::start()
gyro_accum_y.clear();
gyro_accum_z.clear();
UAVObject::Metadata mdata;
gyro_state_accum_x.clear();
gyro_state_accum_y.clear();
gyro_state_accum_z.clear();
UAVObject::Metadata metadata;
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
Q_ASSERT(gyroState);
initialGyroStateMdata = gyroState->getMetadata();
mdata = initialGyroStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
gyroState->setMetadata(mdata);
metadata = initialGyroStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(metadata, UAVObject::UPDATEMODE_PERIODIC);
metadata.flightTelemetryUpdatePeriod = 100;
gyroState->setMetadata(metadata);
UAVObject::Metadata gyroSensorMetadata;
GyroSensor *gyroSensor = GyroSensor::GetInstance(getObjectManager());
Q_ASSERT(gyroSensor);
initialGyroSensorMdata = gyroSensor->getMetadata();
gyroSensorMetadata = initialGyroSensorMdata;
UAVObject::SetFlightTelemetryUpdateMode(gyroSensorMetadata, UAVObject::UPDATEMODE_PERIODIC);
gyroSensorMetadata.flightTelemetryUpdatePeriod = 100;
gyroSensor->setMetadata(gyroSensorMetadata);
// Now connect to the accels and mag updates, gather for 100 samples
collectingData = true;
connect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
connect(gyroSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
}
/**
@ -105,9 +121,20 @@ void GyroBiasCalibrationModel::getSample(UAVObject *obj)
Q_ASSERT(gyroState);
GyroState::DataFields gyroStateData = gyroState->getData();
gyro_accum_x.append(gyroStateData.x);
gyro_accum_y.append(gyroStateData.y);
gyro_accum_z.append(gyroStateData.z);
gyro_state_accum_x.append(gyroStateData.x);
gyro_state_accum_y.append(gyroStateData.y);
gyro_state_accum_z.append(gyroStateData.z);
break;
}
case GyroSensor::OBJID:
{
GyroSensor *gyroSensor = GyroSensor::GetInstance(getObjectManager());
Q_ASSERT(gyroSensor);
GyroSensor::DataFields gyroSensorData = gyroSensor->getData();
gyro_accum_x.append(gyroSensorData.x);
gyro_accum_y.append(gyroSensorData.y);
gyro_accum_z.append(gyroSensorData.z);
break;
}
default:
@ -115,17 +142,21 @@ void GyroBiasCalibrationModel::getSample(UAVObject *obj)
}
// Work out the progress based on whichever has less
double p1 = (double)gyro_accum_x.size() / (double)LEVEL_SAMPLES;
double p1 = (double)gyro_state_accum_x.size() / (double)LEVEL_SAMPLES;
double p2 = (double)gyro_accum_y.size() / (double)LEVEL_SAMPLES;
progressChanged(((p1 < p2) ? p1 : p2) * 100);
progressChanged(((p1 > p2) ? p1 : p2) * 100);
if (gyro_accum_y.size() >= LEVEL_SAMPLES &&
if ((gyro_accum_y.size() >= LEVEL_SAMPLES || (gyro_accum_y.size() == 0 && gyro_state_accum_y.size() >= LEVEL_SAMPLES)) &&
collectingData == true) {
collectingData = false;
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
disconnect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
Q_ASSERT(gyroState);
GyroSensor *gyroSensor = GyroSensor::GetInstance(getObjectManager());
Q_ASSERT(gyroSensor);
disconnect(gyroSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
enableAllCalibrations();
@ -138,11 +169,17 @@ void GyroBiasCalibrationModel::getSample(UAVObject *obj)
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_TRUE;
// Update biases based on collected data
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] += OpenPilot::CalibrationUtils::listMean(gyro_accum_x);
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] += OpenPilot::CalibrationUtils::listMean(gyro_accum_y);
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] += OpenPilot::CalibrationUtils::listMean(gyro_accum_z);
// check whether the board does supports gyroSensor(updates were received)
if (gyro_accum_x.count() < LEVEL_SAMPLES / 10) {
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] += OpenPilot::CalibrationUtils::listMean(gyro_state_accum_x);
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] += OpenPilot::CalibrationUtils::listMean(gyro_state_accum_y);
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] += OpenPilot::CalibrationUtils::listMean(gyro_state_accum_z);
} else {
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] += OpenPilot::CalibrationUtils::listMean(gyro_accum_x);
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] += OpenPilot::CalibrationUtils::listMean(gyro_accum_y);
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] += OpenPilot::CalibrationUtils::listMean(gyro_accum_z);
}
revoCalibration->setData(revoCalibrationData);
revoCalibration->updated();
@ -157,6 +194,7 @@ void GyroBiasCalibrationModel::getSample(UAVObject *obj)
attitudeSettings->updated();
gyroState->setMetadata(initialGyroStateMdata);
gyroSensor->setMetadata(initialGyroSensorMdata);
displayInstructions(tr("Gyroscope calibration computed succesfully."), false);
displayVisualHelp(CALIBRATION_HELPER_IMAGE_EMPTY);

4
ground/openpilotgcs/src/plugins/config/calibration/gyrobiascalibrationmodel.h
View File

@ -61,7 +61,11 @@ private:
QList<double> gyro_accum_x;
QList<double> gyro_accum_y;
QList<double> gyro_accum_z;
QList<double> gyro_state_accum_x;
QList<double> gyro_state_accum_y;
QList<double> gyro_state_accum_z;
UAVObject::Metadata initialGyroStateMdata;
UAVObject::Metadata initialGyroSensorMdata;
UAVObjectManager *getObjectManager();
};
}

49
ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp
View File

@ -47,36 +47,35 @@ SixPointCalibrationModel::SixPointCalibrationModel(QObject *parent) :
calibratingMag(false),
calibratingAccel(false),
collectingData(false)
{
calibrationStepsMag.clear();
calibrationStepsMag
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_NED,
tr("Place horizontally, nose pointing north and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_DWN,
tr("Place with nose down, right side west and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_WDS,
tr("Place right side down, nose west and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_ENU,
tr("Place upside down, nose east and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_USE,
tr("Place with nose up, left side north and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_SUW,
tr("Place with left side down, nose south and click Save Position button..."));
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_NED,
tr("Place horizontally, nose pointing north and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_DWN,
tr("Place with nose down, right side west and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_WDS,
tr("Place right side down, nose west and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_ENU,
tr("Place upside down, nose east and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_USE,
tr("Place with nose up, left side north and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_SUW,
tr("Place with left side down, nose south and click Save Position button..."));
calibrationStepsAccelOnly.clear();
calibrationStepsAccelOnly << CalibrationStep(CALIBRATION_HELPER_IMAGE_NED,
tr("Place horizontally and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_DWN,
tr("Place with nose down and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_WDS,
tr("Place right side down and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_ENU,
tr("Place upside down and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_USE,
tr("Place with nose up and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_SUW,
tr("Place with left side down and click Save Position button..."));
tr("Place horizontally and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_DWN,
tr("Place with nose down and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_WDS,
tr("Place right side down and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_ENU,
tr("Place upside down and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_USE,
tr("Place with nose up and click Save Position button..."))
<< CalibrationStep(CALIBRATION_HELPER_IMAGE_SUW,
tr("Place with left side down and click Save Position button..."));
}
/********** Six point calibration **************/
@ -103,7 +102,7 @@ void SixPointCalibrationModel::start(bool calibrateAccel, bool calibrateMag)
// Store and reset board rotation before calibration starts
storeAndClearBoardRotation();
if(calibrateMag){
if (calibrateMag) {
currentSteps = &calibrationStepsMag;
} else {
currentSteps = &calibrationStepsAccelOnly;

10
ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h
View File

@ -41,15 +41,15 @@ namespace OpenPilot {
class SixPointCalibrationModel : public QObject {
Q_OBJECT
class CalibrationStep{
public:
CalibrationStep(QString newVisualHelp, QString newInstructions){
visualHelp = newVisualHelp;
class CalibrationStep {
public:
CalibrationStep(QString newVisualHelp, QString newInstructions)
{
visualHelp = newVisualHelp;
instructions = newInstructions;
}
QString visualHelp;
QString instructions;
};
typedef struct {