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

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

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

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

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