OP-39 Motor and servo calibration code.

ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.cpp

@@ -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();
+        actuatorCommand->setMetadata(m_savedActuatorCommandMetadata);
-
-        // Restore metadata to what it was before
-        metaData = m_savedActuatorMetadata;
-        actuatorCommand->setMetadata(metaData);
         actuatorCommand->updated();
 
+        ActuatorSettings *actuatorSettings = ActuatorSettings::GetInstance(m_uavObjectManager);
+        Q_ASSERT(actuatorSettings);
+        actuatorSettings->setData(m_savedActuatorSettingData);
+        actuatorSettings->updated();
+
         m_outputChannel = 0;
     }
 }

ground/openpilotgcs/src/plugins/setupwizard/outputcalibrationutil.h

@@ -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

ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.cpp

@@ -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 {

ground/openpilotgcs/src/plugins/setupwizard/pages/outputcalibrationpage.h

@@ -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;
 

ground/openpilotgcs/src/plugins/setupwizard/setupwizard.h

@@ -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;
 };

ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp

@@ -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())
+    switch(m_configSource->getVehicleType()) {
-    {
+        case VehicleConfigurationSource::VEHICLE_MULTI: {
-        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;

ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.h

@@ -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;

ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationsource.h

@@ -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;
 };