Commit 585cc5b3 authored by LM's avatar LM

Cleaned up debug vects, fixed a few bugs in radio control widget

parent 381b3ed7
......@@ -890,10 +890,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
case MAVLINK_MSG_ID_SCALED_PRESSURE:
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
case MAVLINK_MSG_ID_OPTICAL_FLOW:
break;
case MAVLINK_MSG_ID_DEBUG_VECT:
{
mavlink_debug_vect_t debug;
mavlink_msg_debug_vect_decode(&message, &debug);
debug.name[MAVLINK_MSG_DEBUG_VECT_FIELD_NAME_LEN-1] = '\0';
QString name(debug.name);
quint64 time = getUnixTime(debug.time_usec);
emit valueChanged(uasId, name+".x", "raw", debug.x, time);
......
......@@ -20,6 +20,8 @@ MAVLinkDecoder::MAVLinkDecoder(MAVLinkProtocol* protocol, QObject *parent) :
messageFilter.insert(MAVLINK_MSG_ID_MISSION_ITEM, false);
messageFilter.insert(MAVLINK_MSG_ID_MISSION_COUNT, false);
messageFilter.insert(MAVLINK_MSG_ID_MISSION_ACK, false);
messageFilter.insert(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, false);
messageFilter.insert(MAVLINK_MSG_ID_NAMED_VALUE_INT, false);
textMessageFilter.insert(MAVLINK_MSG_ID_DEBUG, false);
textMessageFilter.insert(MAVLINK_MSG_ID_DEBUG_VECT, false);
......
......@@ -77,21 +77,26 @@ QGCRemoteControlView::~QGCRemoteControlView()
void QGCRemoteControlView::setUASId(int id)
{
// Clear channel count
raw.clear();
normalized.clear();
if (uasId != -1) {
UASInterface* uas = UASManager::instance()->getUASForId(id);
if (uas) {
if (uasId != -1)
{
UASInterface* uas = UASManager::instance()->getUASForId(uasId);
if (uas)
{
// The UAS exists, disconnect any existing connections
disconnect(uas, SIGNAL(remoteControlChannelRawChanged(int,float,float)), this, SLOT(setChannel(int,float,float)));
disconnect(uas, SIGNAL(remoteControlRSSIChanged(float)), this, SLOT(setRemoteRSSI(float)));
disconnect(uas, SIGNAL(radioCalibrationRawReceived(const QPointer<RadioCalibrationData>&)), calibrationWindow, SLOT(receive(const QPointer<RadioCalibrationData>&)));
disconnect(uas, SIGNAL(remoteControlChannelRawChanged(int,float)), calibrationWindow, SLOT(setChannel(int,float)));
disconnect(uas, SIGNAL(remoteControlChannelScaledChanged(int,float)), this, SLOT(setChannelScaled(int,float)));
}
}
// Clear channel count
raw.clear();
raw.resize(0);
normalized.clear();
normalized.resize(0);
foreach (QLabel* label, rawLabels)
{
label->deleteLater();
......@@ -103,11 +108,14 @@ void QGCRemoteControlView::setUASId(int id)
}
rawLabels.clear();
rawLabels.resize(0);
progressBars.clear();
progressBars.resize(0);
// Connect the new UAS
UASInterface* newUAS = UASManager::instance()->getUASForId(id);
if (newUAS) {
if (newUAS)
{
// New UAS exists, connect
nameLabel->setText(QString("RC Input of %1").arg(newUAS->getUASName()));
calibrationWindow->setUASId(id);
......@@ -140,11 +148,10 @@ void QGCRemoteControlView::setChannelRaw(int channelId, float raw)
void QGCRemoteControlView::setChannelScaled(int channelId, float normalized)
{
if (this->raw.size() <= channelId) // using raw vector as size indicator
if (this->normalized.size() <= channelId) // using raw vector as size indicator
{
// This is a new channel, append it
this->normalized.append(normalized);
this->raw.append(0);
appendChannelWidget(channelId);
updated = true;
}
......
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