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

OP-975 Move board level calibration to a reusable class

Browse Source
This commit is contained in:
Alessio Morale 2014-04-11 01:45:36 +02:00
parent 7708cdd15d
commit 2249510c52
5 changed files with 272 additions and 151 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

176
ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.cpp Normal file
View File

@ -0,0 +1,176 @@
/**
******************************************************************************
*
* @file levelcalibrationmodel.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @addtogroup board level calibration
* @{
* @addtogroup ConfigPlugin Config Plugin
* @{
* @brief Telemetry configuration panel
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "levelcalibrationmodel.h"
#include <attitudestate.h>
#include <attitudesettings.h>
#include "extensionsystem/pluginmanager.h"
static const int LEVEL_SAMPLES = 100;
namespace OpenPilot {
LevelCalibrationModel::LevelCalibrationModel(QObject *parent) :
QObject(parent)
{
}
/******* Level calibration *******/
/**
* Starts an accelerometer bias calibration.
*/
void LevelCalibrationModel::start()
{
// Store and reset board rotation before calibration starts
disableAllCalibrations();
progressChanged(0);
rot_data_pitch = 0;
rot_data_roll = 0;
UAVObject::Metadata mdata;
AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager());
Q_ASSERT(attitudeState);
initialAttitudeStateMdata = attitudeState->getMetadata();
mdata = initialAttitudeStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
attitudeState->setMetadata(mdata);
/* Show instructions and enable controls */
displayInstructions("Place horizontally and click save position...", true);
displayVisualHelp("plane-horizontal");
disableAllCalibrations();
savePositionEnabledChanged(true);
position = 0;
}
void LevelCalibrationModel::savePosition()
{
QMutexLocker lock(&sensorsUpdateLock);
Q_UNUSED(lock);
savePositionEnabledChanged(false);
rot_accum_pitch.clear();
rot_accum_roll.clear();
collectingData = true;
AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager());
Q_ASSERT(attitudeState);
connect(attitudeState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
displayInstructions("Hold...",false);
}
/**
Updates the accel bias raw values
*/
void LevelCalibrationModel::getSample(UAVObject *obj)
{
QMutexLocker lock(&sensorsUpdateLock);
Q_UNUSED(lock);
switch (obj->getObjID()) {
case AttitudeState::OBJID:
{
AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager());
Q_ASSERT(attitudeState);
AttitudeState::DataFields attitudeStateData = attitudeState->getData();
rot_accum_roll.append(attitudeStateData.Roll);
rot_accum_pitch.append(attitudeStateData.Pitch);
break;
}
default:
Q_ASSERT(0);
}
// Work out the progress based on whichever has less
double p1 = (double)rot_accum_roll.size() / (double)LEVEL_SAMPLES;
progressChanged(p1 * 100);
if (rot_accum_roll.size() >= LEVEL_SAMPLES &&
collectingData == true) {
collectingData = false;
AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager());
disconnect(attitudeState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
position++;
switch (position) {
case 1:
rot_data_pitch = OpenPilot::CalibrationUtils::listMean(rot_accum_pitch);
rot_data_roll = OpenPilot::CalibrationUtils::listMean(rot_accum_roll);
displayInstructions("Leave horizontally, rotate 180° along yaw axis and click save position...", true);
displayVisualHelp("plane-horizontal-rotated");
disableAllCalibrations();
savePositionEnabledChanged(true);
break;
case 2:
rot_data_pitch += OpenPilot::CalibrationUtils::listMean(rot_accum_pitch);
rot_data_pitch /= 2;
rot_data_roll += OpenPilot::CalibrationUtils::listMean(rot_accum_roll);
rot_data_roll /= 2;
attitudeState->setMetadata(initialAttitudeStateMdata);
compute();
enableAllCalibrations();
break;
}
}
}
void LevelCalibrationModel::compute()
{
enableAllCalibrations();
AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
Q_ASSERT(attitudeSettings);
AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData();
// Update the biases based on collected data
// "rotate" the board in the opposite direction as the calculated offset
attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] -= rot_data_pitch;
attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] -= rot_data_roll;
attitudeSettings->setData(attitudeSettingsData);
attitudeSettings->updated();
}
UAVObjectManager *LevelCalibrationModel::getObjectManager()
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objMngr = pm->getObject<UAVObjectManager>();
Q_ASSERT(objMngr);
return objMngr;
}
}

75
ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.h Normal file
View File

@ -0,0 +1,75 @@
/**
******************************************************************************
*
* @file levelcalibrationmodel.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @addtogroup board level calibration
* @{
* @addtogroup ConfigPlugin Config Plugin
* @{
* @brief Telemetry configuration panel
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef LEVELCALIBRATIONMODEL_H
#define LEVELCALIBRATIONMODEL_H
#include <QObject>
#include <QMutex>
#include <QList>
#include "calibration/calibrationutils.h"
#include <revocalibration.h>
#include <accelgyrosettings.h>
#include <homelocation.h>
#include <accelstate.h>
#include <magstate.h>
namespace OpenPilot {
class LevelCalibrationModel : public QObject
{
Q_OBJECT
public:
explicit LevelCalibrationModel(QObject *parent = 0);
signals:
void displayVisualHelp(QString elementID);
void displayInstructions(QString instructions, bool replace);
void disableAllCalibrations();
void enableAllCalibrations();
void savePositionEnabledChanged(bool state);
void progressChanged(int value);
public slots:
// Slots for calibrating the mags
void start();
void savePosition();
private slots:
void getSample(UAVObject *obj);
void compute();
private:
QMutex sensorsUpdateLock;
int position;
bool collectingData;
QList<double> rot_accum_roll;
QList<double> rot_accum_pitch;
double rot_data_roll;
double rot_data_pitch;
UAVObject::Metadata initialAttitudeStateMdata;
UAVObjectManager *getObjectManager();
};
}
#endif // LEVELCALIBRATIONMODEL_H

6
ground/openpilotgcs/src/plugins/config/config.pro
View File

@ -52,7 +52,8 @@ HEADERS += configplugin.h \
calibration/thermal/dataacquisitiontransition.h \
calibration/thermal/settingshandlingtransitions.h \
calibration/thermal/compensationcalculationtransition.h \
calibration/sixpointcalibrationmodel.h
calibration/sixpointcalibrationmodel.h \
calibration/levelcalibrationmodel.h
SOURCES += configplugin.cpp \
configgadgetwidget.cpp \
@ -88,7 +89,8 @@ SOURCES += configplugin.cpp \
calibration/thermal/thermalcalibration.cpp \
calibration/thermal/thermalcalibrationhelper.cpp \
calibration/thermal/thermalcalibrationmodel.cpp \
calibration/sixpointcalibrationmodel.cpp
calibration/sixpointcalibrationmodel.cpp \
calibration/levelcalibrationmodel.cpp
FORMS += airframe.ui \
airframe_ccpm.ui \

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

@ -56,8 +56,8 @@
#define sign(x) ((x < 0) ? -1 : 1)
// Uncomment this to enable 6 point calibration on the accels
#define SAMPLE_SIZE 40
const double ConfigRevoWidget::maxVarValue = 0.1;
#define NOISE_SAMPLES 50
// *****************
@ -75,7 +75,6 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
ConfigTaskWidget(parent),
m_ui(new Ui_RevoSensorsWidget()),
collectingData(false),
position(-1),
isBoardRotationStored(false)
{
m_ui->setupUi(this);
@ -128,9 +127,17 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
// gyro zero calibration
connect(m_ui->gyroBiasStart, SIGNAL(clicked()), this, SLOT(gyroCalibrationStart()));
m_levelCalibrationModel = new OpenPilot::LevelCalibrationModel();
// level calibration
connect(m_ui->boardLevelStart, SIGNAL(clicked()), this, SLOT(levelCalibrationStart()));
connect(m_ui->boardLevelSavePos, SIGNAL(clicked()), this, SLOT(levelCalibrationSavePosition()));
connect(m_ui->boardLevelStart, SIGNAL(clicked()), m_levelCalibrationModel, SLOT(start()));
connect(m_ui->boardLevelSavePos, SIGNAL(clicked()), m_levelCalibrationModel, SLOT(savePosition()));
connect(m_levelCalibrationModel, SIGNAL(disableAllCalibrations()), this, SLOT(disableAllCalibrations()));
connect(m_levelCalibrationModel, SIGNAL(enableAllCalibrations()), this, SLOT(enableAllCalibrations()));
connect(m_levelCalibrationModel, SIGNAL(displayInstructions(QString, bool)), this, SLOT(displayInstructions(QString, bool)));
connect(m_levelCalibrationModel, SIGNAL(displayVisualHelp(QString)), this, SLOT(displayVisualHelp(QString)));
connect(m_levelCalibrationModel, SIGNAL(savePositionEnabledChanged(bool)), this->m_ui->boardLevelSavePos, SLOT(setEnabled(bool)));
connect(m_levelCalibrationModel, SIGNAL(progressChanged(int)), this->m_ui->boardLevelProgress, SLOT(setValue(int)));
connect(m_ui->hlClearButton, SIGNAL(clicked()), this, SLOT(clearHomeLocation()));
@ -242,9 +249,9 @@ void ConfigRevoWidget::gyroBiasCalibrationGetSample(UAVObject *obj)
// Work out the progress based on whichever has less
double p1 = (double)gyro_accum_x.size() / (double)NOISE_SAMPLES;
double p2 = (double)gyro_accum_y.size() / (double)NOISE_SAMPLES;
m_ui->gyroBiasProgress->setValue(((p1 < p2) ? p1 : p2) * 50);
m_ui->gyroBiasProgress->setValue(((p1 < p2) ? p1 : p2) * 100);
if (gyro_accum_y.size() >= 2 * NOISE_SAMPLES &&
if (gyro_accum_y.size() >= NOISE_SAMPLES &&
collectingData == true) {
collectingData = false;
@ -289,134 +296,6 @@ void ConfigRevoWidget::gyroBiasCalibrationGetSample(UAVObject *obj)
}
/******* Level calibration *******/
/**
* Starts an accelerometer bias calibration.
*/
void ConfigRevoWidget::levelCalibrationStart()
{
// Store and reset board rotation before calibration starts
disableAllCalibrations();
m_ui->boardLevelProgress->setValue(0);
rot_data_pitch = 0;
rot_data_roll = 0;
UAVObject::Metadata mdata;
AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager());
Q_ASSERT(attitudeState);
initialAttitudeStateMdata = attitudeState->getMetadata();
mdata = initialAttitudeStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
attitudeState->setMetadata(mdata);
/* Show instructions and enable controls */
displayInstructions("Place horizontally and click save position...", true);
displayVisualHelp("plane-horizontal");
disableAllCalibrations();
m_ui->boardLevelSavePos->setEnabled(true);
position = 0;
}
void ConfigRevoWidget::levelCalibrationSavePosition()
{
QMutexLocker lock(&sensorsUpdateLock);
Q_UNUSED(lock);
m_ui->boardLevelSavePos->setEnabled(false);
rot_accum_pitch.clear();
rot_accum_roll.clear();
collectingData = true;
AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager());
Q_ASSERT(attitudeState);
connect(attitudeState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(levelCalibrationGetSample(UAVObject *)));
displayInstructions("Hold...");
}
/**
Updates the accel bias raw values
*/
void ConfigRevoWidget::levelCalibrationGetSample(UAVObject *obj)
{
QMutexLocker lock(&sensorsUpdateLock);
Q_UNUSED(lock);
switch (obj->getObjID()) {
case AttitudeState::OBJID:
{
AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager());
Q_ASSERT(attitudeState);
AttitudeState::DataFields attitudeStateData = attitudeState->getData();
rot_accum_roll.append(attitudeStateData.Roll);
rot_accum_pitch.append(attitudeStateData.Pitch);
break;
}
default:
Q_ASSERT(0);
}
// Work out the progress based on whichever has less
double p1 = (double)rot_accum_roll.size() / (double)NOISE_SAMPLES;
m_ui->boardLevelProgress->setValue(p1 * 100);
if (rot_accum_roll.size() >= NOISE_SAMPLES &&
collectingData == true) {
collectingData = false;
AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager());
disconnect(attitudeState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(levelCalibrationGetSample(UAVObject *)));
position++;
switch (position) {
case 1:
rot_data_pitch = OpenPilot::CalibrationUtils::listMean(rot_accum_pitch);
rot_data_roll = OpenPilot::CalibrationUtils::listMean(rot_accum_roll);
displayInstructions("Leave horizontally, rotate 180° along yaw axis and click save position...", true);
displayVisualHelp("plane-horizontal-rotated");
disableAllCalibrations();
m_ui->boardLevelSavePos->setEnabled(true);
break;
case 2:
rot_data_pitch += OpenPilot::CalibrationUtils::listMean(rot_accum_pitch);
rot_data_pitch /= 2;
rot_data_roll += OpenPilot::CalibrationUtils::listMean(rot_accum_roll);
rot_data_roll /= 2;
attitudeState->setMetadata(initialAttitudeStateMdata);
levelCalibrationCompute();
enableAllCalibrations();
break;
}
}
}
void ConfigRevoWidget::levelCalibrationCompute()
{
enableAllCalibrations();
AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
Q_ASSERT(attitudeSettings);
AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData();
// Update the biases based on collected data
// "rotate" the board in the opposite direction as the calculated offset
attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] -= rot_data_pitch;
attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] -= rot_data_roll;
attitudeSettings->setData(attitudeSettingsData);
attitudeSettings->updated();
}
void ConfigRevoWidget::storeAndClearBoardRotation()
{
if (!isBoardRotationStored) {

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

@ -41,7 +41,7 @@
#include <QMutex>
#include "calibration/thermal/thermalcalibrationmodel.h"
#include "calibration/sixpointcalibrationmodel.h"
#include "calibration/levelcalibrationmodel.h"
class Ui_Widget;
@ -55,11 +55,12 @@ public:
private:
OpenPilot::SixPointCalibrationModel *m_sixPointCalibrationModel;
OpenPilot::ThermalCalibrationModel *m_thermalCalibrationModel;
OpenPilot::LevelCalibrationModel *m_levelCalibrationModel;
Ui_RevoSensorsWidget *m_ui;
QMutex sensorsUpdateLock;
int phaseCounter;
const static double maxVarValue;
const static int calibrationDelay = 10;
bool collectingData;
@ -68,20 +69,8 @@ private:
QList<double> gyro_accum_y;
QList<double> gyro_accum_z;
QList<double> rot_accum_roll;
QList<double> rot_accum_pitch;
double rot_data_roll;
double rot_data_pitch;
UAVObject::Metadata initialAttitudeStateMdata;
UAVObject::Metadata initialGyroStateMdata;
int position;
static const int NOISE_SAMPLES = 50;
// Board rotation store/recall
qint16 storedBoardRotation[3];