Browse Source

Finished porting, successfully tested with UDP MAVLink v1.0 client

QGC4.4
lm 14 years ago
parent
commit
06be988133
  1. 2
      qgroundcontrol.pro
  2. 39
      src/Waypoint.cc
  3. 2
      src/comm/MAVLinkProtocol.cc
  4. 3
      src/ui/HSIDisplay.cc
  5. 8
      src/ui/MainWindow.cc
  6. 15
      src/ui/RadioCalibration/RadioCalibrationWindow.cc
  7. 6
      src/ui/WaypointView.cc
  8. 136
      src/ui/designer/QGCActionButton.cc
  9. 34
      src/ui/designer/QGCActionButton.h
  10. 17
      src/ui/designer/QGCToolWidget.cc
  11. 2
      src/ui/designer/QGCToolWidget.h

2
qgroundcontrol.pro

@ -299,7 +299,6 @@ HEADERS += src/MG.h \ @@ -299,7 +299,6 @@ HEADERS += src/MG.h \
src/ui/uas/QGCUnconnectedInfoWidget.h \
src/ui/designer/QGCToolWidget.h \
src/ui/designer/QGCParamSlider.h \
src/ui/designer/QGCActionButton.h \
src/ui/designer/QGCCommandButton.h \
src/ui/designer/QGCToolWidgetItem.h \
src/ui/QGCMAVLinkLogPlayer.h \
@ -425,7 +424,6 @@ SOURCES += src/main.cc \ @@ -425,7 +424,6 @@ SOURCES += src/main.cc \
src/ui/uas/QGCUnconnectedInfoWidget.cc \
src/ui/designer/QGCToolWidget.cc \
src/ui/designer/QGCParamSlider.cc \
src/ui/designer/QGCActionButton.cc \
src/ui/designer/QGCCommandButton.cc \
src/ui/designer/QGCToolWidgetItem.cc \
src/ui/QGCMAVLinkLogPlayer.cc \

39
src/Waypoint.cc

@ -109,7 +109,8 @@ void Waypoint::setId(quint16 id) @@ -109,7 +109,8 @@ void Waypoint::setId(quint16 id)
void Waypoint::setX(double x)
{
if (this->x != x && (this->frame == MAV_FRAME_LOCAL)) {
if (!isinf(x) && !isnan(x) && ((this->frame == MAV_FRAME_LOCAL_NED) || (this->frame == MAV_FRAME_LOCAL_ENU)))
{
this->x = x;
emit changed(this);
}
@ -117,7 +118,8 @@ void Waypoint::setX(double x) @@ -117,7 +118,8 @@ void Waypoint::setX(double x)
void Waypoint::setY(double y)
{
if (this->y != y && (this->frame == MAV_FRAME_LOCAL)) {
if (!isinf(y) && !isnan(y) && ((this->frame == MAV_FRAME_LOCAL_NED) || (this->frame == MAV_FRAME_LOCAL_ENU)))
{
this->y = y;
emit changed(this);
}
@ -125,7 +127,8 @@ void Waypoint::setY(double y) @@ -125,7 +127,8 @@ void Waypoint::setY(double y)
void Waypoint::setZ(double z)
{
if (this->z != z && (this->frame == MAV_FRAME_LOCAL)) {
if (!isinf(z) && !isnan(z) && ((this->frame == MAV_FRAME_LOCAL_NED) || (this->frame == MAV_FRAME_LOCAL_ENU)))
{
this->z = z;
emit changed(this);
}
@ -133,7 +136,8 @@ void Waypoint::setZ(double z) @@ -133,7 +136,8 @@ void Waypoint::setZ(double z)
void Waypoint::setLatitude(double lat)
{
if (this->x != lat && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT))) {
if (this->x != lat && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT)))
{
this->x = lat;
emit changed(this);
}
@ -141,7 +145,8 @@ void Waypoint::setLatitude(double lat) @@ -141,7 +145,8 @@ void Waypoint::setLatitude(double lat)
void Waypoint::setLongitude(double lon)
{
if (this->y != lon && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT))) {
if (this->y != lon && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT)))
{
this->y = lon;
emit changed(this);
}
@ -149,7 +154,8 @@ void Waypoint::setLongitude(double lon) @@ -149,7 +154,8 @@ void Waypoint::setLongitude(double lon)
void Waypoint::setAltitude(double altitude)
{
if (this->z != altitude && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT))) {
if (this->z != altitude && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT)))
{
this->z = altitude;
emit changed(this);
}
@ -157,7 +163,8 @@ void Waypoint::setAltitude(double altitude) @@ -157,7 +163,8 @@ void Waypoint::setAltitude(double altitude)
void Waypoint::setYaw(int yaw)
{
if (this->yaw != yaw) {
if (this->yaw != yaw)
{
this->yaw = yaw;
emit changed(this);
}
@ -165,7 +172,8 @@ void Waypoint::setYaw(int yaw) @@ -165,7 +172,8 @@ void Waypoint::setYaw(int yaw)
void Waypoint::setYaw(double yaw)
{
if (this->yaw != yaw) {
if (this->yaw != yaw)
{
this->yaw = yaw;
emit changed(this);
}
@ -173,7 +181,8 @@ void Waypoint::setYaw(double yaw) @@ -173,7 +181,8 @@ void Waypoint::setYaw(double yaw)
void Waypoint::setAction(int action)
{
if (this->action != (MAV_CMD)action) {
if (this->action != (MAV_CMD)action)
{
this->action = (MAV_CMD)action;
emit changed(this);
}
@ -205,7 +214,8 @@ void Waypoint::setAutocontinue(bool autoContinue) @@ -205,7 +214,8 @@ void Waypoint::setAutocontinue(bool autoContinue)
void Waypoint::setCurrent(bool current)
{
if (this->current != current) {
if (this->current != current)
{
this->current = current;
emit changed(this);
}
@ -213,7 +223,8 @@ void Waypoint::setCurrent(bool current) @@ -213,7 +223,8 @@ void Waypoint::setCurrent(bool current)
void Waypoint::setAcceptanceRadius(double radius)
{
if (this->param2 != radius) {
if (this->param2 != radius)
{
this->param2 = radius;
emit changed(this);
}
@ -223,7 +234,8 @@ void Waypoint::setParam1(double param1) @@ -223,7 +234,8 @@ void Waypoint::setParam1(double param1)
{
//qDebug() << "SENDER:" << QObject::sender();
//qDebug() << "PARAM1 SET REQ:" << param1;
if (this->param1 != param1) {
if (this->param1 != param1)
{
this->param1 = param1;
emit changed(this);
}
@ -231,7 +243,8 @@ void Waypoint::setParam1(double param1) @@ -231,7 +243,8 @@ void Waypoint::setParam1(double param1)
void Waypoint::setParam2(double param2)
{
if (this->param2 != param2) {
if (this->param2 != param2)
{
this->param2 = param2;
emit changed(this);
}

2
src/comm/MAVLinkProtocol.cc

@ -395,7 +395,7 @@ void MAVLinkProtocol::sendHeartbeat() @@ -395,7 +395,7 @@ void MAVLinkProtocol::sendHeartbeat()
{
if (m_heartbeatsEnabled) {
mavlink_message_t beat;
mavlink_msg_heartbeat_pack(getSystemId(), getComponentId(),&beat, OCU, MAV_AUTOPILOT_GENERIC);
mavlink_msg_heartbeat_pack(getSystemId(), getComponentId(),&beat, MAV_TYPE_OCU, MAV_CLASS_INVALID);
sendMessage(beat);
}
if (m_authEnabled) {

3
src/ui/HSIDisplay.cc

@ -792,7 +792,8 @@ void HSIDisplay::drawWaypoints(QPainter& painter) @@ -792,7 +792,8 @@ void HSIDisplay::drawWaypoints(QPainter& painter)
for (int i = 0; i < list.size(); i++) {
QPointF in;
if (list.at(i)->getFrame() == MAV_FRAME_LOCAL) {
if (list.at(i)->getFrame() == MAV_FRAME_LOCAL_NED)
{
// Do not transform
in = QPointF(list.at(i)->getX(), list.at(i)->getY());
} else {

8
src/ui/MainWindow.cc

@ -1629,7 +1629,7 @@ void MainWindow::UASCreated(UASInterface* uas) @@ -1629,7 +1629,7 @@ void MainWindow::UASCreated(UASInterface* uas)
}
switch (uas->getAutopilotType()) {
case (MAV_AUTOPILOT_SLUGS): {
case (MAV_CLASS_SLUGS): {
// Build Slugs Widgets
buildSlugsWidgets();
@ -1667,9 +1667,9 @@ void MainWindow::UASCreated(UASInterface* uas) @@ -1667,9 +1667,9 @@ void MainWindow::UASCreated(UASInterface* uas)
}
break;
default:
case (MAV_AUTOPILOT_GENERIC):
case (MAV_AUTOPILOT_ARDUPILOTMEGA):
case (MAV_AUTOPILOT_PIXHAWK): {
case (MAV_CLASS_GENERIC):
case (MAV_CLASS_ARDUPILOTMEGA):
case (MAV_CLASS_PIXHAWK): {
// Build Pixhawk Widgets
buildPxWidgets();

15
src/ui/RadioCalibration/RadioCalibrationWindow.cc

@ -270,13 +270,14 @@ void RadioCalibrationWindow::send() @@ -270,13 +270,14 @@ void RadioCalibrationWindow::send()
void RadioCalibrationWindow::request()
{
qDebug() << __FILE__ << __LINE__ << "READ FROM UAV";
UAS *uas = dynamic_cast<UAS*>(UASManager::instance()->getUASForId(uasId));
if (uas) {
mavlink_message_t msg;
mavlink_msg_action_pack(uasId, 0, &msg, 0, 0, ::MAV_ACTION_CALIBRATE_RC);
uas->sendMessage(msg);
}
// FIXME MAVLINKV10PORTINGNEEDED
// qDebug() << __FILE__ << __LINE__ << "READ FROM UAV";
// UAS *uas = dynamic_cast<UAS*>(UASManager::instance()->getUASForId(uasId));
// if (uas) {
// mavlink_message_t msg;
// mavlink_msg_action_pack(uasId, 0, &msg, 0, 0, ::MAV_ACTION_CALIBRATE_RC);
// uas->sendMessage(msg);
// }
}
void RadioCalibrationWindow::receive(const QPointer<RadioCalibrationData>& radio)

6
src/ui/WaypointView.cc

@ -52,7 +52,7 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) : @@ -52,7 +52,7 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) :
// add frames
m_ui->comboBox_frame->addItem("Abs. Alt/Global",MAV_FRAME_GLOBAL);
m_ui->comboBox_frame->addItem("Rel. Alt/Global", MAV_FRAME_GLOBAL_RELATIVE_ALT);
m_ui->comboBox_frame->addItem("Local/Abs. Alt.",MAV_FRAME_LOCAL);
m_ui->comboBox_frame->addItem("Local/Abs. Alt.",MAV_FRAME_LOCAL_NED);
m_ui->comboBox_frame->addItem("Mission",MAV_FRAME_MISSION);
// Initialize view correctly
@ -322,7 +322,7 @@ void WaypointView::updateFrameView(int frame) @@ -322,7 +322,7 @@ void WaypointView::updateFrameView(int frame)
m_ui->comboBox_frame->show();
m_ui->customActionWidget->hide();
break;
case MAV_FRAME_LOCAL:
case MAV_FRAME_LOCAL_NED:
m_ui->lonSpinBox->hide();
m_ui->latSpinBox->hide();
m_ui->altSpinBox->hide();
@ -386,7 +386,7 @@ void WaypointView::updateValues() @@ -386,7 +386,7 @@ void WaypointView::updateValues()
updateFrameView(frame);
}
switch(frame) {
case MAV_FRAME_LOCAL: {
case MAV_FRAME_LOCAL_NED: {
if (m_ui->posNSpinBox->value() != wp->getX()) {
m_ui->posNSpinBox->setValue(wp->getX());
}

136
src/ui/designer/QGCActionButton.cc

@ -1,136 +0,0 @@ @@ -1,136 +0,0 @@
#include "QGCActionButton.h"
#include "ui_QGCActionButton.h"
#include "MAVLinkProtocol.h"
#include "UASManager.h"
const char* kActionLabels[MAV_ACTION_NB] = {
"HOLD",
"START MOTORS",
"LAUNCH",
"RETURN",
"EMERGENCY LAND",
"EMERGENCY KILL",
"CONFIRM KILL",
"CONTINUE",
"STOP MOTORS",
"HALT",
"SHUTDOWN",
"REBOOT",
"SET MANUAL",
"SET AUTO",
"READ STORAGE",
"WRITE STORAGE",
"CALIBRATE RC",
"CALIBRATE GYRO",
"CALIBRATE MAG",
"CALIBRATE ACC",
"CALIBRATE PRESSURE",
"START REC",
"PAUSE REC",
"STOP REC",
"TAKEOFF",
"NAVIGATE",
"LAND",
"LOITER",
"SET ORIGIN",
"RELAY ON",
//"RELAY OFF",
//"GET IMAGE",
//"START VIDEO",
//"STOP VIDEO",
"RESET MAP",
"RESET PLAN"
};
QGCActionButton::QGCActionButton(QWidget *parent) :
QGCToolWidgetItem("Button", parent),
ui(new Ui::QGCActionButton),
uas(NULL)
{
ui->setupUi(this);
connect(ui->actionButton, SIGNAL(clicked()), this, SLOT(sendAction()));
connect(ui->editFinishButton, SIGNAL(clicked()), this, SLOT(endEditMode()));
connect(ui->editButtonName, SIGNAL(textChanged(QString)), this, SLOT(setActionButtonName(QString)));
connect(ui->editActionComboBox, SIGNAL(currentIndexChanged(QString)), ui->nameLabel, SLOT(setText(QString)));
// Hide all edit items
ui->editActionComboBox->hide();
ui->editActionsRefreshButton->hide();
ui->editFinishButton->hide();
ui->editNameLabel->hide();
ui->editButtonName->hide();
// add action labels to combobox
for (int i = 0; i < MAV_ACTION_NB; i++) {
ui->editActionComboBox->addItem(kActionLabels[i]);
}
}
QGCActionButton::~QGCActionButton()
{
delete ui;
}
void QGCActionButton::sendAction()
{
if (QGCToolWidgetItem::uas) {
MAV_ACTION action = static_cast<MAV_ACTION>(
ui->editActionComboBox->currentIndex());
QGCToolWidgetItem::uas->setAction(action);
} else {
qDebug() << __FILE__ << __LINE__ << "NO UAS SET, DOING NOTHING";
}
}
void QGCActionButton::setActionButtonName(QString text)
{
ui->actionButton->setText(text);
}
void QGCActionButton::startEditMode()
{
ui->editActionComboBox->show();
ui->editActionsRefreshButton->show();
ui->editFinishButton->show();
ui->editNameLabel->show();
ui->editButtonName->show();
isInEditMode = true;
}
void QGCActionButton::endEditMode()
{
ui->editActionComboBox->hide();
ui->editActionsRefreshButton->hide();
ui->editFinishButton->hide();
ui->editNameLabel->hide();
ui->editButtonName->hide();
// Write to settings
emit editingFinished();
isInEditMode = false;
}
void QGCActionButton::writeSettings(QSettings& settings)
{
settings.setValue("TYPE", "BUTTON");
settings.setValue("QGC_ACTION_BUTTON_DESCRIPTION", ui->nameLabel->text());
settings.setValue("QGC_ACTION_BUTTON_BUTTONTEXT", ui->actionButton->text());
settings.setValue("QGC_ACTION_BUTTON_ACTIONID", ui->editActionComboBox->currentIndex());
settings.sync();
}
void QGCActionButton::readSettings(const QSettings& settings)
{
ui->editNameLabel->setText(settings.value("QGC_ACTION_BUTTON_DESCRIPTION", "ERROR LOADING BUTTON").toString());
ui->editButtonName->setText(settings.value("QGC_ACTION_BUTTON_BUTTONTEXT", "UNKNOWN").toString());
ui->editActionComboBox->setCurrentIndex(settings.value("QGC_ACTION_BUTTON_ACTIONID", 0).toInt());
ui->nameLabel->setText(settings.value("QGC_ACTION_BUTTON_DESCRIPTION", "ERROR LOADING BUTTON").toString());
ui->actionButton->setText(settings.value("QGC_ACTION_BUTTON_BUTTONTEXT", "UNKNOWN").toString());
ui->editActionComboBox->setCurrentIndex(settings.value("QGC_ACTION_BUTTON_ACTIONID", 0).toInt());
qDebug() << "DONE READING SETTINGS";
}

34
src/ui/designer/QGCActionButton.h

@ -1,34 +0,0 @@ @@ -1,34 +0,0 @@
#ifndef QGCACTIONBUTTON_H
#define QGCACTIONBUTTON_H
#include "QGCToolWidgetItem.h"
namespace Ui
{
class QGCActionButton;
}
class UASInterface;
class QGCActionButton : public QGCToolWidgetItem
{
Q_OBJECT
public:
explicit QGCActionButton(QWidget *parent = 0);
~QGCActionButton();
public slots:
void sendAction();
void setActionButtonName(QString text);
void startEditMode();
void endEditMode();
void writeSettings(QSettings& settings);
void readSettings(const QSettings& settings);
private:
Ui::QGCActionButton *ui;
UASInterface* uas;
};
#endif // QGCACTIONBUTTON_H

17
src/ui/designer/QGCToolWidget.cc

@ -11,7 +11,6 @@ @@ -11,7 +11,6 @@
#include <QDesktopServices>
#include "QGCParamSlider.h"
#include "QGCActionButton.h"
#include "QGCCommandButton.h"
#include "UASManager.h"
@ -129,10 +128,7 @@ void QGCToolWidget::loadSettings(QSettings& settings) @@ -129,10 +128,7 @@ void QGCToolWidget::loadSettings(QSettings& settings)
QString type = settings.value("TYPE", "UNKNOWN").toString();
if (type != "UNKNOWN") {
QGCToolWidgetItem* item = NULL;
if (type == "BUTTON") {
item = new QGCActionButton(this);
qDebug() << "CREATED BUTTON";
} else if (type == "COMMANDBUTTON") {
if (type == "COMMANDBUTTON") {
item = new QGCCommandButton(this);
qDebug() << "CREATED COMMANDBUTTON";
} else if (type == "SLIDER") {
@ -303,17 +299,6 @@ void QGCToolWidget::addParam() @@ -303,17 +299,6 @@ void QGCToolWidget::addParam()
slider->startEditMode();
}
void QGCToolWidget::addAction()
{
QGCActionButton* button = new QGCActionButton(this);
if (ui->hintLabel) {
ui->hintLabel->deleteLater();
ui->hintLabel = NULL;
}
toolLayout->addWidget(button);
button->startEditMode();
}
void QGCToolWidget::addCommand()
{
QGCCommandButton* button = new QGCCommandButton(this);

2
src/ui/designer/QGCToolWidget.h

@ -82,8 +82,6 @@ protected: @@ -82,8 +82,6 @@ protected:
protected slots:
void addParam();
/** @deprecated */
void addAction();
void addCommand();
void setTitle();
void setTitle(QString title);

Loading…
Cancel
Save