rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

OP-39 More Motor and servo calibration code.

Browse Source
This commit is contained in:
Fredrik Arvidsson 2012-09-05 00:58:53 +02:00
parent 51dc691ab8
commit 0871cd3db2
7 changed files with 162 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.cpp
View File

@ -30,42 +30,59 @@
#include "extensionsystem/pluginmanager.h"
#include "vehicleconfigurationhelper.h"
const quint16 OutputCalibrationUtil::UPDATE_CHANNEL_MAPPING[10] = {1, 1, 1, 2, 3, 4, 3, 3, 4, 4};
const quint16 OutputCalibrationUtil::UPDATE_CHANNEL_MAPPING[10] = {0, 0, 0, 1, 2, 3, 2, 2, 3, 3};
OutputCalibrationUtil::OutputCalibrationUtil(QObject *parent) :
QObject(parent), m_outputChannel(0)
QObject(parent), m_outputChannel(-1), m_safeValue(1000)
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
m_uavObjectManager = pm->getObject<UAVObjectManager>();
Q_ASSERT(m_uavObjectManager);
}
void OutputCalibrationUtil::startChannelOutput(quint16 escUpdateRate, quint16 channel)
OutputCalibrationUtil::~OutputCalibrationUtil()
{
if(m_outputChannel == 0 && channel > 0 && channel <= ActuatorCommand::CHANNEL_NUMELEM)
stopChannelOutput();
}
void OutputCalibrationUtil::setupOutputRates(const QList<quint16> &outputRates)
{
//Set actuator settings for channels
ActuatorSettings *actuatorSettings = ActuatorSettings::GetInstance(m_uavObjectManager);
Q_ASSERT(actuatorSettings);
ActuatorSettings::DataFields data = actuatorSettings->getData();
for(int i = 0; i < outputRates.size(); i++) {
data.ChannelType[i] = ActuatorSettings::CHANNELTYPE_PWM;
data.ChannelAddr[i] = i;
data.ChannelMin[i] = 1000;
data.ChannelNeutral[i] = 1000;
data.ChannelMax[i] = 2000;
data.ChannelUpdateFreq[UPDATE_CHANNEL_MAPPING[i]] = outputRates[i];
}
actuatorSettings->setData(data);
actuatorSettings->updated();
}
void OutputCalibrationUtil::startChannelOutput(quint16 channel, quint16 safeValue)
{
if(m_outputChannel < 0 && channel >= 0 && channel < ActuatorCommand::CHANNEL_NUMELEM)
{
//Set actuator settings for channel
ActuatorSettings *actuatorSettings = ActuatorSettings::GetInstance(m_uavObjectManager);
Q_ASSERT(actuatorSettings);
ActuatorSettings::DataFields data = actuatorSettings->getData();
m_savedActuatorSettingData = data;
//Start output...
m_outputChannel = channel;
m_safeValue = safeValue;
quint16 actuatorChannel = channel - 1;
data.ChannelType[actuatorChannel] = ActuatorSettings::CHANNELTYPE_PWM;
data.ChannelAddr[actuatorChannel] = actuatorChannel;
data.ChannelMin[actuatorChannel] = 1000;
data.ChannelNeutral[actuatorChannel] = 1000;
data.ChannelMax[actuatorChannel] = 2000;
data.ChannelUpdateFreq[UPDATE_CHANNEL_MAPPING[actuatorChannel]] = escUpdateRate;
actuatorSettings->setData(data);
qDebug() << "Starting output for channel " << m_outputChannel << "...";
ActuatorCommand *actuatorCommand = ActuatorCommand::GetInstance(m_uavObjectManager);
Q_ASSERT(actuatorCommand);
UAVObject::Metadata metaData = actuatorCommand->getMetadata();
m_savedActuatorCommandMetadata = metaData;
//Store current data for later restore
m_savedActuatorCommandData = actuatorCommand->getData();
//Enable actuator control from GCS...
//Store current metadata for later restore
UAVObject::SetFlightAccess(metaData, UAVObject::ACCESS_READONLY);
@ -73,43 +90,46 @@ void OutputCalibrationUtil::startChannelOutput(quint16 escUpdateRate, quint16 ch
UAVObject::SetGcsTelemetryAcked(metaData, false);
UAVObject::SetGcsTelemetryUpdateMode(metaData, UAVObject::UPDATEMODE_ONCHANGE);
metaData.gcsTelemetryUpdatePeriod = 100;
//Apply changes
actuatorCommand->setMetadata(metaData);
actuatorCommand->updated();
//Start output...
m_outputChannel = channel;
qDebug() << "Output for channel " << m_outputChannel << " started.";
}
}
void OutputCalibrationUtil::stopChannelOutput()
{
if(m_outputChannel > 0)
if(m_outputChannel >= 0)
{
qDebug() << "Stopping output for channel " << m_outputChannel << "...";
//Stop output...
setChannelOutputValue(m_safeValue);
// Restore metadata to what it was before
ActuatorCommand *actuatorCommand = ActuatorCommand::GetInstance(m_uavObjectManager);
Q_ASSERT(actuatorCommand);
actuatorCommand->setData(m_savedActuatorCommandData);
actuatorCommand->setMetadata(m_savedActuatorCommandMetadata);
actuatorCommand->updated();
ActuatorSettings *actuatorSettings = ActuatorSettings::GetInstance(m_uavObjectManager);
Q_ASSERT(actuatorSettings);
actuatorSettings->setData(m_savedActuatorSettingData);
actuatorSettings->updated();
qDebug() << "Output for channel " << m_outputChannel << " stopped.";
m_outputChannel = 0;
m_outputChannel = -1;
}
}
void OutputCalibrationUtil::setChannelOutputValue(quint16 value)
{
if(m_outputChannel > 0)
if(m_outputChannel >= 0)
{
//Set output value
qDebug() << "Setting output value for channel " << m_outputChannel << " to " << value << ".";
ActuatorCommand *actuatorCommand = ActuatorCommand::GetInstance(m_uavObjectManager);
Q_ASSERT(actuatorCommand);
ActuatorCommand::DataFields data = actuatorCommand->getData();
data.Channel[m_outputChannel - 1] = value;
data.Channel[m_outputChannel] = value;
actuatorCommand->setData(data);
}
}

12
ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.h
View File

@ -29,9 +29,11 @@
#define OUTPUTCALIBRATIONUTIL_H
#include <QObject>
#include <QList>
#include "uavobject.h"
#include "uavobjectmanager.h"
#include "vehicleconfigurationsource.h"
#include "actuatorcommand.h"
class OutputCalibrationUtil : public QObject
@ -39,23 +41,25 @@ class OutputCalibrationUtil : public QObject
Q_OBJECT
public:
explicit OutputCalibrationUtil(QObject *parent = 0);
~OutputCalibrationUtil();
signals:
public slots:
void startChannelOutput(quint16 escUpdateRate, quint16 channel);
void setupOutputRates(const QList<quint16> &outputRates);
void startChannelOutput(quint16 channel, quint16 safeValue);
void stopChannelOutput();
void setChannelOutputValue(quint16 value);
private:
static const quint16 UPDATE_CHANNEL_MAPPING[10];
VehicleConfigurationSource *m_configSource;
actuatorSettings *m_actuatorSettings;
quint16 m_outputChannel;
qint16 m_outputChannel;
quint16 m_safeValue;
UAVObject::Metadata m_savedActuatorCommandMetadata;
ActuatorSettings::DataFields m_savedActuatorSettingData;
ActuatorCommand::DataFields m_savedActuatorCommandData;
UAVObjectManager *m_uavObjectManager;
};

5
ground/openpilotgcs/src/plugins/setupwizard/pages/levellingpage.cpp
View File

@ -70,7 +70,10 @@ void LevellingPage::performLevelling()
return;
}
getWizard()->button(QWizard::BackButton)->setEnabled(false);
getWizard()->button(QWizard::NextButton)->setEnabled(false);
getWizard()->button(QWizard::CancelButton)->setEnabled(false);
ui->levelButton->setEnabled(false);
if(!m_levellingUtil)
@ -121,6 +124,8 @@ void LevellingPage::stopLevelling()
disconnect(m_levellingUtil, SIGNAL(progress(long,long)), this, SLOT(levellingProgress(long,long)));
disconnect(m_levellingUtil, SIGNAL(done(accelGyroBias)), this, SLOT(levellingDone(accelGyroBias)));
disconnect(m_levellingUtil, SIGNAL(timeout(QString)), this, SLOT(levellingTimeout(QString)));
getWizard()->button(QWizard::BackButton)->setEnabled(true);
getWizard()->button(QWizard::NextButton)->setEnabled(true);
getWizard()->button(QWizard::CancelButton)->setEnabled(true);
ui->levelButton->setEnabled(true);
}

117
ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp
View File

@ -34,8 +34,6 @@ OutputCalibrationPage::OutputCalibrationPage(SetupWizard *wizard, QWidget *paren
{
ui->setupUi(this);
m_calibrationUtil = new OutputCalibrationUtil();
m_vehicleRenderer = new QSvgRenderer();
if (QFile::exists(QString(":/setupwizard/resources/multirotor-shapes.svg")) &&
m_vehicleRenderer->load(QString(":/setupwizard/resources/multirotor-shapes.svg")) &&
@ -50,6 +48,7 @@ OutputCalibrationPage::~OutputCalibrationPage()
{
if(m_calibrationUtil) {
delete m_calibrationUtil;
m_calibrationUtil = 0;
}
delete ui;
}
@ -61,53 +60,60 @@ void OutputCalibrationPage::setupVehicle()
m_vehicleHighlightElementIndexes.clear();
m_currentWizardIndex = 0;
m_vehicleScene->clear();
quint16 escUpdateRate = getEscUpdateRate();
quint16 servoUpdateRate = getServoUpdateRate();
switch(getWizard()->getVehicleSubType())
{
case SetupWizard::MULTI_ROTOR_TRI_Y:
m_wizardIndexes << 0 << 1 << 2 << 1 << 2 << 1 << 2 << 3 << 4;
m_vehicleElementIds << "tri" << "tri-frame" << "tri-m1" << "tri-m2" << "tri-m3" << "tri-s1";
m_vehicleHighlightElementIndexes << 0 << 1 << 1 << 2 << 2 << 3 << 3 << 4 << 4;
m_channelUpdateRates << getEscUpdateRate() << getEscUpdateRate() << getEscUpdateRate() << getServoUpdateRate();
m_channelUpdateRates << escUpdateRate << escUpdateRate << escUpdateRate << servoUpdateRate;
break;
case SetupWizard::MULTI_ROTOR_QUAD_X:
m_wizardIndexes << 0 << 1 << 2 << 1 << 2 << 1 << 2 << 1 << 2;
m_vehicleElementIds << "quad-x" << "quad-x-frame" << "quad-x-m1" << "quad-x-m2" << "quad-x-m3" << "quad-x-m4";
m_vehicleHighlightElementIndexes << 0 << 1 << 1 << 2 << 2 << 3 << 3 << 4 << 4;
m_channelUpdateRates << getEscUpdateRate() << getEscUpdateRate() << getEscUpdateRate() << getEscUpdateRate();
m_channelUpdateRates << escUpdateRate << escUpdateRate << escUpdateRate << escUpdateRate;
break;
case SetupWizard::MULTI_ROTOR_QUAD_PLUS:
m_wizardIndexes << 0 << 1 << 2 << 1 << 2 << 1 << 2 << 1 << 2;
m_vehicleElementIds << "quad-p" << "quad-p-frame" << "quad-p-m1" << "quad-p-m2" << "quad-p-m3" << "quad-p-m4";
m_vehicleHighlightElementIndexes << 0 << 1 << 1 << 2 << 2 << 3 << 3 << 4 << 4;
m_channelUpdateRates << getEscUpdateRate() << getEscUpdateRate() << getEscUpdateRate() << getEscUpdateRate();
m_channelUpdateRates << escUpdateRate << escUpdateRate << escUpdateRate << escUpdateRate;
break;
case SetupWizard::MULTI_ROTOR_HEXA:
m_wizardIndexes << 0 << 1 << 2 << 1 << 2 << 1 << 2 << 1 << 2 << 1 << 2 << 1 << 2;
m_vehicleElementIds << "hexa" << "hexa-frame" << "hexa-m1" << "hexa-m2" << "hexa-m3" << "hexa-m4"
<< "hexa-m5" << "hexa-m6";
m_vehicleHighlightElementIndexes << 0 << 1 << 1 << 2 << 2 << 3 << 3 << 4 << 4 << 5 << 5 << 6 << 6;
m_channelUpdateRates << getEscUpdateRate() << getEscUpdateRate() << getEscUpdateRate() << getEscUpdateRate()
<< getEscUpdateRate() << getEscUpdateRate();
m_channelUpdateRates << escUpdateRate << escUpdateRate << escUpdateRate << escUpdateRate << escUpdateRate << escUpdateRate;
break;
case SetupWizard::MULTI_ROTOR_HEXA_COAX_Y:
m_wizardIndexes << 0 << 1 << 2 << 1 << 2 << 1 << 2 << 1 << 2 << 1 << 2 << 1 << 2;
m_vehicleElementIds << "hexa-y6" << "hexa-y6-frame" << "hexa-y6-m1" << "hexa-y6-m2" << "hexa-y6-m3" << "hexa-y6-m4"
<< "hexa-y6-m5" << "hexa-y6-m6";
m_vehicleHighlightElementIndexes << 0 << 1 << 1 << 2 << 2 << 3 << 3 << 4 << 4 << 5 << 5 << 6 << 6;
m_channelUpdateRates << getEscUpdateRate() << getEscUpdateRate() << getEscUpdateRate() << getEscUpdateRate()
<< getEscUpdateRate() << getEscUpdateRate();
m_channelUpdateRates << escUpdateRate << escUpdateRate << escUpdateRate << escUpdateRate << escUpdateRate << escUpdateRate;
break;
case SetupWizard::MULTI_ROTOR_HEXA_H:
m_wizardIndexes << 0 << 1 << 2 << 1 << 2 << 1 << 2 << 1 << 2 << 1 << 2 << 1 << 2;
m_vehicleElementIds << "hexa-h" << "hexa-h-frame" << "hexa-h-m1" << "hexa-h-m2" << "hexa-h-m3" << "hexa-h-m4"
<< "hexa-h-m5" << "hexa-h-m6";
m_vehicleHighlightElementIndexes << 0 << 1 << 1 << 2 << 2 << 3 << 3 << 4 << 4 << 5 << 5 << 6 << 6;
m_channelUpdateRates << getEscUpdateRate() << getEscUpdateRate() << getEscUpdateRate() << getEscUpdateRate()
<< getEscUpdateRate() << getEscUpdateRate();
m_channelUpdateRates << escUpdateRate << escUpdateRate << escUpdateRate << escUpdateRate << escUpdateRate << escUpdateRate;
break;
default:
break;
}
if(m_calibrationUtil) {
delete m_calibrationUtil;
m_calibrationUtil = 0;
}
m_calibrationUtil = new OutputCalibrationUtil();
m_calibrationUtil->setupOutputRates(m_channelUpdateRates);
setupVehicleItems();
}
@ -159,8 +165,25 @@ void OutputCalibrationPage::setWizardPage()
{
m_calibrationUtil->stopChannelOutput();
ui->backPageButton->setEnabled(m_currentWizardIndex > 0);
ui->nextPageButton->setEnabled(m_currentWizardIndex < (m_wizardIndexes.size() - 1));
ui->calibrationStack->setCurrentIndex(m_wizardIndexes.at(m_currentWizardIndex));
ui->nextPageButton->setEnabled(m_currentWizardIndex < m_wizardIndexes.size() - 1);
int currentPageIndex = m_wizardIndexes.at(m_currentWizardIndex);
ui->calibrationStack->setCurrentIndex(currentPageIndex);
int currentChannel = getCurrentChannel();
if(currentChannel >= 0) {
if(currentPageIndex == 1) {
ui->motorNeutralSlider->setValue(m_actuatorSettings.channels[currentChannel].channelNeutral);
}
else if (currentPageIndex == 2) {
ui->motorMaxSlider->setValue(m_actuatorSettings.channels[currentChannel].channelMax);
}
else if(currentPageIndex == 3) {
ui->servoCenterSlider->setValue(m_actuatorSettings.channels[currentChannel].channelNeutral);
}
else if(currentPageIndex == 4) {
ui->servoMinAngleSlider->setValue(m_actuatorSettings.channels[currentChannel].channelMin);
ui->servoMaxAngleSlider->setValue(m_actuatorSettings.channels[currentChannel].channelMax);
}
}
setupVehicleHighlightedPart();
}
@ -183,54 +206,85 @@ void OutputCalibrationPage::showEvent(QShowEvent *event)
void OutputCalibrationPage::on_nextPageButton_clicked()
{
if(m_currentWizardIndex < m_wizardIndexes.size()) {
m_currentWizardIndex++;
setWizardPage();
}
m_currentWizardIndex++;
setWizardPage();
}
void OutputCalibrationPage::on_backPageButton_clicked()
{
if(m_currentWizardIndex > 0) {
m_currentWizardIndex--;
setWizardPage();
}
m_currentWizardIndex--;
setWizardPage();
}
quint16 OutputCalibrationPage::getCurrentChannel()
{
return m_vehicleHighlightElementIndexes[m_currentWizardIndex] - 1;
}
void OutputCalibrationPage::enableButtons(bool enable)
{
ui->nextPageButton->setEnabled(enable);
ui->backPageButton->setEnabled(enable);
getWizard()->button(QWizard::NextButton)->setEnabled(enable);
getWizard()->button(QWizard::CancelButton)->setEnabled(enable);
getWizard()->button(QWizard::BackButton)->setEnabled(enable);
}
void OutputCalibrationPage::on_motorNeutralButton_toggled(bool checked)
{
ui->motorNeutralButton->setText(checked ? tr("Stop") : tr("Start"));
quint16 channel = getCurrentChannel();
onStartButtonToggle(checked, channel, m_actuatorSettings.channels[channel].channelNeutral, 1000,ui->motorNeutralSlider);
}
void OutputCalibrationPage::onStartButtonToggle(bool checked, quint16 channel, quint16 &value, quint16 safeValue, QSlider *slider) {
if(checked) {
quint16 channel = m_vehicleHighlightElementIndexes[m_currentWizardIndex];
m_calibrationUtil->startChannelOutput(m_channelUpdateRates[channel], channel);
m_calibrationUtil->setChannelOutputValue(ui->motorNeutralSlider->value());
enableButtons(false);
m_calibrationUtil->startChannelOutput(channel, safeValue);
slider->setValue(value);
m_calibrationUtil->setChannelOutputValue(slider->value());
}
else {
m_calibrationUtil->setChannelOutputValue(ui->motorNeutralSlider->minimum());
value = slider->value();
m_calibrationUtil->stopChannelOutput();
enableButtons(true);
}
}
void OutputCalibrationPage::on_motorNeutralSlider_valueChanged(int value)
{
if(ui->motorNeutralButton->isChecked())
{
if(ui->motorNeutralButton->isChecked()) {
m_calibrationUtil->setChannelOutputValue(ui->motorNeutralSlider->value());
}
}
void OutputCalibrationPage::on_motorMaxButton_toggled(bool checked)
{
ui->motorNeutralButton->setText(checked ? tr("Stop") : tr("Start"));
quint16 channel = getCurrentChannel();
onStartButtonToggle(checked, channel, m_actuatorSettings.channels[channel].channelMax, 1000, ui->motorMaxSlider);
}
void OutputCalibrationPage::on_motorMaxSlider_valueChanged(int position)
{
if(ui->motorMaxButton->isChecked()) {
m_calibrationUtil->setChannelOutputValue(ui->motorMaxSlider->value());
}
}
void OutputCalibrationPage::on_servoCenterButton_toggled(bool checked)
{
ui->motorNeutralButton->setText(checked ? tr("Stop") : tr("Start"));
quint16 channel = getCurrentChannel();
m_actuatorSettings.channels[channel].channelNeutral = 1500;
onStartButtonToggle(checked, channel, m_actuatorSettings.channels[channel].channelNeutral, 1500, ui->servoCenterSlider);
}
void OutputCalibrationPage::on_servoCenterSlider_valueChanged(int position)
{
if(ui->servoCenterButton->isChecked()) {
m_calibrationUtil->setChannelOutputValue(ui->servoCenterSlider->value());
}
}
void OutputCalibrationPage::on_servoAngleButton_toggled(bool checked)
@ -247,8 +301,3 @@ void OutputCalibrationPage::on_servoMinAngleSlider_valueChanged(int position)
{
}
void OutputCalibrationPage::on_servoCenterSlider_valueChanged(int position)
{
}

5
ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.h
View File

@ -73,6 +73,9 @@ private:
void setupVehicleItems();
void setupVehicleHighlightedPart();
void setWizardPage();
void enableButtons(bool enable);
void onStartButtonToggle(bool checked, quint16 channel, quint16 &value, quint16 safeValue, QSlider *slider);
quint16 getCurrentChannel();
quint16 getEscUpdateRate(){ return getWizard()->getESCType() == VehicleConfigurationSource::ESC_RAPID ?
VehicleConfigurationHelper::RAPID_ESC_FREQUENCE : VehicleConfigurationHelper::LEGACY_ESC_FREQUENCE; }
@ -92,6 +95,8 @@ private:
QList<quint16> m_wizardIndexes;
QList<quint16> m_channelUpdateRates;
actuatorSettings m_actuatorSettings;
OutputCalibrationUtil *m_calibrationUtil;
};

12
ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.ui
View File

@ -36,7 +36,7 @@
</rect>
</property>
<property name="currentIndex">
<number>1</number>
<number>0</number>
</property>
<widget class="QWidget" name="intro">
<widget class="QLabel" name="label_3">
@ -95,7 +95,7 @@
<number>1000</number>
</property>
<property name="maximum">
<number>1400</number>
<number>1300</number>
</property>
<property name="singleStep">
<number>10</number>
@ -188,7 +188,7 @@
</rect>
</property>
<property name="minimum">
<number>1600</number>
<number>1400</number>
</property>
<property name="maximum">
<number>2000</number>
@ -267,7 +267,7 @@
<number>1500</number>
</property>
<property name="tracking">
<bool>false</bool>
<bool>true</bool>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
@ -407,7 +407,7 @@
<number>1500</number>
</property>
<property name="tracking">
<bool>false</bool>
<bool>true</bool>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
@ -453,7 +453,7 @@
<number>1500</number>
</property>
<property name="tracking">
<bool>false</bool>
<bool>true</bool>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>

9
ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h
View File

@ -42,9 +42,12 @@ struct accelGyroBias {
};
struct actuatorChannelSettings {
qint16 channelMax;
qint16 channelNeutral;
qint16 channelMin;
quint16 channelMin;
quint16 channelNeutral;
quint16 channelMax;
//Default values
actuatorChannelSettings(): channelMin(1000), channelNeutral(1080), channelMax(1800) {}
};
struct actuatorSettings {