rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

OP-1351 Apply and Save logic for gyro bias calibration now works as expected

Browse Source
This commit is contained in:
Philippe Renon 2014-05-30 15:09:07 +02:00
parent bd6bd043d1
commit 934a11bd96
4 changed files with 147 additions and 129 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -25,52 +25,61 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <attitudestate.h>
#include <attitudesettings.h>
#include "extensionsystem/pluginmanager.h"
#include <gyrostate.h>
#include <gyrosensor.h>
#include <revocalibration.h>
#include <accelgyrosettings.h>
#include "calibration/gyrobiascalibrationmodel.h"
#include "calibration/calibrationutils.h"
#include "calibration/calibrationuiutils.h"
#include "extensionsystem/pluginmanager.h"
static const int LEVEL_SAMPLES = 100;
namespace OpenPilot {
GyroBiasCalibrationModel::GyroBiasCalibrationModel(QObject *parent) :
QObject(parent),
collectingData(false)
{}
QObject(parent), collectingData(false), m_dirty(false)
{
gyroState = GyroState::GetInstance(getObjectManager());
Q_ASSERT(gyroState);
gyroSensor = GyroSensor::GetInstance(getObjectManager());
Q_ASSERT(gyroSensor);
revoCalibration = RevoCalibration::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration);
attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
Q_ASSERT(attitudeSettings);
accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
Q_ASSERT(accelGyroSettings);
}
/******* gyro bias zero ******/
void GyroBiasCalibrationModel::start()
{
// Store and reset board rotation before calibration starts
storeAndClearBoardRotation();
started();
progressChanged(0);
RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration);
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
memento.revoCalibrationData = revoCalibrationData;
revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_FALSE;
revoCalibration->setData(revoCalibrationData);
revoCalibration->updated();
// Disable gyro bias correction while calibrating
AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
Q_ASSERT(attitudeSettings);
AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData();
memento.attitudeSettingsData = attitudeSettingsData;
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 vehicle steady..."));
UAVObject::Metadata gyroStateMetadata = gyroState->getMetadata();
memento.gyroStateMetadata = gyroStateMetadata;
UAVObject::SetFlightTelemetryUpdateMode(gyroStateMetadata, UAVObject::UPDATEMODE_PERIODIC);
gyroStateMetadata.flightTelemetryUpdatePeriod = 100;
gyroState->setMetadata(gyroStateMetadata);
UAVObject::Metadata gyroSensorMetadata = gyroSensor->getMetadata();
memento.gyroSensorMetadata = gyroSensorMetadata;
UAVObject::SetFlightTelemetryUpdateMode(gyroSensorMetadata, UAVObject::UPDATEMODE_PERIODIC);
gyroSensorMetadata.flightTelemetryUpdatePeriod = 100;
gyroSensor->setMetadata(gyroSensorMetadata);
gyro_accum_x.clear();
gyro_accum_y.clear();
@ -80,24 +89,13 @@ void GyroBiasCalibrationModel::start()
gyro_state_accum_y.clear();
gyro_state_accum_z.clear();
UAVObject::Metadata metadata;
// reset dirty state to forget previous unsaved runs
m_dirty = false;
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
Q_ASSERT(gyroState);
initialGyroStateMdata = gyroState->getMetadata();
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);
started();
progressChanged(0);
displayVisualHelp(CALIBRATION_HELPER_PLANE_PREFIX + CALIBRATION_HELPER_IMAGE_NED);
displayInstructions(tr("Calibrating the gyroscopes. Keep the vehicle steady..."));
// Now connect to the accels and mag updates, gather for 100 samples
collectingData = true;
@ -112,15 +110,10 @@ void GyroBiasCalibrationModel::getSample(UAVObject *obj)
{
QMutexLocker lock(&sensorsUpdateLock);
Q_UNUSED(lock);
switch (obj->getObjID()) {
case GyroState::OBJID:
{
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
Q_ASSERT(gyroState);
GyroState::DataFields gyroStateData = gyroState->getData();
gyro_state_accum_x.append(gyroStateData.x);
gyro_state_accum_y.append(gyroStateData.y);
gyro_state_accum_z.append(gyroStateData.z);
@ -128,10 +121,7 @@ void GyroBiasCalibrationModel::getSample(UAVObject *obj)
}
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);
@ -149,61 +139,48 @@ void GyroBiasCalibrationModel::getSample(UAVObject *obj)
if ((gyro_accum_y.size() >= LEVEL_SAMPLES || (gyro_accum_y.size() == 0 && gyro_state_accum_y.size() >= LEVEL_SAMPLES)) &&
collectingData == true) {
collectingData = false;
m_dirty = true;
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 *)));
disconnect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
stopped();
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 biases based on collected data
// 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();
accelGyroSettings->setData(accelGyroSettingsData);
accelGyroSettings->updated();
AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
Q_ASSERT(attitudeSettings);
AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData();
attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE;
attitudeSettings->setData(attitudeSettingsData);
attitudeSettings->updated();
gyroState->setMetadata(initialGyroStateMdata);
gyroSensor->setMetadata(initialGyroSensorMdata);
displayInstructions(tr("Gyroscope calibration completed succesfully."), WizardModel::Success);
displayVisualHelp(CALIBRATION_HELPER_IMAGE_EMPTY);
gyroState->setMetadata(memento.gyroStateMetadata);
gyroSensor->setMetadata(memento.gyroSensorMetadata);
revoCalibration->setData(memento.revoCalibrationData);
attitudeSettings->setData(memento.attitudeSettingsData);
// Recall saved board rotation
recallBoardRotation();
stopped();
displayInstructions(tr("Gyroscope calibration completed succesfully."), WizardModel::Success);
displayVisualHelp(CALIBRATION_HELPER_IMAGE_EMPTY);
}
}
void GyroBiasCalibrationModel::save()
{
if (!m_dirty) {
return;
}
// Update biases based on collected data
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
// 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);
}
accelGyroSettings->setData(accelGyroSettingsData);
m_dirty = false;
}
UAVObjectManager *GyroBiasCalibrationModel::getObjectManager()
{

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

@ -34,6 +34,12 @@
#include "uavobjectmanager.h"
#include "uavobject.h"
#include <gyrostate.h>
#include <gyrosensor.h>
#include <attitudesettings.h>
#include <revocalibration.h>
#include <accelgyrosettings.h>
#include <QObject>
namespace OpenPilot {
@ -43,27 +49,41 @@ class GyroBiasCalibrationModel : public QObject {
public:
explicit GyroBiasCalibrationModel(QObject *parent = 0);
bool dirty()
{
return m_dirty;
}
signals:
void displayVisualHelp(QString elementID);
void displayInstructions(QString text, WizardModel::MessageType type = WizardModel::Info);
void started();
void stopped();
void storeAndClearBoardRotation();
void recallBoardRotation();
void progressChanged(int value);
void displayVisualHelp(QString elementID);
void displayInstructions(QString text, WizardModel::MessageType type = WizardModel::Info);
public slots:
// Slots for gyro bias zero
void start();
void save();
private slots:
void getSample(UAVObject *obj);
private:
typedef struct {
UAVObject::Metadata gyroStateMetadata;
UAVObject::Metadata gyroSensorMetadata;
RevoCalibration::DataFields revoCalibrationData;
AttitudeSettings::DataFields attitudeSettingsData;
} Memento;
QMutex sensorsUpdateLock;
bool collectingData;
bool m_dirty;
Memento memento;
QList<double> gyro_accum_x;
QList<double> gyro_accum_y;
@ -71,8 +91,14 @@ private:
QList<double> gyro_state_accum_x;
QList<double> gyro_state_accum_y;
QList<double> gyro_state_accum_z;
UAVObject::Metadata initialGyroStateMdata;
UAVObject::Metadata initialGyroSensorMdata;
// convenience pointers
GyroState *gyroState;
GyroSensor *gyroSensor;
RevoCalibration *revoCalibration;
AttitudeSettings *attitudeSettings;
AccelGyroSettings *accelGyroSettings;
UAVObjectManager *getObjectManager();
};
}

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

@ -26,6 +26,19 @@
*/
#include "configrevowidget.h"
#include <attitudestate.h>
#include <attitudesettings.h>
#include <revocalibration.h>
#include <accelgyrosettings.h>
#include <homelocation.h>
#include <accelstate.h>
#include <magstate.h>
#include "assertions.h"
#include "calibration.h"
#include "calibration/calibrationutils.h"
#include "math.h"
#include <QDebug>
#include <QTimer>
@ -40,26 +53,12 @@
#include <iostream>
#include <QDesktopServices>
#include <QUrl>
#include <attitudestate.h>
#include <attitudesettings.h>
#include <revocalibration.h>
#include <accelgyrosettings.h>
#include <homelocation.h>
#include <accelstate.h>
#include <magstate.h>
#include "assertions.h"
#include "calibration.h"
#include "calibration/calibrationutils.h"
#define sign(x) ((x < 0) ? -1 : 1)
// Uncomment this to enable 6 point calibration on the accels
#define NOISE_SAMPLES 50
// *****************
class Thread : public QThread {
public:
static void usleep(unsigned long usecs)
@ -68,14 +67,15 @@ public:
}
};
// *****************
ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
ConfigTaskWidget(parent),
m_ui(new Ui_RevoSensorsWidget()),
isBoardRotationStored(false)
{
m_ui->setupUi(this);
m_ui->tabWidget->setCurrentIndex(0);
addApplySaveButtons(m_ui->revoCalSettingsSaveRAM, m_ui->revoCalSettingsSaveSD);
// Initialization of the visual help
m_ui->calibrationVisualHelp->setScene(new QGraphicsScene(this));
@ -86,11 +86,11 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
// Must set up the UI (above) before setting up the UAVO mappings or refreshWidgetValues
// will be dealing with some null pointers
addUAVObject("RevoCalibration");
// addUAVObject("RevoCalibration");
addUAVObject("HomeLocation");
addUAVObject("AttitudeSettings");
addUAVObject("RevoSettings");
addUAVObject("AccelGyroSettings");
// addUAVObject("AttitudeSettings");
// addUAVObject("RevoSettings");
// addUAVObject("AccelGyroSettings");
autoLoadWidgets();
// thermal calibration
@ -114,7 +114,6 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
connect(m_thermalCalibrationModel, SIGNAL(temperatureChanged(float)), this, SLOT(displayTemperature(float)));
connect(m_thermalCalibrationModel, SIGNAL(temperatureGradientChanged(float)), this, SLOT(displayTemperatureGradient(float)));
connect(m_thermalCalibrationModel, SIGNAL(progressChanged(int)), m_ui->thermalBiasProgress, SLOT(setValue(int)));
// note: init for m_thermalCalibrationModel is done in showEvent to prevent cases with "Start" button not enabled due to some timing issue.
// six point calibration
m_sixPointCalibrationModel = new OpenPilot::SixPointCalibrationModel(this);
@ -169,8 +168,9 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
populateWidgets();
refreshWidgetsValues();
m_ui->tabWidget->setCurrentIndex(0);
enableAllCalibrations();
forceConnectedState();
}
ConfigRevoWidget::~ConfigRevoWidget()
@ -246,17 +246,21 @@ void ConfigRevoWidget::displayVisualHelp(QString elementID)
void ConfigRevoWidget::clearInstructions()
{
m_ui->calibrationInstructions->clear();
// addInstructions(tr("Press any Start button to start a calibration step."), WizardModel::Prompt);
}
void ConfigRevoWidget::addInstructions(QString text, WizardModel::MessageType type)
{
if (!text.isNull()) {
switch (type) {
case WizardModel::Failure:
text = QString("<font color='red'>%1</font>").arg(text);
break;
case WizardModel::Prompt:
text = QString("<font color='blue'>%1</font>").arg(text);
text = QString("<b><font color='blue'>%1</font>").arg(text);
break;
case WizardModel::Success:
text = QString("<b><font color='green'>%1</font>").arg(text);
break;
case WizardModel::Failure:
text = QString("<b><font color='red'>%1</font>").arg(text);
break;
default:
break;
@ -275,8 +279,6 @@ void ConfigRevoWidget::displayTemperatureGradient(float tempGradient)
m_ui->temperatureGradientLabel->setText(tr("Temperature rise %1 °C/min").arg(tempGradient, 5, 'f', 2));
}
/********** UI Functions *************/
/**
* Called by the ConfigTaskWidget parent when RevoCalibration is updated
* to update the UI
@ -285,9 +287,6 @@ void ConfigRevoWidget::refreshWidgetsValues(UAVObject *object)
{
ConfigTaskWidget::refreshWidgetsValues(object);
m_ui->calibInstructions->setText(QString("Press \"Start\" above to calibrate."));
m_ui->isSetCheckBox->setEnabled(false);
HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
@ -298,6 +297,15 @@ void ConfigRevoWidget::refreshWidgetsValues(UAVObject *object)
m_ui->beBox->setText(beStr);
}
void ConfigRevoWidget::updateObjectsFromWidgets()
{
ConfigTaskWidget::updateObjectsFromWidgets();
if (m_gyroBiasCalibrationModel->dirty()) {
m_gyroBiasCalibrationModel->save();
}
}
void ConfigRevoWidget::clearHomeLocation()
{
HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
@ -318,6 +326,7 @@ void ConfigRevoWidget::clearHomeLocation()
void ConfigRevoWidget::disableAllCalibrations()
{
clearInstructions();
m_ui->sixPointsStartAccel->setEnabled(false);
m_ui->sixPointsStartMag->setEnabled(false);
m_ui->boardLevelStart->setEnabled(false);
@ -327,6 +336,11 @@ void ConfigRevoWidget::disableAllCalibrations()
void ConfigRevoWidget::enableAllCalibrations()
{
// TODO should use a signal from m_gyroBiasCalibrationModel instead
if (m_gyroBiasCalibrationModel->dirty()) {
widgetsContentsChanged();
}
m_ui->sixPointsStartAccel->setEnabled(true);
m_ui->sixPointsStartMag->setEnabled(true);
m_ui->boardLevelStart->setEnabled(true);

1
ground/openpilotgcs/src/plugins/config/configrevowidget.h
View File

@ -75,6 +75,7 @@ private slots:
// ! Overriden method from the configTaskWidget to update UI
virtual void refreshWidgetsValues(UAVObject *object = NULL);
virtual void updateObjectsFromWidgets();
// Slot for clearing home location
void clearHomeLocation();