Skip to content

Q
qgroundcontrol
Commit 88450aa6 authored by Don Gagne's avatar Don Gagne
Browse files
Options

Merge pull request #1322 from DonLakeFlyer/UnusedCode 

Remove unused code
parents 4e74c374 379d3819
Hide whitespace changes
Inline Side-by-side
Showing with 0 additions and 1017 deletions
src/comm/OpalLink.cc deleted 100644 → 0
View file @ 4e74c374
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
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 <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Implementation of class OpalLink
* @author Bryan Godbolt <godbolt@ualberta.ca>
*/
#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<QByteArray>()),
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))&& i<length); ++i);
/* perform the appropriate action */
if (decodeSuccess) {
switch(msg.msgid) {
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
qDebug() << "OpalLink::writeBytes(): request params";
mavlink_message_t param;
OpalRT::ParameterList::const_iterator paramIter;
for (paramIter = params->begin(); paramIter != params->end(); ++paramIter) {
mavlink_msg_param_value_pack(systemID,
(*paramIter).getComponentID(),
&param,
(*paramIter).getParamID().toInt8_t(),
(static_cast<OpalRT::Parameter>(*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, &param);
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(),
&paramMsg,
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<bool>(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<uint16_t>(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<uint8_t>((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<uint8_t>(norm*255);
break;
}
}
int8_t OpalLink::rescaleControllerOutput(double raw)
{
return static_cast<int8_t>((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;
}
src/comm/OpalLink.h deleted 100644 → 0
View file @ 4e74c374
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
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 <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Connection to OpalRT
* @author Bryan Godbolt <godbolt@ualberta.ca>
*/
#ifndef OPALLINK_H
#define OPALLINK_H
#include <QMutex>
#include <QDebug>
#include <QTextStreamManipulator>
#include <QTimer>
#include <QQueue>
#include <QByteArray>
#include <QObject>
#include <stdlib.h>
#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 <godbolt@ualberta.ca>
* @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<int>(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<unsigned short>(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<QByteArray>* 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
src/comm/OpalRT.cc deleted 100644 → 0
View file @ 4e74c374
#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;
}
}
src/comm/OpalRT.h deleted 100644 → 0
View file @ 4e74c374
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
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 <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Types used for Opal-RT interface configuration
* @author Bryan Godbolt <godbolt@ualberta.ca>
*/
#ifndef OPALRT_H
#define OPALRT_H
#include <QString>
#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
src/ui/OpalLinkConfigurationWindow.cc deleted 100644 → 0
View file @ 4e74c374
#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);
}
src/ui/OpalLinkConfigurationWindow.h deleted 100644 → 0
View file @ 4e74c374
#ifndef OPALLINKCONFIGURATIONWINDOW_H
#define OPALLINKCONFIGURATIONWINDOW_H
#include <QWidget>
#include <QDebug>
#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
src/ui/OpalLinkSettings.ui deleted 100644 → 0
View file @ 4e74c374
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>OpalLinkSettings</class>
<widget class="QWidget" name="OpalLinkSettings">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>537</width>
<height>250</height>
</rect>
</property>
<property name="windowTitle">
<string>OpalLink Configuration</string>
</property>
<layout class="QGridLayout" name="gridLayout" rowstretch="0,0,0,0">
<property name="margin">
<number>6</number>
</property>
<item row="0" column="0">
<widget class="QLabel" name="portlabel">
<property name="text">
<string>Model Instance ID</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="portlabel_2">
<property name="text">
<string>Opal-RT Parameter File</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="portlabel_3">
<property name="text">
<string>Servo Configuration File</string>
</property>
</widget>
</item>
<item row="3" column="0" colspan="2">
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>431</width>
<height>47</height>
</size>
</property>
</spacer>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_2">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QPushButton" name="paramFileButton">
<property name="enabled">
<bool>false</bool>
</property>
<property name="text">
<string>Change</string>
</property>
<property name="icon">
<iconset resource="../../qgroundcontrol.qrc">
<normaloff>:/files/images/status/folder-open.svg</normaloff>:/files/images/status/folder-open.svg</iconset>
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QPushButton" name="servoConfigFileButton">
<property name="enabled">
<bool>false</bool>
</property>
<property name="text">
<string>Change</string>
</property>
<property name="icon">
<iconset resource="../../qgroundcontrol.qrc">
<normaloff>:/files/images/status/folder-open.svg</normaloff>:/files/images/status/folder-open.svg</iconset>
</property>
</widget>
</item>
<item row="0" column="1" colspan="2">
<layout class="QHBoxLayout" name="horizontalLayout">
<item>
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QSpinBox" name="opalInstIDSpinBox">
<property name="minimum">
<number>101</number>
</property>
<property name="maximum">
<number>200</number>
</property>
<property name="value">
<number>101</number>
</property>
</widget>
</item>
</layout>
</item>
</layout>
<action name="actionDelete">
<property name="text">
<string>Delete</string>
</property>
<property name="toolTip">
<string>Delete this link</string>
</property>
</action>
<action name="actionConnect">
<property name="text">
<string>Connect</string>
</property>
<property name="toolTip">
<string>Connect this link</string>
</property>
</action>
<action name="actionClose">
<property name="text">
<string>Close</string>
</property>
<property name="toolTip">
<string>Close the configuration window</string>
</property>
</action>
</widget>
<resources>
<include location="../../qgroundcontrol.qrc"/>
</resources>
<connections>
<connection>
<sender>actionClose</sender>
<signal>triggered()</signal>
<receiver>OpalLinkSettings</receiver>
<slot>close()</slot>
<hints>
<hint type="sourcelabel">
<x>-1</x>
<y>-1</y>
</hint>
<hint type="destinationlabel">
<x>224</x>
<y>195</y>
</hint>
</hints>
</connection>
</connections>
</ui>
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment