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

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       outputcalibrationutil.cpp
   5  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015.
   6  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
   7  * @addtogroup
   8  * @{
   9  * @addtogroup OutputCalibrationUtil
  10  * @{
  11  * @brief
  12  *****************************************************************************/
  13 /*
  14  * This program is free software; you can redistribute it and/or modify
  15  * it under the terms of the GNU General Public License as published by
  16  * the Free Software Foundation; either version 3 of the License, or
  17  * (at your option) any later version.
  18  *
  19  * This program is distributed in the hope that it will be useful, but
  20  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  21  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  22  * for more details.
  23  *
  24  * You should have received a copy of the GNU General Public License along
  25  * with this program; if not, write to the Free Software Foundation, Inc.,
  26  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  27  */
  28
  29 #include "outputcalibrationutil.h"
  30 #include "uavobject.h"
  31 #include "uavobjectmanager.h"
  32 #include "extensionsystem/pluginmanager.h"
  33 #include "vehicleconfigurationhelper.h"
  34 #include "manualcontrolsettings.h"
  35
  36 bool OutputCalibrationUtil::c_prepared = false;
  37 ActuatorCommand::Metadata OutputCalibrationUtil::c_savedActuatorCommandMetaData;
  38
  39 OutputCalibrationUtil::OutputCalibrationUtil(QObject *parent) :
  40     QObject(parent), m_safeValue(1000)
  41 {}
  42
  43 OutputCalibrationUtil::~OutputCalibrationUtil()
  44 {
  45     stopChannelOutput();
  46 }
  47
  48 ActuatorCommand *OutputCalibrationUtil::getActuatorCommandObject()
  49 {
  50     ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
  51
  52     Q_ASSERT(pm);
  53
  54     UAVObjectManager *uavObjectManager = pm->getObject<UAVObjectManager>();
  55     Q_ASSERT(uavObjectManager);
  56
  57     ActuatorCommand *actuatorCommand   = ActuatorCommand::GetInstance(uavObjectManager);
  58     Q_ASSERT(actuatorCommand);
  59
  60     return actuatorCommand;
  61 }
  62
  63 void OutputCalibrationUtil::startOutputCalibration()
  64 {
  65     if (!c_prepared) {
  66         ActuatorCommand *actuatorCommand = getActuatorCommandObject();
  67         UAVObject::Metadata metaData     = actuatorCommand->getMetadata();
  68         c_savedActuatorCommandMetaData = metaData;
  69
  70         // Enable actuator control from GCS...
  71         // Store current metadata for later restore
  72         UAVObject::SetFlightAccess(metaData, UAVObject::ACCESS_READONLY);
  73         UAVObject::SetFlightTelemetryUpdateMode(metaData, UAVObject::UPDATEMODE_ONCHANGE);
  74         UAVObject::SetGcsTelemetryAcked(metaData, false);
  75         UAVObject::SetGcsTelemetryUpdateMode(metaData, UAVObject::UPDATEMODE_ONCHANGE);
  76         metaData.gcsTelemetryUpdatePeriod = 100;
  77
  78         // Apply changes
  79         actuatorCommand->setMetadata(metaData);
  80         actuatorCommand->updated();
  81         c_prepared = true;
  82         qDebug() << "OutputCalibrationUtil started.";
  83     }
  84 }
  85
  86 void OutputCalibrationUtil::stopOutputCalibration()
  87 {
  88     if (c_prepared) {
  89         ActuatorCommand *actuatorCommand = getActuatorCommandObject();
  90         actuatorCommand->setMetadata(c_savedActuatorCommandMetaData);
  91         actuatorCommand->updated();
  92         c_prepared = false;
  93         qDebug() << "OutputCalibrationUtil stopped.";
  94     }
  95 }
  96
  97 void OutputCalibrationUtil::startChannelOutput(quint16 channel, quint16 safeValue)
  98 {
  99     QList<quint16> channels;
 100     channels.append(channel);
 101     startChannelOutput(channels, safeValue);
 102 }
 103
 104 void OutputCalibrationUtil::startChannelOutput(QList<quint16> &channels, quint16 safeValue)
 105 {
 106     if (c_prepared) {
 107         m_outputChannels = channels;
 108         m_safeValue = safeValue;
 109     } else {
 110         qDebug() << "OutputCalibrationUtil not started.";
 111     }
 112 }
 113
 114 void OutputCalibrationUtil::stopChannelOutput()
 115 {
 116     if (c_prepared) {
 117         setChannelOutputValue(m_safeValue);
 118         m_outputChannels.clear();
 119         qDebug() << "OutputCalibrationUtil output stopped.";
 120     } else {
 121         qDebug() << "OutputCalibrationUtil not started.";
 122     }
 123 }
 124
 125 void OutputCalibrationUtil::stopChannelDualOutput(quint16 safeValue1, quint16 safeValue2)
 126 {
 127     if (c_prepared) {
 128         setChannelDualOutputValue(safeValue1, safeValue2);
 129         m_outputChannels.clear();
 130         qDebug() << "OutputCalibrationUtil Dual output stopped.";
 131     } else {
 132         qDebug() << "OutputCalibrationUtil Dual output not started.";
 133     }
 134 }
 135
 136 void OutputCalibrationUtil::setChannelOutputValue(quint16 value)
 137 {
 138     if (c_prepared) {
 139         ActuatorCommand *actuatorCommand = getActuatorCommandObject();
 140         ActuatorCommand::DataFields data = actuatorCommand->getData();
 141         foreach(quint32 channel, m_outputChannels) {
 142             // Set output value
 143             if (channel <= ActuatorCommand::CHANNEL_NUMELEM) {
 144                 qDebug() << "OutputCalibrationUtil setting output value for channel " << channel << " to " << value << ".";
 145                 data.Channel[channel] = value;
 146             } else {
 147                 qDebug() << "OutputCalibrationUtil could not set output value for channel " << channel
 148                          << " to " << value << "." << "Channel out of bounds" << channel << ".";
 149             }
 150         }
 151         actuatorCommand->setData(data);
 152     } else {
 153         qDebug() << "OutputCalibrationUtil not started.";
 154     }
 155 }
 156
 157 void OutputCalibrationUtil::setChannelDualOutputValue(quint16 value1, quint16 value2)
 158 {
 159     if (c_prepared && (m_outputChannels.size() == 2)) {
 160         ActuatorCommand *actuatorCommand = getActuatorCommandObject();
 161         ActuatorCommand::DataFields data = actuatorCommand->getData();
 162         quint32 channel1 = m_outputChannels[0];
 163         quint32 channel2 = m_outputChannels[1];
 164         // Set output value
 165         if (channel1 <= ActuatorCommand::CHANNEL_NUMELEM) {
 166             qDebug() << "OutputCalibrationUtil (Dual) setting output value for channel1 " << channel1 << " to " << value1 << ".";
 167             data.Channel[channel1] = value1;
 168             actuatorCommand->setData(data);
 169         } else {
 170             qDebug() << "OutputCalibrationUtil could not set output value for channel1 " << channel1
 171                      << " to " << value1 << "." << "Channel out of bounds" << channel1 << ".";
 172         }
 173
 174         if (channel2 <= ActuatorCommand::CHANNEL_NUMELEM) {
 175             qDebug() << "OutputCalibrationUtil (Dual) setting output value for channel2 " << channel2 << " to " << value2 << ".";
 176             data.Channel[channel2] = value2;
 177             actuatorCommand->setData(data);
 178         } else {
 179             qDebug() << "OutputCalibrationUtil could not set output value for channel2 " << channel2
 180                      << " to " << value2 << "." << "Channel out of bounds" << channel2 << ".";
 181         }
 182     } else {
 183         qDebug() << "OutputCalibrationUtil not started.";
 184     }
 185 }