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-39 Motor and servo calibration code.

Browse Source
This commit is contained in:
Fredrik Arvidsson 2012-09-04 01:28:28 +02:00
parent b150375ff5
commit 51dc691ab8
8 changed files with 104 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -28,6 +28,9 @@
#include "outputcalibrationutil.h"
#include "actuatorcommand.h"
#include "extensionsystem/pluginmanager.h"
#include "vehicleconfigurationhelper.h"
const quint16 OutputCalibrationUtil::UPDATE_CHANNEL_MAPPING[10] = {1, 1, 1, 2, 3, 4, 3, 3, 4, 4};
OutputCalibrationUtil::OutputCalibrationUtil(QObject *parent) :
QObject(parent), m_outputChannel(0)
@ -37,21 +40,39 @@ OutputCalibrationUtil::OutputCalibrationUtil(QObject *parent) :
Q_ASSERT(m_uavObjectManager);
}
void OutputCalibrationUtil::startChannelOutput(quint16 channel)
void OutputCalibrationUtil::startChannelOutput(quint16 escUpdateRate, quint16 channel)
{
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;
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);
ActuatorCommand *actuatorCommand = ActuatorCommand::GetInstance(m_uavObjectManager);
Q_ASSERT(actuatorCommand);
UAVObject::Metadata metaData = actuatorCommand->getMetadata();
m_savedActuatorCommandMetadata = metaData;
m_savedActuatorMetadata = metaData;
//Enable actuator control from GCS...
//Store current metadata for later restore
UAVObject::SetFlightAccess(metaData, UAVObject::ACCESS_READONLY);
UAVObject::SetFlightTelemetryUpdateMode(metaData, UAVObject::UPDATEMODE_ONCHANGE);
UAVObject::SetGcsTelemetryAcked(metaData, false);
UAVObject::SetGcsTelemetryUpdateMode(metaData, UAVObject::UPDATEMODE_ONCHANGE);
metaData.gcsTelemetryUpdatePeriod = 100;
actuatorCommand->setMetadata(metaData);
actuatorCommand->updated();
@ -65,15 +86,17 @@ void OutputCalibrationUtil::stopChannelOutput()
if(m_outputChannel > 0)
{
//Stop output...
// Restore metadata to what it was before
ActuatorCommand *actuatorCommand = ActuatorCommand::GetInstance(m_uavObjectManager);
Q_ASSERT(actuatorCommand);
UAVObject::Metadata metaData = actuatorCommand->getMetadata();
// Restore metadata to what it was before
metaData = m_savedActuatorMetadata;
actuatorCommand->setMetadata(metaData);
actuatorCommand->setMetadata(m_savedActuatorCommandMetadata);
actuatorCommand->updated();
ActuatorSettings *actuatorSettings = ActuatorSettings::GetInstance(m_uavObjectManager);
Q_ASSERT(actuatorSettings);
actuatorSettings->setData(m_savedActuatorSettingData);
actuatorSettings->updated();
m_outputChannel = 0;
}
}

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

@ -31,6 +31,7 @@
#include <QObject>
#include "uavobject.h"
#include "uavobjectmanager.h"
#include "vehicleconfigurationsource.h"
class OutputCalibrationUtil : public QObject
@ -42,16 +43,20 @@ public:
signals:
public slots:
void startChannelOutput(quint16 channel);
void startChannelOutput(quint16 escUpdateRate, quint16 channel);
void stopChannelOutput();
void setChannelOutputValue(quint16 value);
private:
static const quint16 UPDATE_CHANNEL_MAPPING[10];
VehicleConfigurationSource *m_configSource;
actuatorSettings *m_actuatorSettings;
quint16 m_outputChannel;
UAVObject::Metadata m_savedActuatorMetadata;
UAVObject::Metadata m_savedActuatorCommandMetadata;
ActuatorSettings::DataFields m_savedActuatorSettingData;
UAVObjectManager *m_uavObjectManager;
};
#endif // OUTPUTCALIBRATIONUTIL_H

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

@ -67,31 +67,43 @@ void OutputCalibrationPage::setupVehicle()
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();
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();
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();
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_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();
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_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();
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_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();
break;
default:
break;
@ -188,7 +200,8 @@ void OutputCalibrationPage::on_backPageButton_clicked()
void OutputCalibrationPage::on_motorNeutralButton_toggled(bool checked)
{
if(checked) {
m_calibrationUtil->startChannelOutput(m_vehicleHighlightElementIndexes[m_currentWizardIndex]);
quint16 channel = m_vehicleHighlightElementIndexes[m_currentWizardIndex];
m_calibrationUtil->startChannelOutput(m_channelUpdateRates[channel], channel);
m_calibrationUtil->setChannelOutputValue(ui->motorNeutralSlider->value());
}
else {

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

@ -32,6 +32,7 @@
#include "abstractwizardpage.h"
#include "setupwizard.h"
#include "outputcalibrationutil.h"
#include "vehicleconfigurationhelper.h"
namespace Ui {
class OutputCalibrationPage;
@ -73,6 +74,11 @@ private:
void setupVehicleHighlightedPart();
void setWizardPage();
quint16 getEscUpdateRate(){ return getWizard()->getESCType() == VehicleConfigurationSource::ESC_RAPID ?
VehicleConfigurationHelper::RAPID_ESC_FREQUENCE : VehicleConfigurationHelper::LEGACY_ESC_FREQUENCE; }
quint16 getServoUpdateRate(){ return VehicleConfigurationHelper::LEGACY_ESC_FREQUENCE; }
Ui::OutputCalibrationPage *ui;
QSvgRenderer *m_vehicleRenderer;
QGraphicsScene *m_vehicleScene;
@ -84,6 +90,7 @@ private:
QList<QGraphicsSvgItem*> m_vehicleItems;
QList<quint16> m_vehicleHighlightElementIndexes;
QList<quint16> m_wizardIndexes;
QList<quint16> m_channelUpdateRates;
OutputCalibrationUtil *m_calibrationUtil;

8
ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h
View File

@ -33,6 +33,7 @@
#include <coreplugin/icore.h>
#include <coreplugin/connectionmanager.h>
#include "vehicleconfigurationsource.h"
#include "vehicleconfigurationhelper.h"
class SetupWizard : public QWizard, public VehicleConfigurationSource
@ -62,9 +63,9 @@ public:
bool isLevellingPerformed() const { return m_levellingPerformed; }
accelGyroBias getLevellingBias() const { return m_levellingBias; }
void setActuatorNeutralSettings(QList<quint16> neutralSettings) { m_actuatorNeutralConfig = neutralSettings; }
void setActuatorSettings(actuatorSettings neutralSettings) { m_actuatorNeutralConfig = neutralSettings; }
bool isMotorCalibrationPerformed() const { return m_motorCalibrationPerformed; }
QList<quint16> getActuatorNeutralSettings() const { return m_actuatorNeutralConfig; }
actuatorSettings getActuatorSettings() const { return m_actuatorNeutralConfig; }
QString getSummaryText();
@ -87,11 +88,12 @@ private:
VEHICLE_SUB_TYPE m_vehicleSubType;
INPUT_TYPE m_inputType;
ESC_TYPE m_escType;
bool m_levellingPerformed;
accelGyroBias m_levellingBias;
bool m_motorCalibrationPerformed;
QList<quint16> m_actuatorNeutralConfig;
actuatorSettings m_actuatorNeutralConfig;
Core::ConnectionManager *m_connectionManager;
};

35
ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp
View File

@ -35,6 +35,9 @@
#include "manualcontrolsettings.h"
#include "stabilizationsettings.h"
const qint16 VehicleConfigurationHelper::LEGACY_ESC_FREQUENCE = 50;
const qint16 VehicleConfigurationHelper::RAPID_ESC_FREQUENCE = 400;
VehicleConfigurationHelper::VehicleConfigurationHelper(VehicleConfigurationSource *configSource)
: m_configSource(configSource), m_uavoManager(0),
m_transactionOK(false), m_transactionTimeout(false), m_currentTransactionObjectID(-1),
@ -177,30 +180,27 @@ void VehicleConfigurationHelper::applyVehicleConfiguration()
void VehicleConfigurationHelper::applyActuatorConfiguration()
{
ActuatorSettings* actSettings = ActuatorSettings::GetInstance(m_uavoManager);
switch(m_configSource->getVehicleType())
{
case VehicleConfigurationSource::VEHICLE_MULTI:
{
switch(m_configSource->getVehicleType()) {
case VehicleConfigurationSource::VEHICLE_MULTI: {
ActuatorSettings::DataFields data = actSettings->getData();
actuatorSettings actuatorSettings = m_configSource->getActuatorSettings();
for(quint16 i = 0; i < ActuatorSettings::CHANNELMAX_NUMELEM; i++) {
data.ChannelType[i] = ActuatorSettings::CHANNELTYPE_PWM;
data.ChannelAddr[i] = i;
data.ChannelMin[i] = actuatorSettings.channels[i].channelMin;
data.ChannelNeutral[i] = actuatorSettings.channels[i].channelNeutral;
data.ChannelMax[i] = actuatorSettings.channels[i].channelMax;
}
data.MotorsSpinWhileArmed = ActuatorSettings::MOTORSSPINWHILEARMED_FALSE;
for(quint16 i = 0; i < ActuatorSettings::CHANNELUPDATEFREQ_NUMELEM; i++) {
data.ChannelUpdateFreq[i] = LEGACY_ESC_FREQUENCE;
}
for(quint16 i = 0; i < ActuatorSettings::CHANNELMAX_NUMELEM; i++) {
data.ChannelType[i] = ActuatorSettings::CHANNELTYPE_PWM;
data.ChannelAddr[i] = i;
data.ChannelMin[i] = ACTUATOR_MIN;
data.ChannelNeutral[i] = ACTUATOR_NEUTRAL;
data.ChannelMax[i] = ACTUATOR_MAX;
}
data.MotorsSpinWhileArmed = ActuatorSettings::MOTORSSPINWHILEARMED_FALSE;
qint16 updateFrequence = LEGACY_ESC_FREQUENCE;
switch(m_configSource->getESCType())
{
switch(m_configSource->getESCType()) {
case VehicleConfigurationSource::ESC_LEGACY:
updateFrequence = LEGACY_ESC_FREQUENCE;
break;
@ -211,8 +211,7 @@ void VehicleConfigurationHelper::applyActuatorConfiguration()
break;
}
switch(m_configSource->getVehicleSubType())
{
switch(m_configSource->getVehicleSubType()) {
case VehicleConfigurationSource::MULTI_ROTOR_TRI_Y:
data.ChannelUpdateFreq[0] = updateFrequence;
break;

13
ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h
View File

@ -34,8 +34,9 @@
#include "uavobjectmanager.h"
#include "systemsettings.h"
#include "cfg_vehicletypes/vehicleconfig.h"
#include "actuatorsettings.h"
struct channelSettings {
struct mixerChannelSettings {
int type;
int throttle1;
int throttle2;
@ -43,14 +44,14 @@ struct channelSettings {
int pitch;
int yaw;
channelSettings() : type(), throttle1(), throttle2(), roll(), pitch(), yaw() {}
mixerChannelSettings() : type(), throttle1(), throttle2(), roll(), pitch(), yaw() {}
channelSettings(int t, int th1, int th2, int r, int p, int y)
mixerChannelSettings(int t, int th1, int th2, int r, int p, int y)
: type(t), throttle1(th1), throttle2(th2), roll(r), pitch(p), yaw(y) {}
};
struct mixerSettings {
channelSettings channels[10];
mixerChannelSettings channels[10];
};
class VehicleConfigurationHelper : public QObject
@ -60,13 +61,13 @@ class VehicleConfigurationHelper : public QObject
public:
VehicleConfigurationHelper(VehicleConfigurationSource* configSource);
bool setupVehicle();
static const qint16 LEGACY_ESC_FREQUENCE;
static const qint16 RAPID_ESC_FREQUENCE;
signals:
void saveProgress(int total, int current, QString description);
private:
static const qint16 LEGACY_ESC_FREQUENCE = 50;
static const qint16 RAPID_ESC_FREQUENCE = 400;
static const qint16 ACTUATOR_MIN = 1000;
static const qint16 ACTUATOR_NEUTRAL = 1000;

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

@ -29,6 +29,7 @@
#define VEHICLECONFIGURATIONSOURCE_H
#include <QString>
#include "actuatorsettings.h"
struct accelGyroBias {
float m_accelerometerXBias;
@ -40,6 +41,16 @@ struct accelGyroBias {
float m_gyroZBias;
};
struct actuatorChannelSettings {
qint16 channelMax;
qint16 channelNeutral;
qint16 channelMin;
};
struct actuatorSettings {
actuatorChannelSettings channels[ActuatorSettings::CHANNELADDR_NUMELEM];
};
class VehicleConfigurationSource
{
public:
@ -64,7 +75,7 @@ public:
virtual accelGyroBias getLevellingBias() const = 0;
virtual bool isMotorCalibrationPerformed() const = 0;
virtual QList<quint16> getActuatorNeutralSettings() const = 0;
virtual actuatorSettings getActuatorSettings() const = 0;
virtual QString getSummaryText() = 0;
};