Commit e2261020 authored by LM's avatar LM

Added controller output support

parent 90e3dbf4
......@@ -17,7 +17,6 @@
#include <QSettings>
#include <QDesktopServices>
//#include "MG.h"
#include "MAVLinkProtocol.h"
#include "UASInterface.h"
#include "UASManager.h"
......@@ -28,7 +27,6 @@
#include "ArduPilotMegaMAV.h"
#include "configuration.h"
#include "LinkManager.h"
//#include "MainWindow.h"
#include "QGCMAVLink.h"
#include "QGCMAVLinkUASFactory.h"
#include "QGC.h"
......@@ -182,14 +180,14 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
unsigned int decodeState = mavlink_parse_char(link->getId(), (uint8_t)(b.at(position)), &message, &status);
if (decodeState == 1) {
#ifdef MAVLINK_MESSAGE_LENGTHS
const uint8_t message_lengths[] = MAVLINK_MESSAGE_LENGTHS;
if (message.msgid >= sizeof(message_lengths) ||
message.len != message_lengths[message.msgid]) {
qDebug() << "MAVLink message " << message.msgid << " length incorrect (was " << message.len << " expected " << message_lengths[message.msgid] << ")";
continue;
}
#endif
//#ifdef MAVLINK_MESSAGE_LENGTHS
// const uint8_t message_lengths[] = MAVLINK_MESSAGE_LENGTHS;
// if (message.msgid >= sizeof(message_lengths) ||
// message.len != message_lengths[message.msgid]) {
// qDebug() << "MAVLink message " << message.msgid << " length incorrect (was " << message.len << " expected " << message_lengths[message.msgid] << ")";
// continue;
// }
//#endif
// Log data
if (m_loggingEnabled && m_logfile) {
const int len = MAVLINK_MAX_PACKET_LEN+sizeof(quint64);
......
......@@ -824,26 +824,16 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
case MAVLINK_MSG_ID_DEBUG:
emit valueChanged(uasId, QString("debug ") + QString::number(mavlink_msg_debug_get_ind(&message)), "raw", mavlink_msg_debug_get_value(&message), MG::TIME::getGroundTimeNow());
break;
case MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT:
case MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT:
{
mavlink_attitude_controller_output_t out;
mavlink_msg_attitude_controller_output_decode(&message, &out);
quint64 time = MG::TIME::getGroundTimeNowUsecs();
emit attitudeThrustSetPointChanged(this, out.roll/127.0f, out.pitch/127.0f, out.yaw/127.0f, (uint8_t)out.thrust, time);
emit valueChanged(uasId, "att control roll", "raw", out.roll, time/1000.0f);
emit valueChanged(uasId, "att control pitch", "raw", out.pitch, time/1000.0f);
emit valueChanged(uasId, "att control yaw", "raw", out.yaw, time/1000.0f);
}
break;
case MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT:
{
mavlink_position_controller_output_t out;
mavlink_msg_position_controller_output_decode(&message, &out);
quint64 time = MG::TIME::getGroundTimeNow();
//emit positionSetPointsChanged(uasId, out.x/127.0f, out.y/127.0f, out.z/127.0f, out.yaw, time);
emit valueChanged(uasId, "pos control x", "raw", out.x, time);
emit valueChanged(uasId, "pos control y", "raw", out.y, time);
emit valueChanged(uasId, "pos control z", "raw", out.z, time);
mavlink_roll_pitch_yaw_thrust_setpoint_t out;
mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(&message, &out);
quint64 time = getUnixTime(out.time_ms*1000);
emit attitudeThrustSetPointChanged(this, out.roll, out.pitch, out.yaw, out.thrust, time);
emit valueChanged(uasId, "att control roll", "rad", out.roll, time);
emit valueChanged(uasId, "att control pitch", "rad", out.pitch, time);
emit valueChanged(uasId, "att control yaw", "rad", out.yaw, time);
emit valueChanged(uasId, "att control thrust", "0-1", out.thrust, time);
}
break;
case MAVLINK_MSG_ID_WAYPOINT_COUNT:
......
......@@ -55,6 +55,7 @@ void QGCToolBar::setActiveUAS(UASInterface* active)
disconnect(mav, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
disconnect(mav, SIGNAL(nameChanged(QString)), this, SLOT(updateName(QString)));
disconnect(mav, SIGNAL(systemTypeSet(UASInterface*,uint)), this, SLOT(setSystemType(UASInterface*,uint)));
disconnect(mav, SIGNAL(textMessageReceived(int,int,int,QString)), this, SLOT(receiveTextMessage(int,int,int,QString)));
}
// Connect new system
......@@ -63,6 +64,7 @@ void QGCToolBar::setActiveUAS(UASInterface* active)
connect(active, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
connect(active, SIGNAL(nameChanged(QString)), this, SLOT(updateName(QString)));
connect(active, SIGNAL(systemTypeSet(UASInterface*,uint)), this, SLOT(setSystemType(UASInterface*,uint)));
connect(active, SIGNAL(textMessageReceived(int,int,int,QString)), this, SLOT(receiveTextMessage(int,int,int,QString)));
// Update all values once
nameLabel->setText(mav->getUASName());
......@@ -81,6 +83,7 @@ void QGCToolBar::createCustomWidgets()
stateLabel = new QLabel("------", this);
wpLabel = new QLabel("---", this);
distlabel = new QLabel("--- ---- m", this);
messageLabel = new QLabel("No system messages.", this);
//symbolButton->setIcon(":");
symbolButton->setStyleSheet("QWidget { background-color: #050508; color: #DDDDDF; background-clip: border; } QToolButton { font-weight: bold; font-size: 12px; border: 0px solid #999999; border-radius: 5px; min-width:22px; max-width: 22px; min-height: 22px; max-height: 22px; padding: 0px; margin: 0px; background-color: none; }");
addWidget(symbolButton);
......@@ -89,6 +92,7 @@ void QGCToolBar::createCustomWidgets()
addWidget(stateLabel);
addWidget(wpLabel);
addWidget(distlabel);
addWidget(messageLabel);
toggleLoggingAction = new QAction(QIcon(":"), "Start Logging", this);
logReplayAction = new QAction(QIcon(":"), "Start Replay", this);
......@@ -154,6 +158,14 @@ void QGCToolBar::setSystemType(UASInterface* uas, unsigned int systemType)
}
}
void QGCToolBar::receiveTextMessage(int uasid, int componentid, int severity, QString text)
{
Q_UNUSED(uasid);
Q_UNUSED(componentid);
Q_UNUSED(severity);
messageLabel->setText(text);
}
QGCToolBar::~QGCToolBar()
{
delete toggleLoggingAction;
......
......@@ -50,6 +50,8 @@ public slots:
void updateName(const QString& name);
/** @brief Set the MAV system type */
void setSystemType(UASInterface* uas, unsigned int systemType);
/** @brief Received system text message */
void receiveTextMessage(int uasid, int componentid, int severity, QString text);
protected:
void createCustomWidgets();
......@@ -63,7 +65,7 @@ protected:
QLabel* stateLabel;
QLabel* wpLabel;
QLabel* distlabel;
QLabel* messageLabel;
};
#endif // QGCTOOLBAR_H
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