From 379d381975910d337bc454abaec8cf56c6a62dc8 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 4 Mar 2015 19:44:22 -0800 Subject: [PATCH] Remove unused code --- src/comm/OpalLink.cc | 467 -------------------------- src/comm/OpalLink.h | 155 --------- src/comm/OpalRT.cc | 24 -- src/comm/OpalRT.h | 133 -------- src/ui/OpalLinkConfigurationWindow.cc | 36 -- src/ui/OpalLinkConfigurationWindow.h | 29 -- src/ui/OpalLinkSettings.ui | 173 ---------- 7 files changed, 1017 deletions(-) delete mode 100644 src/comm/OpalLink.cc delete mode 100644 src/comm/OpalLink.h delete mode 100644 src/comm/OpalRT.cc delete mode 100644 src/comm/OpalRT.h delete mode 100644 src/ui/OpalLinkConfigurationWindow.cc delete mode 100644 src/ui/OpalLinkConfigurationWindow.h delete mode 100644 src/ui/OpalLinkSettings.ui diff --git a/src/comm/OpalLink.cc b/src/comm/OpalLink.cc deleted file mode 100644 index b792b33ab..000000000 --- a/src/comm/OpalLink.cc +++ /dev/null @@ -1,467 +0,0 @@ -/*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 QGROUNDCONTROL PROJECT - -This file is part of the QGROUNDCONTROL project - - QGROUNDCONTROL 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. - - QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see . - -======================================================================*/ - -/** - * @file - * @brief Implementation of class OpalLink - * @author Bryan Godbolt - */ - -#include "OpalLink.h" - -OpalLink::OpalLink() : - connectState(false), - heartbeatTimer(new QTimer(this)), - heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE), - m_heartbeatsEnabled(true), - getSignalsTimer(new QTimer(this)), - getSignalsPeriod(10), - receiveBuffer(new QQueue()), - systemID(1), - componentID(1), - params(NULL), - opalInstID(101), - sendRCValues(false), - sendRawController(false), - sendPosition(false) -{ - start(QThread::LowPriority); - - // Set unique ID and add link to the list of links - this->id = getNextLinkId(); - this->name = tr("OpalRT link ") + QString::number(getId()); - LinkManager::instance()->add(this); - - // Start heartbeat timer, emitting a heartbeat at the configured rate - QObject::connect(heartbeatTimer, SIGNAL(timeout()), this, SLOT(heartbeat())); - - QObject::connect(getSignalsTimer, SIGNAL(timeout()), this, SLOT(getSignals())); -} - -void OpalLink::writeBytes(const char *bytes, qint64 length) -{ - /* decode the message */ - mavlink_message_t msg; - mavlink_status_t status; - int decodeSuccess = 0; - for (int i=0; (!(decodeSuccess=mavlink_parse_char(this->getId(), bytes[i], &msg, &status))&& ibegin(); paramIter != params->end(); ++paramIter) { - mavlink_msg_param_value_pack(systemID, - (*paramIter).getComponentID(), - ¶m, - (*paramIter).getParamID().toInt8_t(), - (static_cast(*paramIter)).getValue(), - params->count(), - params->indexOf(*paramIter)); - receiveMessage(param); - } - - - } - case MAVLINK_MSG_ID_PARAM_SET: { - -// qDebug() << "OpalLink::writeBytes(): Attempt to set a parameter"; - - mavlink_param_set_t param; - mavlink_msg_param_set_decode(&msg, ¶m); - OpalRT::QGCParamID paramName((char*)param.param_id); - -// qDebug() << "OpalLink::writeBytes():paramName: " << paramName; - - if ((*params).contains(param.target_component, paramName)) { - OpalRT::Parameter p = (*params)(param.target_component, paramName); -// qDebug() << __FILE__ << ":" << __LINE__ << ": " << p; - // Set the param value in Opal-RT - p.setValue(param.param_value); - - // Get the param value from Opal-RT to make sure it was set properly - mavlink_message_t paramMsg; - mavlink_msg_param_value_pack(systemID, - p.getComponentID(), - ¶mMsg, - p.getParamID().toInt8_t(), - p.getValue(), - params->count(), - params->indexOf(p)); - receiveMessage(paramMsg); - } - } - break; -// case MAVLINK_MSG_ID_REQUEST_RC_CHANNELS: -// { -// mavlink_request_rc_channels_t rc; -// mavlink_msg_request_rc_channels_decode(&msg, &rc); -// this->sendRCValues = static_cast(rc.enabled); -// } -// break; -#ifdef MAVLINK_ENABLED_UALBERTA_MESSAGES - case MAVLINK_MSG_ID_RADIO_CALIBRATION: { - mavlink_radio_calibration_t radio; - mavlink_msg_radio_calibration_decode(&msg, &radio); -// qDebug() << "RADIO CALIBRATION RECEIVED"; -// qDebug() << "AILERON: " << radio.aileron[0] << " " << radio.aileron[1] << " " << radio.aileron[2]; -// qDebug() << "ELEVATOR: " << radio.elevator[0] << " " << radio.elevator[1] << " " << radio.elevator[2]; -// qDebug() << "RUDDER: " << radio.rudder[0] << " " << radio.rudder[1] << " " << radio.rudder[2]; -// qDebug() << "GYRO: " << radio.gyro[0] << " " << radio.gyro[1]; -// qDebug() << "PITCH: " << radio.pitch[0] << radio.pitch[1] << radio.pitch[2] << radio.pitch[3] << radio.pitch[4]; -// qDebug() << "THROTTLE: " << radio.throttle[0] << radio.throttle[1] << radio.throttle[2] << radio.throttle[3] << radio.throttle[4]; - - /* AILERON SERVO */ - if (params->contains(OpalRT::SERVO_INPUTS, "AIL_RIGHT_IN")) - params->getParameter(OpalRT::SERVO_INPUTS, "AIL_RIGHT_IN").setValue(((radio.aileron[0]>900 /*in us?*/)?radio.aileron[0]/1000:radio.aileron[0])); - if (params->contains(OpalRT::SERVO_OUTPUTS, "AIL_RIGHT_OUT")) - params->getParameter(OpalRT::SERVO_OUTPUTS, "AIL_RIGHT_OUT").setValue(((radio.aileron[0]>900 /*in us?*/)?radio.aileron[0]/1000:radio.aileron[0])); - if (params->contains(OpalRT::SERVO_INPUTS, "AIL_CENTER_IN")) - params->getParameter(OpalRT::SERVO_INPUTS, "AIL_CENTER_IN").setValue(((radio.aileron[1]>900 /*in us?*/)?radio.aileron[1]/1000:radio.aileron[1])); - if (params->contains(OpalRT::SERVO_OUTPUTS, "AIL_CENTER_OUT")) - params->getParameter(OpalRT::SERVO_OUTPUTS, "AIL_CENTER_OUT").setValue(((radio.aileron[1]>900 /*in us?*/)?radio.aileron[1]/1000:radio.aileron[1])); - if (params->contains(OpalRT::SERVO_INPUTS, "AIL_LEFT_IN")) - params->getParameter(OpalRT::SERVO_INPUTS, "AIL_LEFT_IN").setValue(((radio.aileron[2]>900 /*in us?*/)?radio.aileron[2]/1000:radio.aileron[2])); - if (params->contains(OpalRT::SERVO_OUTPUTS, "AIL_LEFT_OUT")) - params->getParameter(OpalRT::SERVO_OUTPUTS, "AIL_LEFT_OUT").setValue(((radio.aileron[2]>900 /*in us?*/)?radio.aileron[2]/1000:radio.aileron[2])); - /* ELEVATOR SERVO */ - if (params->contains(OpalRT::SERVO_INPUTS, "ELE_DOWN_IN")) - params->getParameter(OpalRT::SERVO_INPUTS, "ELE_DOWN_IN").setValue(((radio.elevator[0]>900 /*in us?*/)?radio.elevator[0]/1000:radio.elevator[0])); - if (params->contains(OpalRT::SERVO_OUTPUTS, "ELE_DOWN_OUT")) - params->getParameter(OpalRT::SERVO_OUTPUTS, "ELE_DOWN_OUT").setValue(((radio.elevator[0]>900 /*in us?*/)?radio.elevator[0]/1000:radio.elevator[0])); - if (params->contains(OpalRT::SERVO_INPUTS, "ELE_CENTER_IN")) - params->getParameter(OpalRT::SERVO_INPUTS, "ELE_CENTER_IN").setValue(((radio.elevator[1]>900 /*in us?*/)?radio.elevator[1]/1000:radio.elevator[1])); - if (params->contains(OpalRT::SERVO_OUTPUTS, "ELE_CENTER_OUT")) - params->getParameter(OpalRT::SERVO_OUTPUTS, "ELE_CENTER_OUT").setValue(((radio.elevator[1]>900 /*in us?*/)?radio.elevator[1]/1000:radio.elevator[1])); - if (params->contains(OpalRT::SERVO_INPUTS, "ELE_UP_IN")) - params->getParameter(OpalRT::SERVO_INPUTS, "ELE_UP_IN").setValue(((radio.elevator[2]>900 /*in us?*/)?radio.elevator[2]/1000:radio.elevator[2])); - if (params->contains(OpalRT::SERVO_OUTPUTS, "ELE_UP_OUT")) - params->getParameter(OpalRT::SERVO_OUTPUTS, "ELE_UP_OUT").setValue(((radio.elevator[2]>900 /*in us?*/)?radio.elevator[2]/1000:radio.elevator[2])); - /* THROTTLE SERVO */ - if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET0_IN")) - params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET0_IN").setValue(((radio.throttle[0]>900 /*in us?*/)?radio.throttle[0]/1000:radio.throttle[0])); - if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET1_IN")) - params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET1_IN").setValue(((radio.throttle[1]>900 /*in us?*/)?radio.throttle[1]/1000:radio.throttle[1])); - if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET2_IN")) - params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET2_IN").setValue(((radio.throttle[2]>900 /*in us?*/)?radio.throttle[2]/1000:radio.throttle[2])); - if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET3_IN")) - params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET3_IN").setValue(((radio.throttle[3]>900 /*in us?*/)?radio.throttle[3]/1000:radio.throttle[3])); - if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET4_IN")) - params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET4_IN").setValue(((radio.throttle[4]>900 /*in us?*/)?radio.throttle[4]/1000:radio.throttle[4])); - /* RUDDER SERVO */ - if (params->contains(OpalRT::SERVO_INPUTS, "RUD_LEFT_IN")) - params->getParameter(OpalRT::SERVO_INPUTS, "RUD_LEFT_IN").setValue(((radio.rudder[0]>900 /*in us?*/)?radio.rudder[0]/1000:radio.rudder[0])); - if (params->contains(OpalRT::SERVO_INPUTS, "RUD_CENTER_IN")) - params->getParameter(OpalRT::SERVO_INPUTS, "RUD_CENTER_IN").setValue(((radio.rudder[1]>900 /*in us?*/)?radio.rudder[1]/1000:radio.rudder[1])); - if (params->contains(OpalRT::SERVO_INPUTS, "RUD_RIGHT_IN")) - params->getParameter(OpalRT::SERVO_INPUTS, "RUD_RIGHT_IN").setValue(((radio.rudder[2]>900 /*in us?*/)?radio.rudder[2]/1000:radio.rudder[2])); - /* GYRO MODE/GAIN SWITCH */ - if (params->contains(OpalRT::SERVO_INPUTS, "GYRO_DEF_IN")) - params->getParameter(OpalRT::SERVO_INPUTS, "GYRO_DEF_IN").setValue(((radio.gyro[0]>900 /*in us?*/)?radio.gyro[0]/1000:radio.gyro[0])); - if (params->contains(OpalRT::SERVO_INPUTS, "GYRO_TOG_IN")) - params->getParameter(OpalRT::SERVO_INPUTS, "GYRO_TOG_IN").setValue(((radio.gyro[1]>900 /*in us?*/)?radio.gyro[1]/1000:radio.gyro[1])); - /* PITCH SERVO */ - if (params->contains(OpalRT::SERVO_INPUTS, "PIT_SET0_IN")) - params->getParameter(OpalRT::SERVO_INPUTS, "PIT_SET0_IN").setValue(((radio.pitch[0]>900 /*in us?*/)?radio.pitch[0]/1000:radio.pitch[0])); - if (params->contains(OpalRT::SERVO_INPUTS, "PIT_SET1_IN")) - params->getParameter(OpalRT::SERVO_INPUTS, "PIT_SET1_IN").setValue(((radio.pitch[1]>900 /*in us?*/)?radio.pitch[1]/1000:radio.pitch[1])); - if (params->contains(OpalRT::SERVO_INPUTS, "PIT_SET2_IN")) - params->getParameter(OpalRT::SERVO_INPUTS, "PIT_SET2_IN").setValue(((radio.pitch[2]>900 /*in us?*/)?radio.pitch[2]/1000:radio.pitch[2])); - if (params->contains(OpalRT::SERVO_INPUTS, "PIT_SET3_IN")) - params->getParameter(OpalRT::SERVO_INPUTS, "PIT_SET3_IN").setValue(((radio.pitch[3]>900 /*in us?*/)?radio.pitch[3]/1000:radio.pitch[3])); - if (params->contains(OpalRT::SERVO_INPUTS, "PIT_SET4_IN")) - params->getParameter(OpalRT::SERVO_INPUTS, "PIT_SET4_IN").setValue(((radio.pitch[4]>900 /*in us?*/)?radio.pitch[4]/1000:radio.pitch[4])); - } - break; -#endif - } - - // Log the amount and time written out for future data rate calculations. - QMutexLocker dataRateLocker(&dataRateMutex); - logDataRateToBuffer(outDataWriteAmounts, outDataWriteTimes, &outDataIndex, size, QDateTime::currentMSecsSinceEpoch()); -} - - -void OpalLink::readBytes() -{ - receiveDataMutex.lock(); - emit bytesReceived(this, receiveBuffer->dequeue()); - receiveDataMutex.unlock(); - - // Log the amount and time received for future data rate calculations. - QMutexLocker dataRateLocker(&dataRateMutex); - logDataRateToBuffer(inDataWriteAmounts, inDataWriteTimes, &inDataIndex, s, QDateTime::currentMSecsSinceEpoch()); -} - -void OpalLink::receiveMessage(mavlink_message_t message) -{ - - // Create buffer - char buffer[MAVLINK_MAX_PACKET_LEN]; - // Write message into buffer, prepending start sign - int len = mavlink_msg_to_send_buffer((uint8_t*)(buffer), &message); - // If link is connected - if (isConnected()) { - receiveDataMutex.lock(); - receiveBuffer->enqueue(QByteArray(buffer, len)); - receiveDataMutex.unlock(); - readBytes(); - } - -} - -void OpalLink::heartbeat() -{ - - if (m_heartbeatsEnabled) { - mavlink_message_t beat; - mavlink_msg_heartbeat_pack(systemID, componentID,&beat, MAV_HELICOPTER, MAV_AUTOPILOT_GENERIC); - receiveMessage(beat); - } - -} -void OpalLink::setSignals(double *values) -{ - unsigned short numSignals = 2; - unsigned short logicalId = 1; - unsigned short signalIndex[] = {0,1}; - - int returnValue; - returnValue = OpalSetSignals( numSignals, logicalId, signalIndex, values); - if (returnValue != EOK) { - OpalRT::OpalErrorMsg::displayLastErrorMsg(); - } -} -void OpalLink::getSignals() -{ - unsigned long timeout = 0; - unsigned short acqGroup = 0; //this is actually group 1 in the model - unsigned short *numSignals = new unsigned short(0); - double *timestep = new double(0); - double values[OpalRT::NUM_OUTPUT_SIGNALS] = {}; - unsigned short *lastValues = new unsigned short(false); - unsigned short *decimation = new unsigned short(0); - - while (!(*lastValues)) { - int returnVal = OpalGetSignals(timeout, acqGroup, OpalRT::NUM_OUTPUT_SIGNALS, numSignals, timestep, - values, lastValues, decimation); - - if (returnVal == EOK ) { - /* Send position info to qgroundcontrol */ - if (sendPosition) { - mavlink_message_t local_position; - mavlink_msg_local_position_pack(systemID, componentID, &local_position, - (*timestep)*1000000, - values[OpalRT::X_POS], - values[OpalRT::Y_POS], - values[OpalRT::Z_POS], - values[OpalRT::X_VEL], - values[OpalRT::Y_VEL], - values[OpalRT::Z_VEL]); - receiveMessage(local_position); - } - /* send attitude info to qgroundcontrol */ - mavlink_message_t attitude; - mavlink_msg_attitude_pack(systemID, componentID, &attitude, - (*timestep)*1000000, - values[OpalRT::ROLL], - values[OpalRT::PITCH], - values[OpalRT::YAW], - values[OpalRT::ROLL_SPEED], - values[OpalRT::PITCH_SPEED], - values[OpalRT::YAW_SPEED] - ); - receiveMessage(attitude); - - /* send bias info to qgroundcontrol */ - mavlink_message_t bias; - mavlink_msg_nav_filter_bias_pack(systemID, componentID, &bias, - (*timestep)*1000000, - values[OpalRT::B_F_0], - values[OpalRT::B_F_1], - values[OpalRT::B_F_2], - values[OpalRT::B_W_0], - values[OpalRT::B_W_1], - values[OpalRT::B_W_2] - ); - receiveMessage(bias); - - /* send radio outputs */ - if (sendRCValues) { - mavlink_message_t rc; - mavlink_msg_rc_channels_raw_pack(systemID, componentID, &rc, - duty2PulseMicros(values[OpalRT::RAW_CHANNEL_1]), - duty2PulseMicros(values[OpalRT::RAW_CHANNEL_2]), - duty2PulseMicros(values[OpalRT::RAW_CHANNEL_3]), - duty2PulseMicros(values[OpalRT::RAW_CHANNEL_4]), - duty2PulseMicros(values[OpalRT::RAW_CHANNEL_5]), - duty2PulseMicros(values[OpalRT::RAW_CHANNEL_6]), - duty2PulseMicros(values[OpalRT::RAW_CHANNEL_7]), - duty2PulseMicros(values[OpalRT::RAW_CHANNEL_8]), - 0 //rssi unused - ); - receiveMessage(rc); - } - if (sendRawController) { - mavlink_message_t rawController; - mavlink_msg_attitude_controller_output_pack(systemID, componentID, &rawController, - 1, - rescaleControllerOutput(values[OpalRT::CONTROLLER_AILERON]), - rescaleControllerOutput(values[OpalRT::CONTROLLER_ELEVATOR]), - 0, // yaw not used - 0 // thrust not used - ); - receiveMessage(rawController); - } - } else if (returnVal != EAGAIN) { // if returnVal == EAGAIN => data just wasn't ready - getSignalsTimer->stop(); - OpalRT::OpalErrorMsg::displayLastErrorMsg(); - } - } - - /* deallocate used memory */ - - delete numSignals; - delete timestep; - delete lastValues; - delete decimation; - -} - - -/* - * - Administrative - * - */ -void OpalLink::run() -{ -// qDebug() << "OpalLink::run():: Starting the thread"; -} - -int OpalLink::getId() const -{ - return id; -} - -QString OpalLink::getName() const -{ - return name; -} - -void OpalLink::setName(QString name) -{ - this->name = name; - emit nameChanged(this->name); -} - -bool OpalLink::isConnected() const -{ - return connectState; -} - -uint16_t OpalLink::duty2PulseMicros(double duty) -{ - /* duty cycle assumed to be of a signal at 70 Hz */ - return static_cast(duty/70*1000000); -} - -uint8_t OpalLink::rescaleNorm(double norm, int ch) -{ - switch(ch) { - case OpalRT::NORM_CHANNEL_1: - case OpalRT::NORM_CHANNEL_2: - case OpalRT::NORM_CHANNEL_4: - default: - // three setpoints - return static_cast((norm+1)/2*255); - break; - case OpalRT::NORM_CHANNEL_5: - //two setpoints - case OpalRT::NORM_CHANNEL_3: - case OpalRT::NORM_CHANNEL_6: - return static_cast(norm*255); - break; - } -} - -int8_t OpalLink::rescaleControllerOutput(double raw) -{ - return static_cast((raw>=0?raw*127:raw*128)); -} - -bool OpalLink::_connect(void) -{ - short modelState; - - if ((OpalConnect(opalInstID, false, &modelState) == EOK) - && (OpalGetSignalControl(0, true) == EOK) - && (OpalGetParameterControl(true) == EOK)) { - connectState = true; - if (params) - delete params; - params = new OpalRT::ParameterList(); - emit connected(); - heartbeatTimer->start(1000/heartbeatRate); - getSignalsTimer->start(getSignalsPeriod); - } else { - connectState = false; - OpalRT::OpalErrorMsg::displayLastErrorMsg(); - } - - emit connected(connectState); - return connectState; -} - -bool OpalLink::_disconnect(void) -{ - // OpalDisconnect returns void so its success or failure cannot be tested - OpalDisconnect(); - heartbeatTimer->stop(); - getSignalsTimer->stop(); - connectState = false; - emit connected(connectState); - return true; -} - -// Data rate functions -qint64 OpalLink::getConnectionSpeed() const -{ - return 0; //unknown -} - -qint64 OpalLink::getCurrentInDataRate() const -{ - return 0; -} - -qint64 OpalLink::getCurrentOutDataRate() const -{ - return 0; -} diff --git a/src/comm/OpalLink.h b/src/comm/OpalLink.h deleted file mode 100644 index 5f17ea0b7..000000000 --- a/src/comm/OpalLink.h +++ /dev/null @@ -1,155 +0,0 @@ -/*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 QGROUNDCONTROL PROJECT - -This file is part of the QGROUNDCONTROL project - - QGROUNDCONTROL 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. - - QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see . - -======================================================================*/ - -/** - * @file - * @brief Connection to OpalRT - * @author Bryan Godbolt - */ - -#ifndef OPALLINK_H -#define OPALLINK_H - -#include -#include -#include -#include -#include -#include -#include -#include - - -#include "LinkInterface.h" -#include "LinkManager.h" -#include "MG.h" -#include "QGCMAVLink.h" -#include "QGCConfig.h" -#include "OpalRT.h" -#include "ParameterList.h" -#include "Parameter.h" -#include "QGCParamID.h" -#include "OpalApi.h" -#include "errno.h" -#include "string.h" - -/** - * @brief Interface to OpalRT targets - * - * This is an interface to the OpalRT hardware-in-the-loop (HIL) simulator. - * This class receives MAVLink packets as if it is a true link, but it - * interprets the packets internally, and calls the appropriate api functions. - * - * @author Bryan Godbolt - * @ref http://www.opal-rt.com/ - */ -class OpalLink : public LinkInterface -{ - Q_OBJECT - -public: - OpalLink(); - /* Connection management */ - - int getId() const; - QString getName() const; - bool isConnected() const; - - qint64 getConnectionSpeed() const; - qint64 getCurrentInDataRate() const; - qint64 getCurrentOutDataRate() const; - - void run(); - - int getOpalInstID() { - return static_cast(opalInstID); - } - - // These are left unimplemented in order to cause linker errors which indicate incorrect usage of - // connect/disconnect on link directly. All connect/disconnect calls should be made through LinkManager. - bool connect(void); - bool disconnect(void); - -public slots: - - void writeBytes(const char *bytes, qint64 length); - - void readBytes(); - - void heartbeat(); - - void getSignals(); - - void setOpalInstID(int instID) { - opalInstID = static_cast(instID); - } - -protected slots: - - void receiveMessage(mavlink_message_t message); - void setSignals(double *values); - -protected: - QString name; - int id; - bool connectState; - - quint64 connectionStartTime; - - QMutex receiveDataMutex; - - void setName(QString name); - - QTimer* heartbeatTimer; ///< Timer to emit heartbeats - int heartbeatRate; ///< Heartbeat rate, controls the timer interval - bool m_heartbeatsEnabled; ///< Enabled/disable heartbeat emission - - QTimer* getSignalsTimer; - int getSignalsPeriod; - - QQueue* receiveBuffer; - QByteArray* sendBuffer; - - const int systemID; - const int componentID; - - void getParameterList(); - OpalRT::ParameterList *params; - - unsigned short opalInstID; - - uint16_t duty2PulseMicros(double duty); - uint8_t rescaleNorm(double norm, int ch); - int8_t rescaleControllerOutput(double raw); - - bool sendRCValues; - bool sendRawController; - bool sendPosition; - -private: - // From LinkInterface - virtual bool _connect(void); - virtual bool _disconnect(void); -}; - -#endif // OPALLINK_H diff --git a/src/comm/OpalRT.cc b/src/comm/OpalRT.cc deleted file mode 100644 index 16cdee55d..000000000 --- a/src/comm/OpalRT.cc +++ /dev/null @@ -1,24 +0,0 @@ -#include "OpalRT.h" -#include "QGCMessageBox.h" - -namespace OpalRT -{ -// lastErrorMsg = QString(); -void OpalErrorMsg::displayLastErrorMsg() -{ - static QString lastErrorMsg; - setLastErrorMsg(); - QGCMessageBox::critical(QString(), lastErrorMsg); -} - -void OpalErrorMsg::setLastErrorMsg() -{ - char* buf = new char[512]; - unsigned short len; - static QString lastErrorMsg; - OpalGetLastErrMsg(buf, sizeof(buf), &len); - lastErrorMsg.clear(); - lastErrorMsg.append(buf); - delete buf; -} -} diff --git a/src/comm/OpalRT.h b/src/comm/OpalRT.h deleted file mode 100644 index 7920c0118..000000000 --- a/src/comm/OpalRT.h +++ /dev/null @@ -1,133 +0,0 @@ -/*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 QGROUNDCONTROL PROJECT - -This file is part of the QGROUNDCONTROL project - - QGROUNDCONTROL 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. - - QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see . - -======================================================================*/ - -/** - * @file - * @brief Types used for Opal-RT interface configuration - * @author Bryan Godbolt - */ - -#ifndef OPALRT_H -#define OPALRT_H - -#include - -#include "OpalApi.h" - -namespace OpalRT -{ -/** - Configuration info for the model - */ - -const unsigned short NUM_OUTPUT_SIGNALS=48; - -/* ------------------------------ Outputs ------------------------------ -* -* Port 1: Navigation state estimates -* 1 t [s] time elapsed since INS mode started -* 2-4 p^n [m] navigation frame position (N,E,D) -* 5-7 v^n [m/s] navigation frame velocity (N,E,D) -* 8-10 Euler angles [rad] (roll, pitch, yaw) -* 11-13 Angular rates -* 14-16 b_f [m/s^2] accelerometer biases -* 17-19 b_w [rad/s] gyro biases -* -* Port 2: Navigation system status -* 1 mode (0: initialization, 1: INS) -* 2 t_GPS time elapsed since last valid GPS measurement -* 3 solution status (0: SOL_COMPUTED, 1: INSUFFICIENT_OBS) -* 4 position solution type ( 0: NONE, 34: NARROW_FLOAT, -* 49: WIDE_INT, 50: NARROW_INT) -* 5 # obs (number of visible satellites) -* -* Port 3: Covariance matrix diagonal -* 1-15 diagonal elements of estimation error covariance matrix P -*/ -enum SignalPort { - T_ELAPSED, - X_POS, - Y_POS, - Z_POS, - X_VEL, - Y_VEL, - Z_VEL, - ROLL, - PITCH, - YAW, - ROLL_SPEED, - PITCH_SPEED, - YAW_SPEED, - B_F_0, - B_F_1, - B_F_2, - B_W_0, - B_W_1, - B_W_2, - RAW_CHANNEL_1 = 24, - RAW_CHANNEL_2, - RAW_CHANNEL_3, - RAW_CHANNEL_4, - RAW_CHANNEL_5, - RAW_CHANNEL_6, - RAW_CHANNEL_7, - RAW_CHANNEL_8, - NORM_CHANNEL_1, - NORM_CHANNEL_2, - NORM_CHANNEL_3, - NORM_CHANNEL_4, - NORM_CHANNEL_5, - NORM_CHANNEL_6, - NORM_CHANNEL_7, - NORM_CHANNEL_8, - CONTROLLER_AILERON, - CONTROLLER_ELEVATOR, - AIL_POUT, - AIL_IOUT, - AIL_DOUT, - ELE_POUT, - ELE_IOUT, - ELE_DOUT -}; - -/** Component IDs of the parameters. Currently they are all 1 becuase there is no advantage - to dividing them between component ids. However this syntax is used so that this can - easily be changed in the future. - */ -enum SubsystemIds { - NAV = 1, - LOG, - CONTROLLER, - SERVO_OUTPUTS, - SERVO_INPUTS -}; - -class OpalErrorMsg -{ - static QString lastErrorMsg; -public: - static void displayLastErrorMsg(); - static void setLastErrorMsg(); -}; -} -#endif // OPALRT_H diff --git a/src/ui/OpalLinkConfigurationWindow.cc b/src/ui/OpalLinkConfigurationWindow.cc deleted file mode 100644 index 922d15851..000000000 --- a/src/ui/OpalLinkConfigurationWindow.cc +++ /dev/null @@ -1,36 +0,0 @@ -#include "OpalLinkConfigurationWindow.h" - -OpalLinkConfigurationWindow::OpalLinkConfigurationWindow(OpalLink* link, - QWidget *parent, - Qt::WindowFlags flags) : - QWidget(parent, flags), - link(link) - -{ - - - ui.setupUi(this); - - ui.opalInstIDSpinBox->setValue(this->link->getOpalInstID()); - - connect(ui.opalInstIDSpinBox, SIGNAL(valueChanged(int)), link, SLOT(setOpalInstID(int))); - connect(link, &LinkInterface::connected, this, OpalLinkConfigurationWindow::_linkConnected); - connect(link, &LinkInterface::disconnected, this, OpalLinkConfigurationWindow::_linkDisconnected); - this->show(); -} - -void OpalLinkConfigurationWindow::_linkConnected(void) -{ - _allowSettingsAccess(true); -} - -void OpalLinkConfigurationWindow::_linkConnected(void) -{ - _allowSettingsAccess(false); -} - -void OpalLinkConfigurationWindow::_allowSettingsAccess(bool enabled) -{ - ui.paramFileButton->setEnabled(enabled); - ui.servoConfigFileButton->setEnabled(enabled); -} diff --git a/src/ui/OpalLinkConfigurationWindow.h b/src/ui/OpalLinkConfigurationWindow.h deleted file mode 100644 index f29433086..000000000 --- a/src/ui/OpalLinkConfigurationWindow.h +++ /dev/null @@ -1,29 +0,0 @@ -#ifndef OPALLINKCONFIGURATIONWINDOW_H -#define OPALLINKCONFIGURATIONWINDOW_H - -#include -#include - -#include "LinkInterface.h" -#include "ui_OpalLinkSettings.h" -#include "OpalLink.h" - -class OpalLinkConfigurationWindow : public QWidget -{ - Q_OBJECT -public: - explicit OpalLinkConfigurationWindow(OpalLink* link, QWidget *parent = 0, Qt::WindowFlags flags = Qt::Sheet); -signals: - -private slots: - void _linkConnected(void); - void _linkDisconnected(void); - -private: - void _allowSettingsAccess(bool enabled); - - Ui::OpalLinkSettings ui; - OpalLink* link; -}; - -#endif // OPALLINKCONFIGURATIONWINDOW_H diff --git a/src/ui/OpalLinkSettings.ui b/src/ui/OpalLinkSettings.ui deleted file mode 100644 index 419dd0227..000000000 --- a/src/ui/OpalLinkSettings.ui +++ /dev/null @@ -1,173 +0,0 @@ - - - OpalLinkSettings - - - - 0 - 0 - 537 - 250 - - - - OpalLink Configuration - - - - 6 - - - - - Model Instance ID - - - - - - - Opal-RT Parameter File - - - - - - - Servo Configuration File - - - - - - - Qt::Vertical - - - - 431 - 47 - - - - - - - - - - - - - - - - - - - - - - false - - - Change - - - - :/files/images/status/folder-open.svg:/files/images/status/folder-open.svg - - - - - - - false - - - Change - - - - :/files/images/status/folder-open.svg:/files/images/status/folder-open.svg - - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - 101 - - - 200 - - - 101 - - - - - - - - - Delete - - - Delete this link - - - - - Connect - - - Connect this link - - - - - Close - - - Close the configuration window - - - - - - - - - actionClose - triggered() - OpalLinkSettings - close() - - - -1 - -1 - - - 224 - 195 - - - - - -- 2.22.0