From 965cd4e68c3900f1aba47be54d7281e5355262a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 4 Apr 2013 18:03:24 +0200 Subject: [PATCH 01/13] Tiny typo fixes in HIL --- src/comm/QGCXPlaneLink.cc | 8 ++++---- src/uas/UAS.cc | 16 ++++++++++++++-- src/ui/uas/UASControlWidget.cc | 12 ++++++------ 3 files changed, 24 insertions(+), 12 deletions(-) diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc index c45a67e..c2078ed 100644 --- a/src/comm/QGCXPlaneLink.cc +++ b/src/comm/QGCXPlaneLink.cc @@ -678,8 +678,8 @@ bool QGCXPlaneLink::disconnectSimulation() disconnect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t))); disconnect(mav, SIGNAL(hilActuatorsChanged(uint64_t, float, float, float, float, float, float, float, float)), this, SLOT(updateActuators(uint64_t,float,float,float,float,float,float,float,float))); - disconnect(this, SIGNAL(hilStateChanged(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t))); - disconnect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,int)), mav, SLOT(sendHilGps(uint64_t, double, double, double, int, float, float, float, float, int))); + disconnect(this, SIGNAL(hilStateChanged(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t))); + disconnect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,int)), mav, SLOT(sendHilGps(quint64, double, double, double, int, float, float, float, float, int))); disconnect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint16)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint16))); UAS* uas = dynamic_cast(mav); @@ -862,8 +862,8 @@ bool QGCXPlaneLink::connectSimulation() connect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t))); connect(mav, SIGNAL(hilActuatorsChanged(uint64_t, float, float, float, float, float, float, float, float)), this, SLOT(updateActuators(uint64_t,float,float,float,float,float,float,float,float))); - connect(this, SIGNAL(hilStateChanged(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t))); - connect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,int)), mav, SLOT(sendHilGps(uint64_t, double, double, double, int, float, float, float, float, int))); + connect(this, SIGNAL(hilStateChanged(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t))); + connect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,int)), mav, SLOT(sendHilGps(quint64, double, double, double, int, float, float, float, float, int))); connect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint16)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint16))); UAS* uas = dynamic_cast(mav); diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 9c2d6ad..a03020c 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -1691,10 +1691,22 @@ QList UAS::getComponentIds() void UAS::setMode(int mode) { //this->mode = mode; //no call assignament, update receive message from UAS + + // Strip armed / disarmed call, this is not relevant for setting the mode + uint8_t newMode = mode; + newMode &= (~(uint8_t)MAV_MODE_FLAG_SAFETY_ARMED); + // Now set current state (request no change) + newMode |= (uint8_t)(this->mode) & (uint8_t)(MAV_MODE_FLAG_SAFETY_ARMED); + + // Strip HIL part, replace it with current system state + newMode &= (~(uint8_t)MAV_MODE_FLAG_HIL_ENABLED); + // Now set current state (request no change) + newMode |= (uint8_t)(this->mode) & (uint8_t)(MAV_MODE_FLAG_HIL_ENABLED); + mavlink_message_t msg; - mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, (uint8_t)mode, (uint16_t)navMode); + mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, newMode, (uint16_t)navMode); sendMessage(msg); - qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << (uint8_t)mode; + qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << newMode; } /** diff --git a/src/ui/uas/UASControlWidget.cc b/src/ui/uas/UASControlWidget.cc index 9d970ee..6632aa3 100644 --- a/src/ui/uas/UASControlWidget.cc +++ b/src/ui/uas/UASControlWidget.cc @@ -51,12 +51,12 @@ UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent), connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*))); ui.modeComboBox->clear(); - ui.modeComboBox->insertItem(0, UAS::getShortModeTextFor(MAV_MODE_PREFLIGHT), MAV_MODE_PREFLIGHT); - ui.modeComboBox->insertItem(1, UAS::getShortModeTextFor((MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED)), (MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED)); - ui.modeComboBox->insertItem(2, UAS::getShortModeTextFor(MAV_MODE_FLAG_MANUAL_INPUT_ENABLED), MAV_MODE_FLAG_MANUAL_INPUT_ENABLED); - ui.modeComboBox->insertItem(3, UAS::getShortModeTextFor((MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED)), (MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED)); - ui.modeComboBox->insertItem(4, UAS::getShortModeTextFor((MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED)), (MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED)); - ui.modeComboBox->insertItem(5, UAS::getShortModeTextFor((MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_TEST_ENABLED)), (MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_TEST_ENABLED)); + ui.modeComboBox->insertItem(0, UAS::getShortModeTextFor(MAV_MODE_PREFLIGHT).remove(0, 2), MAV_MODE_PREFLIGHT); + ui.modeComboBox->insertItem(1, UAS::getShortModeTextFor((MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED)).remove(0, 2), (MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED)); + ui.modeComboBox->insertItem(2, UAS::getShortModeTextFor(MAV_MODE_FLAG_MANUAL_INPUT_ENABLED).remove(0, 2), MAV_MODE_FLAG_MANUAL_INPUT_ENABLED); + ui.modeComboBox->insertItem(3, UAS::getShortModeTextFor((MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED)).remove(0, 2), (MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED)); + ui.modeComboBox->insertItem(4, UAS::getShortModeTextFor((MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED)).remove(0, 2), (MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED)); + ui.modeComboBox->insertItem(5, UAS::getShortModeTextFor((MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_TEST_ENABLED)).remove(0, 2), (MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_TEST_ENABLED)); connect(ui.modeComboBox, SIGNAL(activated(int)), this, SLOT(setMode(int))); connect(ui.setModeButton, SIGNAL(clicked()), this, SLOT(transmitMode())); From 2f2406a52d100b7057b7d5a74e712be6e518a416 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 7 Apr 2013 18:41:01 +0200 Subject: [PATCH 02/13] Expanded fields changed flags --- src/comm/QGCXPlaneLink.cc | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc index c2078ed..8529055 100644 --- a/src/comm/QGCXPlaneLink.cc +++ b/src/comm/QGCXPlaneLink.cc @@ -476,12 +476,14 @@ void QGCXPlaneLink::readBytes() xacc = p.f[5] * 9.81f; xacc = p.f[6] * 9.81f; zacc = -p.f[4] * 9.81f; + fields_changed |= (1 << 0) | (1 << 1) | (1 << 2); } else if (p.index == 6 && xPlaneVersion == 10) { // inHg to kPa abs_pressure = p.f[0] * 3.3863886666718317f; temperature = p.f[1]; + fields_changed |= (1 << 9) | (1 << 12); } // Forward controls from X-Plane to MAV, not very useful // better: Connect Joystick to QGroundControl @@ -500,7 +502,7 @@ void QGCXPlaneLink::readBytes() rollspeed = p.f[2]; pitchspeed = p.f[1]; yawspeed = p.f[0]; - fields_changed |= (1 << 0) | (1 << 1) | (1 << 2); + fields_changed |= (1 << 3) | (1 << 4) | (1 << 5); } else if ((xPlaneVersion == 10 && p.index == 17) || (xPlaneVersion == 9 && p.index == 18)) { @@ -528,6 +530,7 @@ void QGCXPlaneLink::readBytes() xmag = cos(yawmag) * 0.4f - sin(yawmag) * 0.0f + 0.0f; ymag = sin(yawmag) * 0.4f - sin(yawmag) * 0.0f + 0.0f; zmag = 0.0f + 0.0f + 1.0f * 0.4f; + fields_changed |= (1 << 6) | (1 << 7) | (1 << 8); emitUpdate = true; } @@ -536,7 +539,7 @@ void QGCXPlaneLink::readBytes() rollspeed = p.f[2]; pitchspeed = p.f[1]; yawspeed = p.f[0]; - fields_changed |= (1 << 0) | (1 << 1) | (1 << 2); + fields_changed |= (1 << 3) | (1 << 4) | (1 << 5); } // else if (p.index == 19) @@ -614,6 +617,8 @@ void QGCXPlaneLink::readBytes() { diff_pressure = 0.0f; pressure_alt = alt; + // set pressure alt to changed + fields_changed |= (1 << 11); emit sensorHilRawImuChanged(QGC::groundTimeUsecs(), xacc, yacc, zacc, rollspeed, pitchspeed, yawspeed, xmag, ymag, zmag, abs_pressure, diff_pressure, pressure_alt, temperature, fields_changed); From e0534e82bbbded2c9a3ef646901a0528d2c87155 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Apr 2013 12:31:01 +0200 Subject: [PATCH 03/13] Better differentiation between estimator position and raw GPS --- src/uas/UAS.cc | 30 +++++++++++++++++++----------- src/uas/UAS.h | 10 +++++++--- 2 files changed, 26 insertions(+), 14 deletions(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 8f8661a..cb3ca6e 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -80,6 +80,10 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), latitude(0.0), longitude(0.0), altitude(0.0), + globalEstimatorActive(false), + latitude_gps(0.0), + longitude_gps(0.0), + altitude_gps(0.0), roll(0.0), pitch(0.0), yaw(0.0), @@ -733,6 +737,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) latitude = pos.lat/(double)1E7; longitude = pos.lon/(double)1E7; altitude = pos.alt/1000.0; + globalEstimatorActive = true; speedX = pos.vx/100.0; speedY = pos.vy/100.0; speedZ = pos.vz/100.0; @@ -772,10 +777,17 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) if (pos.fix_type > 2) { - emit globalPositionChanged(this, pos.lat/(double)1E7, pos.lon/(double)1E7, pos.alt/1000.0, time); - latitude = pos.lat/(double)1E7; - longitude = pos.lon/(double)1E7; - altitude = pos.alt/1000.0; + latitude_gps = pos.lat/(double)1E7; + longitude_gps = pos.lon/(double)1E7; + altitude_gps = pos.alt/1000.0; + + if (!globalEstimatorActive) { + latitude = latitude_gps; + longitude = longitude_gps; + altitude = altitude_gps; + emit globalPositionChanged(this, latitude, longitude, altitude, time); + } + positionLock = true; isGlobalPositionKnown = true; @@ -784,18 +796,15 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) if (!isnan(alt) && !isinf(alt)) { alt = 0; - //emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN or Inf FOR ALTITUDE"); + emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN or Inf FOR ALTITUDE"); } - // FIXME REMOVE LATER emit valueChanged(uasId, "altitude", "m", pos.alt/(double)1E3, time); // Smaller than threshold and not NaN float vel = pos.vel/100.0f; - if (vel < 1000000 && !isnan(vel) && !isinf(vel)) + if (!globalEstimatorActive && (vel < 1000000) && !isnan(vel) && !isinf(vel)) { - // FIXME REMOVE LATER emit valueChanged(uasId, "speed", "m/s", vel, time); - //qDebug() << "GOT GPS RAW"; - // emit speedChanged(this, (double)pos.v, 0.0, 0.0, time); + emit speedChanged(this, (double)pos.v, 0.0, 0.0, time); } else { @@ -905,7 +914,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) // Emit change emit parameterChanged(uasId, message.compid, parameterName, param); emit parameterChanged(uasId, message.compid, value.param_count, value.param_index, parameterName, param); -// qDebug() << "RECEIVED PARAM:" << param; } break; case MAV_PARAM_TYPE_INT32: diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 1f517f7..de2d067 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -278,9 +278,13 @@ protected: //COMMENTS FOR TEST UNIT double localX; double localY; double localZ; - double latitude; - double longitude; - double altitude; + double latitude; ///< Global latitude as estimated by position estimator + double longitude; ///< Global longitude as estimated by position estimator + double altitude; ///< Global altitude as estimated by position estimator + bool globalEstimatorActive; ///< Global position estimator present, do not fall back to GPS raw for position + double latitude_gps; ///< Global latitude as estimated by raw GPS + double longitude_gps; ///< Global longitude as estimated by raw GPS + double altitude_gps; ///< Global altitude as estimated by raw GPS double speedX; ///< True speed in X axis double speedY; ///< True speed in Y axis double speedZ; ///< True speed in Z axis From c67c91a785e97a78d4625a0d027b7b5dec2c4bfe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Apr 2013 12:31:18 +0200 Subject: [PATCH 04/13] silenced settings load --- src/ui/designer/QGCCommandButton.cc | 4 ++-- src/ui/designer/QGCToolWidget.cc | 36 ++++++++++++++++++------------------ 2 files changed, 20 insertions(+), 20 deletions(-) diff --git a/src/ui/designer/QGCCommandButton.cc b/src/ui/designer/QGCCommandButton.cc index d41e8fc..86ae0fd 100644 --- a/src/ui/designer/QGCCommandButton.cc +++ b/src/ui/designer/QGCCommandButton.cc @@ -118,7 +118,7 @@ void QGCCommandButton::sendCommand() int component = ui->editComponentSpinBox->value(); QGCToolWidgetItem::uas->executeCommand(command, confirm, param1, param2, param3, param4, param5, param6, param7, component); - qDebug() << __FILE__ << __LINE__ << "SENDING COMMAND" << index; + //qDebug() << __FILE__ << __LINE__ << "SENDING COMMAND" << index; } } } @@ -226,7 +226,7 @@ void QGCCommandButton::endEditMode() void QGCCommandButton::writeSettings(QSettings& settings) { - qDebug() << "COMMAND BUTTON WRITING SETTINGS"; + //qDebug() << "COMMAND BUTTON WRITING SETTINGS"; settings.setValue("TYPE", "COMMANDBUTTON"); settings.setValue("QGC_COMMAND_BUTTON_DESCRIPTION", ui->nameLabel->text()); settings.setValue("QGC_COMMAND_BUTTON_BUTTONTEXT", ui->commandButton->text()); diff --git a/src/ui/designer/QGCToolWidget.cc b/src/ui/designer/QGCToolWidget.cc index f988931..27df744 100644 --- a/src/ui/designer/QGCToolWidget.cc +++ b/src/ui/designer/QGCToolWidget.cc @@ -28,7 +28,7 @@ QGCToolWidget::QGCToolWidget(const QString& title, QWidget *parent, QSettings* s { widgetTitle = QString("%1 %2").arg(title).arg(QGCToolWidget::instances()->count()); } - qDebug() << "WidgetTitle" << widgetTitle; + //qDebug() << "WidgetTitle" << widgetTitle; setObjectName(widgetTitle); createActions(); @@ -99,12 +99,12 @@ QList QGCToolWidget::createWidgetsFromSettings(QWidget* parent, if (!settingsFile.isEmpty()) { settings = new QSettings(settingsFile, QSettings::IniFormat); - qDebug() << "LOADING SETTINGS FROM" << settingsFile; + //qDebug() << "LOADING SETTINGS FROM" << settingsFile; } else { settings = new QSettings(); - qDebug() << "LOADING SETTINGS FROM DEFAULT" << settings->fileName(); + //qDebug() << "LOADING SETTINGS FROM DEFAULT" << settings->fileName(); } QList newWidgets; @@ -116,7 +116,7 @@ QList QGCToolWidget::createWidgetsFromSettings(QWidget* parent, if (!instances()->contains(name) && name.length() != 0) { - qDebug() << "CREATED WIDGET:" << name; + //qDebug() << "CREATED WIDGET:" << name; QGCToolWidget* tool = new QGCToolWidget(name, parent, settings); newWidgets.append(tool); } @@ -127,12 +127,12 @@ QList QGCToolWidget::createWidgetsFromSettings(QWidget* parent, } else { - qDebug() << "WIDGET" << name << "DID ALREADY EXIST, REJECTING"; + //qDebug() << "WIDGET" << name << "DID ALREADY EXIST, REJECTING"; } } settings->endArray(); - qDebug() << "NEW WIDGETS: " << newWidgets.size(); + //qDebug() << "NEW WIDGETS: " << newWidgets.size(); // Load individual widget items for (int i = 0; i < newWidgets.size(); i++) @@ -158,7 +158,7 @@ bool QGCToolWidget::loadSettings(const QString& settings, bool singleinstance) // Do not use setTitle() here, // interferes with loading settings widgetTitle = widgetName; - qDebug() << "WIDGET TITLE LOADED: " << widgetName; + //qDebug() << "WIDGET TITLE LOADED: " << widgetName; loadSettings(set); return true; } @@ -172,9 +172,9 @@ void QGCToolWidget::loadSettings(QSettings& settings) { QString widgetName = getTitle(); settings.beginGroup(widgetName); - qDebug() << "LOADING FOR" << widgetName; + //qDebug() << "LOADING FOR" << widgetName; int size = settings.beginReadArray("QGC_TOOL_WIDGET_ITEMS"); - qDebug() << "CHILDREN SIZE:" << size; + //qDebug() << "CHILDREN SIZE:" << size; for (int j = 0; j < size; j++) { settings.setArrayIndex(j); @@ -185,12 +185,12 @@ void QGCToolWidget::loadSettings(QSettings& settings) if (type == "COMMANDBUTTON") { item = new QGCCommandButton(this); - qDebug() << "CREATED COMMANDBUTTON"; + //qDebug() << "CREATED COMMANDBUTTON"; } else if (type == "SLIDER") { item = new QGCParamSlider(this); - qDebug() << "CREATED PARAM SLIDER"; + //qDebug() << "CREATED PARAM SLIDER"; } if (item) @@ -199,12 +199,12 @@ void QGCToolWidget::loadSettings(QSettings& settings) addToolWidget(item); item->readSettings(settings); - qDebug() << "Created tool widget"; + //qDebug() << "Created tool widget"; } } else { - qDebug() << "UNKNOWN TOOL WIDGET TYPE"; + //qDebug() << "UNKNOWN TOOL WIDGET TYPE"; } } settings.endArray(); @@ -218,12 +218,12 @@ void QGCToolWidget::storeWidgetsToSettings(QString settingsFile) if (!settingsFile.isEmpty()) { settings = new QSettings(settingsFile, QSettings::IniFormat); - qDebug() << "STORING SETTINGS TO" << settings->fileName(); + //qDebug() << "STORING SETTINGS TO" << settings->fileName(); } else { settings = new QSettings(); - qDebug() << "STORING SETTINGS TO DEFAULT" << settings->fileName(); + //qDebug() << "STORING SETTINGS TO DEFAULT" << settings->fileName(); } int preArraySize = settings->beginReadArray("QGC_TOOL_WIDGET_NAMES"); @@ -237,7 +237,7 @@ void QGCToolWidget::storeWidgetsToSettings(QString settingsFile) { // Updating value settings->setValue("TITLE", instances()->values().at(i)->getTitle()); - qDebug() << "WRITING TITLE" << instances()->values().at(i)->getTitle(); + //qDebug() << "WRITING TITLE" << instances()->values().at(i)->getTitle(); } else { @@ -269,7 +269,7 @@ void QGCToolWidget::storeSettings(const QString& settingsFile) void QGCToolWidget::storeSettings(QSettings& settings) { - qDebug() << "WRITING WIDGET" << widgetTitle << "TO SETTINGS"; + //qDebug() << "WRITING WIDGET" << widgetTitle << "TO SETTINGS"; settings.beginGroup(widgetTitle); settings.beginWriteArray("QGC_TOOL_WIDGET_ITEMS"); int k = 0; // QGCToolItem counter @@ -285,7 +285,7 @@ void QGCToolWidget::storeSettings(QSettings& settings) item->writeSettings(settings); } } - qDebug() << "WROTE" << k << "SUB-WIDGETS TO SETTINGS"; + //qDebug() << "WROTE" << k << "SUB-WIDGETS TO SETTINGS"; settings.endArray(); settings.endGroup(); } From 8b5026a73a4e8081b708eb139d8d9511c78c33c4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Apr 2013 12:32:51 +0200 Subject: [PATCH 05/13] Build fix --- src/uas/UAS.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index cb3ca6e..bcc64ac 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -804,7 +804,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) if (!globalEstimatorActive && (vel < 1000000) && !isnan(vel) && !isinf(vel)) { - emit speedChanged(this, (double)pos.v, 0.0, 0.0, time); + emit speedChanged(this, vel, 0.0, 0.0, time); } else { From 16558d7004ac0ff117ec3c5e47ef7e9a4f9de88b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 16 Apr 2013 08:19:20 +0200 Subject: [PATCH 06/13] Icon fixes, HSI shows satellites correctly rotated --- libs/opmapcontrol/src/mapwidget/uavitem.cpp | 3 +++ src/ui/HSIDisplay.cc | 12 ++++++++---- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/libs/opmapcontrol/src/mapwidget/uavitem.cpp b/libs/opmapcontrol/src/mapwidget/uavitem.cpp index f854c9c..28011cb 100644 --- a/libs/opmapcontrol/src/mapwidget/uavitem.cpp +++ b/libs/opmapcontrol/src/mapwidget/uavitem.cpp @@ -58,7 +58,10 @@ namespace mapcontrol Q_UNUSED(option); Q_UNUSED(widget); // painter->rotate(-90); + QPainter::RenderHints oldhints = painter->renderHints(); + painter->setRenderHints(QPainter::Antialiasing | QPainter::SmoothPixmapTransform); painter->drawPixmap(-pic.width()/2,-pic.height()/2,pic); + painter->setRenderHints(oldhints); // painter->drawRect(QRectF(-pic.width()/2,-pic.height()/2,pic.width()-1,pic.height()-1)); } QRectF UAVItem::boundingRect()const diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 7129f26..736b5f2 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -1310,9 +1310,13 @@ void HSIDisplay::drawSafetyArea(const QPointF &topLeft, const QPointF &bottomRig void HSIDisplay::drawGPS(QPainter &painter) { float xCenter = xCenterPos; - float yCenter = xCenterPos; - // Max satellite circle radius + float yCenter = yCenterPos; + const float yawDeg = ((yaw/M_PI)*180.0f); + int yawRotate = static_cast(yawDeg) % 360; + // XXX check rotation direction + + // Max satellite circle radius const float margin = 0.15f; // 20% margin of total width on each side float radius = (vwidth - vwidth * 2.0f * margin) / 2.0f; quint64 currTime = MG::TIME::getGroundTimeNowUsecs(); @@ -1353,8 +1357,8 @@ void HSIDisplay::drawGPS(QPainter &painter) painter.setPen(color); painter.setBrush(brush); - float xPos = xCenter + (sin(((sat->azimuth/255.0f)*360.0f)/180.0f * M_PI) * cos(sat->elevation/180.0f * M_PI)) * radius; - float yPos = yCenter - (cos(((sat->azimuth/255.0f)*360.0f)/180.0f * M_PI) * cos(sat->elevation/180.0f * M_PI)) * radius; + float xPos = xCenter + (sin(((sat->azimuth/255.0f)*360.0f-yawRotate)/180.0f * M_PI) * cos(sat->elevation/180.0f * M_PI)) * radius; + float yPos = yCenter - (cos(((sat->azimuth/255.0f)*360.0f-yawRotate)/180.0f * M_PI) * cos(sat->elevation/180.0f * M_PI)) * radius; // Draw circle for satellite, filled for used satellites drawCircle(xPos, yPos, vwidth*0.02f, 1.0f, color, &painter); From e784c17ea1be75f434f7dbffb1199f55d4e88213 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 16 Apr 2013 14:07:09 +0200 Subject: [PATCH 07/13] Ask mavlink app to start on PX4 boards if connected via NSH --- src/comm/LinkManager.cc | 2 ++ src/comm/MAVLinkProtocol.cc | 27 +++++++++++++++++++++++++++ src/comm/MAVLinkProtocol.h | 1 + src/comm/ProtocolInterface.h | 1 + 4 files changed, 31 insertions(+) diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index e4dc520..0db0139 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -94,6 +94,8 @@ void LinkManager::addProtocol(LinkInterface* link, ProtocolInterface* protocol) { // Protocol is new, add connect(link, SIGNAL(bytesReceived(LinkInterface*, QByteArray)), protocol, SLOT(receiveBytes(LinkInterface*, QByteArray))); + // Add status + connect(link, SIGNAL(connected(bool)), protocol, SLOT(linkStatusChanged(bool))); // Store the connection information in the protocol links map protocolLinks.insertMulti(protocol, link); } diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index e14d6bd..36f3f0a 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -174,6 +174,33 @@ QString MAVLinkProtocol::getLogfileName() } } +void MAVLinkProtocol::linkStatusChanged(bool connected) +{ + LinkInterface* link = qobject_cast(QObject::sender()); + + if (link) { + if (connected) { + // Send command to start MAVLink + // XXX hacky but safe + // Start NSH + const char init[] = {0x0d, 0x0d, 0x0d}; + link->writeBytes(init, 1); + QGC::SLEEP::msleep(500); + + // Stop any running mavlink instance + char* cmd = "mavlink stop\n"; + link->writeBytes(cmd, strlen(cmd)); + link->writeBytes(init, 2); + cmd = "uorb start"; + link->writeBytes(cmd, strlen(cmd)); + link->writeBytes(init, 2); + cmd = "sh /etc/init.d/rc.usb\n"; + link->writeBytes(cmd, strlen(cmd)); + link->writeBytes(init, 4); + } + } +} + /** * The bytes are copied by calling the LinkInterface::readBytes() method. * This method parses all incoming bytes and constructs a MAVLink packet. diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index 03d8bfe..829b8e6 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -128,6 +128,7 @@ public: public slots: /** @brief Receive bytes from a communication interface */ void receiveBytes(LinkInterface* link, QByteArray b); + void linkStatusChanged(bool connected); /** @brief Send MAVLink message through serial interface */ void sendMessage(mavlink_message_t message); /** @brief Send MAVLink message */ diff --git a/src/comm/ProtocolInterface.h b/src/comm/ProtocolInterface.h index 6f0e583..758a0a7 100644 --- a/src/comm/ProtocolInterface.h +++ b/src/comm/ProtocolInterface.h @@ -55,6 +55,7 @@ public: public slots: virtual void receiveBytes(LinkInterface *link, QByteArray b) = 0; + virtual void linkStatusChanged(bool connected) = 0; signals: /** @brief Update the packet loss from one system */ From 72e70e1923517751b164a2e2cb236b4ae33c500a Mon Sep 17 00:00:00 2001 From: Christopher Hrabia Date: Wed, 17 Apr 2013 14:12:48 +0800 Subject: [PATCH 08/13] Store directory where the last log file was stored and reuse it when loading or saving new log files --- src/ui/QGCMAVLinkLogPlayer.cc | 16 ++++++++++++---- src/ui/QGCMAVLinkLogPlayer.h | 2 +- src/ui/QGCToolBar.cc | 31 +++++++++++++++++++++++++++---- src/ui/QGCToolBar.h | 3 +++ 4 files changed, 43 insertions(+), 9 deletions(-) diff --git a/src/ui/QGCMAVLinkLogPlayer.cc b/src/ui/QGCMAVLinkLogPlayer.cc index 56076d0..83f2a78 100644 --- a/src/ui/QGCMAVLinkLogPlayer.cc +++ b/src/ui/QGCMAVLinkLogPlayer.cc @@ -167,9 +167,14 @@ bool QGCMAVLinkLogPlayer::reset(int packetIndex) } } -bool QGCMAVLinkLogPlayer::selectLogFile() +/** + * @brief QGCMAVLinkLogPlayer::selectLogFile + * @param startDirectory Directory where the file dialog will be opened + * @return filename of the logFile + */ +bool QGCMAVLinkLogPlayer::selectLogFile(const QString startDirectory) { - QString fileName = QFileDialog::getOpenFileName(this, tr("Specify MAVLink log file name to replay"), QDesktopServices::storageLocation(QDesktopServices::DesktopLocation), tr("MAVLink or Binary Logfile (*.mavlink *.bin *.log)")); + QString fileName = QFileDialog::getOpenFileName(this, tr("Specify MAVLink log file name to replay"), startDirectory, tr("MAVLink or Binary Logfile (*.mavlink *.bin *.log)")); if (fileName == "") { @@ -370,6 +375,7 @@ void QGCMAVLinkLogPlayer::logLoop() //qDebug() << "START TIME: " << startTime; // Check if these bytes could be correctly decoded + // TODO if (!ok) { ui->logStatsLabel->setText(tr("Error decoding first timestamp, aborting.")); @@ -381,14 +387,15 @@ void QGCMAVLinkLogPlayer::logLoop() // Initialization seems fine, load next chunk + //this is ok because before we already read the timestamp of this paket before QByteArray chunk = logFile.read(timeLen+packetLen); - QByteArray packet = chunk.mid(0, packetLen); + QByteArray packet = chunk.left(packetLen); // Emit this packet emit bytesReady(logLink, packet); // Check if reached end of file before reading next timestamp - if (chunk.length() < timeLen+packetLen || logFile.atEnd()) + if (chunk.length() < (timeLen + packetLen) || logFile.atEnd()) { // Reached end of file reset(); @@ -400,6 +407,7 @@ void QGCMAVLinkLogPlayer::logLoop() } // End of file not reached, read next timestamp + // which is located after current packet QByteArray rawTime = chunk.mid(packetLen); // This is the timestamp of the next packet diff --git a/src/ui/QGCMAVLinkLogPlayer.h b/src/ui/QGCMAVLinkLogPlayer.h index 9e2028c..1ea0b79 100644 --- a/src/ui/QGCMAVLinkLogPlayer.h +++ b/src/ui/QGCMAVLinkLogPlayer.h @@ -49,7 +49,7 @@ public slots: /** @brief Reset the logfile */ bool reset(int packetIndex=0); /** @brief Select logfile */ - bool selectLogFile(); + bool selectLogFile(const QString startDirectory); /** @brief Load log file */ bool loadLogFile(const QString& file); /** @brief Jump to a position in the logfile */ diff --git a/src/ui/QGCToolBar.cc b/src/ui/QGCToolBar.cc index 583bec6..1d25a9b 100644 --- a/src/ui/QGCToolBar.cc +++ b/src/ui/QGCToolBar.cc @@ -38,7 +38,8 @@ QGCToolBar::QGCToolBar(QWidget *parent) : batteryVoltage(0), wpId(0), wpDistance(0), - systemArmed(false) + systemArmed(false), + lastLogDirectory(QDesktopServices::storageLocation(QDesktopServices::DesktopLocation)) { setObjectName("QGC_TOOLBAR"); @@ -123,6 +124,8 @@ QGCToolBar::QGCToolBar(QWidget *parent) : // Set the toolbar to be updated every 2s connect(&updateViewTimer, SIGNAL(timeout()), this, SLOT(updateView())); updateViewTimer.start(2000); + + loadSettings(); } void QGCToolBar::heartbeatTimeout(bool timeout, unsigned int ms) @@ -171,13 +174,13 @@ void QGCToolBar::playLogFile(bool checked) player->playPause(false); if (checked) { - if (!player->selectLogFile()) return; + if (!player->selectLogFile(lastLogDirectory)) return; } } // If no replaying happens already, start it else { - if (!player->selectLogFile()) return; + if (!player->selectLogFile(lastLogDirectory)) return; } player->playPause(checked); } @@ -192,7 +195,7 @@ void QGCToolBar::logging(bool checked) if (checked) { // Prompt the user for a filename/location to save to - QString fileName = QFileDialog::getSaveFileName(this, tr("Specify MAVLink log file to save to"), QDesktopServices::storageLocation(QDesktopServices::DesktopLocation), tr("MAVLink Logfile (*.mavlink *.log *.bin);;")); + QString fileName = QFileDialog::getSaveFileName(this, tr("Specify MAVLink log file to save to"), lastLogDirectory, tr("MAVLink Logfile (*.mavlink *.log *.bin);;")); // Check that they didn't cancel out if (fileName.isNull()) @@ -224,6 +227,7 @@ void QGCToolBar::logging(bool checked) { MainWindow::instance()->getMAVLink()->setLogfileName(fileName); MainWindow::instance()->getMAVLink()->enableLogging(true); + lastLogDirectory = file.absoluteDir().absolutePath(); //save last log directory } } } @@ -486,6 +490,24 @@ void QGCToolBar::connectLink(bool connect) } + +void QGCToolBar::loadSettings() +{ + QSettings settings; + settings.beginGroup("QGC_TOOLBAR"); + lastLogDirectory = settings.value("LAST_LOG_DIRECTORY", lastLogDirectory).toString(); + settings.endGroup(); +} + +void QGCToolBar::storeSettings() +{ + QSettings settings; + settings.beginGroup("QGC_TOOLBAR"); + settings.setValue("LAST_LOG_DIRECTORY", lastLogDirectory); + settings.endGroup(); + settings.sync(); +} + void QGCToolBar::clearStatusString() { lastSystemMessage = ""; @@ -494,6 +516,7 @@ void QGCToolBar::clearStatusString() QGCToolBar::~QGCToolBar() { + storeSettings(); if (toggleLoggingAction) toggleLoggingAction->deleteLater(); if (logReplayAction) logReplayAction->deleteLater(); } diff --git a/src/ui/QGCToolBar.h b/src/ui/QGCToolBar.h index 2ab4344..29b85f1 100644 --- a/src/ui/QGCToolBar.h +++ b/src/ui/QGCToolBar.h @@ -80,6 +80,8 @@ public slots: protected: void createCustomWidgets(); + void storeSettings(); + void loadSettings(); QAction* toggleLoggingAction; QAction* logReplayAction; @@ -109,6 +111,7 @@ protected: quint64 lastSystemMessageTimeMs; QTimer updateViewTimer; bool systemArmed; + QString lastLogDirectory; }; #endif // QGCTOOLBAR_H From f1627158289e1df749255e87eb5fc2b96f29d320 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Apr 2013 08:47:03 +0200 Subject: [PATCH 09/13] Fixed typo --- src/comm/MAVLinkProtocol.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 36f3f0a..306ce52 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -184,7 +184,7 @@ void MAVLinkProtocol::linkStatusChanged(bool connected) // XXX hacky but safe // Start NSH const char init[] = {0x0d, 0x0d, 0x0d}; - link->writeBytes(init, 1); + link->writeBytes(init, sizeof(init)); QGC::SLEEP::msleep(500); // Stop any running mavlink instance From 42e630ebf3e71397aa6bca0ff3f31fcc726c8922 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Apr 2013 21:50:04 +0200 Subject: [PATCH 10/13] Fixed up logplayer button --- src/ui/QGCMAVLinkLogPlayer.cc | 33 ++++++++++++++++++++++++++++++++- src/ui/QGCMAVLinkLogPlayer.h | 6 ++++++ 2 files changed, 38 insertions(+), 1 deletion(-) diff --git a/src/ui/QGCMAVLinkLogPlayer.cc b/src/ui/QGCMAVLinkLogPlayer.cc index 83f2a78..e989fc2 100644 --- a/src/ui/QGCMAVLinkLogPlayer.cc +++ b/src/ui/QGCMAVLinkLogPlayer.cc @@ -22,6 +22,7 @@ QGCMAVLinkLogPlayer::QGCMAVLinkLogPlayer(MAVLinkProtocol* mavlink, QWidget *pare binaryBaudRate(57600), isPlaying(false), currPacketCount(0), + lastLogDirectory(QDesktopServices::storageLocation(QDesktopServices::DesktopLocation)), ui(new Ui::QGCMAVLinkLogPlayer) { ui->setupUi(this); @@ -43,10 +44,12 @@ QGCMAVLinkLogPlayer::QGCMAVLinkLogPlayer(MAVLinkProtocol* mavlink, QWidget *pare setAccelerationFactorInt(49); ui->speedSlider->setValue(49); ui->positionSlider->setValue(ui->positionSlider->minimum()); + loadSettings(); } QGCMAVLinkLogPlayer::~QGCMAVLinkLogPlayer() { + storeSettings(); delete ui; } @@ -167,8 +170,35 @@ bool QGCMAVLinkLogPlayer::reset(int packetIndex) } } +void QGCMAVLinkLogPlayer::loadSettings() +{ + QSettings settings; + settings.beginGroup("QGC_MAVLINKLOGPLAYER"); + lastLogDirectory = settings.value("LAST_LOG_DIRECTORY", lastLogDirectory).toString(); + settings.endGroup(); +} + +void QGCMAVLinkLogPlayer::storeSettings() +{ + QSettings settings; + settings.beginGroup("QGC_MAVLINKLOGPLAYER"); + settings.setValue("LAST_LOG_DIRECTORY", lastLogDirectory); + settings.endGroup(); + settings.sync(); +} + +/** + * @brief Select a log file + * @param startDirectory Directory where the file dialog will be opened + * @return filename of the logFile + */ +bool QGCMAVLinkLogPlayer::selectLogFile() +{ + return selectLogFile(lastLogDirectory); +} + /** - * @brief QGCMAVLinkLogPlayer::selectLogFile + * @brief Select a log file * @param startDirectory Directory where the file dialog will be opened * @return filename of the logFile */ @@ -182,6 +212,7 @@ bool QGCMAVLinkLogPlayer::selectLogFile(const QString startDirectory) } else { + lastLogDirectory = fileName; return loadLogFile(fileName); } } diff --git a/src/ui/QGCMAVLinkLogPlayer.h b/src/ui/QGCMAVLinkLogPlayer.h index 1ea0b79..e4c0e4f 100644 --- a/src/ui/QGCMAVLinkLogPlayer.h +++ b/src/ui/QGCMAVLinkLogPlayer.h @@ -50,6 +50,8 @@ public slots: bool reset(int packetIndex=0); /** @brief Select logfile */ bool selectLogFile(const QString startDirectory); + /** @brief Select logfile */ + bool selectLogFile(); /** @brief Load log file */ bool loadLogFile(const QString& file); /** @brief Jump to a position in the logfile */ @@ -81,8 +83,12 @@ protected: unsigned int currPacketCount; static const int packetLen = MAVLINK_MAX_PACKET_LEN; static const int timeLen = sizeof(quint64); + QString lastLogDirectory; void changeEvent(QEvent *e); + void loadSettings(); + void storeSettings(); + private: Ui::QGCMAVLinkLogPlayer *ui; }; From 2e625497ec1671ba9c3695528cf381916bdb28e9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Apr 2013 17:53:40 +0200 Subject: [PATCH 11/13] Simplified link creation / connection a bit --- src/comm/LinkManager.cc | 8 +++-- src/comm/LinkManager.h | 3 +- src/comm/MAVLinkSimulationLink.cc | 1 - src/uas/UAS.cc | 48 +++++++++++++++---------- src/uas/UAS.h | 1 + src/ui/CommConfigurationWindow.cc | 4 +++ src/ui/QGCToolBar.cc | 76 +++++++++++++++++++++++++++++++-------- src/ui/QGCToolBar.h | 7 ++++ 8 files changed, 111 insertions(+), 37 deletions(-) diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index 0db0139..0f0a876 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -74,7 +74,7 @@ void LinkManager::add(LinkInterface* link) if (!links.contains(link)) { if(!link) return; - connect(link, SIGNAL(destroyed(QObject*)), this, SLOT(removeLink(QObject*))); + connect(link, SIGNAL(destroyed(QObject*)), this, SLOT(removeObj(QObject*))); links.append(link); emit newLink(link); } @@ -147,7 +147,7 @@ bool LinkManager::disconnectLink(LinkInterface* link) return link->disconnect(); } -void LinkManager::removeLink(QObject* link) +void LinkManager::removeObj(QObject* link) { LinkInterface* linkInterface = dynamic_cast(link); if (linkInterface) @@ -173,6 +173,10 @@ bool LinkManager::removeLink(LinkInterface* link) { protocolLinks.remove(proto, link); } + + // Emit removal of link + emit linkRemoved(link); + return true; } return false; diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index 178b363..b960ebb 100644 --- a/src/comm/LinkManager.h +++ b/src/comm/LinkManager.h @@ -72,7 +72,7 @@ public slots: void add(LinkInterface* link); void addProtocol(LinkInterface* link, ProtocolInterface* protocol); - void removeLink(QObject* link); + void removeObj(QObject* obj); bool removeLink(LinkInterface* link); bool connectAll(); @@ -91,6 +91,7 @@ private: signals: void newLink(LinkInterface* link); + void linkRemoved(LinkInterface* link); }; diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index baa1d80..f94f402 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -107,7 +107,6 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile maxTimeNoise = 0; this->id = getNextLinkId(); LinkManager::instance()->add(this); - QObject::connect(this, SIGNAL(destroyed(QObject*)), LinkManager::instance(), SLOT(removeLink(QObject*))); } MAVLinkSimulationLink::~MAVLinkSimulationLink() diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index bcc64ac..1eed69c 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -791,24 +791,19 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) positionLock = true; isGlobalPositionKnown = true; - // Check for NaN - int alt = pos.alt; - if (!isnan(alt) && !isinf(alt)) - { - alt = 0; - emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN or Inf FOR ALTITUDE"); - } // Smaller than threshold and not NaN float vel = pos.vel/100.0f; - if (!globalEstimatorActive && (vel < 1000000) && !isnan(vel) && !isinf(vel)) - { - emit speedChanged(this, vel, 0.0, 0.0, time); - } - else - { - emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(vel)); + if (!globalEstimatorActive) { + if ((vel < 1000000) && !isnan(vel) && !isinf(vel)) + { + emit speedChanged(this, vel, 0.0, 0.0, time); + } + else + { + emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(vel)); + } } } } @@ -2737,11 +2732,23 @@ void UAS::sendHilState(uint64_t time_us, float roll, float pitch, float yaw, flo { if (this->mode & MAV_MODE_FLAG_HIL_ENABLED) { - mavlink_message_t msg; - mavlink_msg_hil_state_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, - time_us, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, - lat*1e7f, lon*1e7f, alt*1000, vx*100, vy*100, vz*100, xacc*1000, yacc*1000, zacc*1000); - sendMessage(msg); + if (sensorHil) { + // Emit attitude for cross-check + emit attitudeChanged(this, 201, roll, pitch, yaw, time_us/1000); + emit valueChanged(uasId, "roll sim", "rad", roll, time_us/1000); + emit valueChanged(uasId, "pitch sim", "rad", pitch, time_us/1000); + emit valueChanged(uasId, "yaw sim", "rad", yaw, time_us/1000); + + emit valueChanged(uasId, "roll rate sim", "rad/s", rollspeed, time_us/1000); + emit valueChanged(uasId, "pitch rate sim", "rad/s", pitchspeed, time_us/1000); + emit valueChanged(uasId, "yaw rate sim", "rad/s", yawspeed, time_us/1000); + } else { + mavlink_message_t msg; + mavlink_msg_hil_state_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, + time_us, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, + lat*1e7f, lon*1e7f, alt*1000, vx*100, vy*100, vz*100, xacc*1000, yacc*1000, zacc*1000); + sendMessage(msg); + } } else { @@ -2764,6 +2771,7 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl xmag, ymag, zmag, abs_pressure, diff_pressure, pressure_alt, temperature, fields_changed); sendMessage(msg); + sensorHil = true; } else { @@ -2802,6 +2810,7 @@ void UAS::startHil() { if (hilEnabled) return; hilEnabled = true; + sensorHil = false; mavlink_message_t msg; mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_HIL_ENABLED, navMode); sendMessage(msg); @@ -2819,6 +2828,7 @@ void UAS::stopHil() mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode & !MAV_MODE_FLAG_HIL_ENABLED, navMode); sendMessage(msg); hilEnabled = false; + sensorHil = false; } void UAS::shutdown() diff --git a/src/uas/UAS.h b/src/uas/UAS.h index de2d067..e4dfd78 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -703,6 +703,7 @@ protected: quint64 lastNonNullTime; ///< The last timestamp from the MAV that was not null unsigned int onboardTimeOffsetInvalidCount; ///< Count when the offboard time offset estimation seemed wrong bool hilEnabled; ///< Set to true if HIL mode is enabled from GCS (UAS might be in HIL even if this flag is not set, this defines the GCS HIL setting) + bool sensorHil; ///< True if sensor HIL is enabled protected slots: /** @brief Write settings to disk */ diff --git a/src/ui/CommConfigurationWindow.cc b/src/ui/CommConfigurationWindow.cc index 55db5f2..3654175 100644 --- a/src/ui/CommConfigurationWindow.cc +++ b/src/ui/CommConfigurationWindow.cc @@ -274,6 +274,10 @@ void CommConfigurationWindow::setConnection() { if(!link->isConnected()) { link->connect(); + QGC::SLEEP::msleep(100); + if (link->isConnected()) + // Auto-close window on connect + this->window()->close(); } else { link->disconnect(); } diff --git a/src/ui/QGCToolBar.cc b/src/ui/QGCToolBar.cc index 1d25a9b..74acfc0 100644 --- a/src/ui/QGCToolBar.cc +++ b/src/ui/QGCToolBar.cc @@ -23,6 +23,7 @@ This file is part of the QGROUNDCONTROL project #include #include +#include #include "QGCToolBar.h" #include "UASManager.h" #include "MainWindow.h" @@ -39,7 +40,8 @@ QGCToolBar::QGCToolBar(QWidget *parent) : wpId(0), wpDistance(0), systemArmed(false), - lastLogDirectory(QDesktopServices::storageLocation(QDesktopServices::DesktopLocation)) + lastLogDirectory(QDesktopServices::storageLocation(QDesktopServices::DesktopLocation)), + currentLink(NULL) { setObjectName("QGC_TOOLBAR"); @@ -110,8 +112,13 @@ QGCToolBar::QGCToolBar(QWidget *parent) : toolBarMessageLabel->setToolTip(tr("Most recent system message")); addWidget(toolBarMessageLabel); + QWidget* spacer = new QWidget(); + spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + addWidget(spacer); + connectButton = new QPushButton(tr("Connect"), this); connectButton->setToolTip(tr("Connect wireless link to MAV")); + connectButton->setCheckable(true); addWidget(connectButton); connect(connectButton, SIGNAL(clicked(bool)), this, SLOT(connectLink(bool))); @@ -121,6 +128,16 @@ QGCToolBar::QGCToolBar(QWidget *parent) : setActiveUAS(UASManager::instance()->getActiveUAS()); connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); + if (LinkManager::instance()->getLinks().count() > 2) + addLink(LinkManager::instance()->getLinks().last()); + // XXX implies that connect button is always active for the last used link + connect(LinkManager::instance(), SIGNAL(newLink(LinkInterface*)), this, SLOT(addLink(LinkInterface*))); + + // Update label if required + if (LinkManager::instance()->getLinks().count() < 3) { + connectButton->setText(tr("New Link")); + } + // Set the toolbar to be updated every 2s connect(&updateViewTimer, SIGNAL(timeout()), this, SLOT(updateView())); updateViewTimer.start(2000); @@ -465,8 +482,52 @@ void QGCToolBar::receiveTextMessage(int uasid, int componentid, int severity, QS lastSystemMessageTimeMs = QGC::groundTimeMilliseconds(); } +void QGCToolBar::addLink(LinkInterface* link) +{ + // XXX magic number + if (LinkManager::instance()->getLinks().count() > 2) { + currentLink = link; + connect(currentLink, SIGNAL(connected(bool)), this, SLOT(updateLinkState(bool))); + updateLinkState(link->isConnected()); + } +} + +void QGCToolBar::removeLink(LinkInterface* link) +{ + if (link == currentLink) { + currentLink = NULL; + // XXX magic number + if (LinkManager::instance()->getLinks().count() > 2) { + currentLink = LinkManager::instance()->getLinks().last(); + updateLinkState(currentLink->isConnected()); + } else { + connectButton->setText(tr("New Link")); + } + } +} + +void QGCToolBar::updateLinkState(bool connected) +{ + if (currentLink && currentLink->isConnected()) + { + connectButton->setText(tr("Disconnect")); + connectButton->blockSignals(true); + connectButton->setChecked(true); + connectButton->blockSignals(false); + } + else + { + connectButton->setText(tr("Connect")); + connectButton->blockSignals(true); + connectButton->setChecked(false); + connectButton->blockSignals(false); + } +} + void QGCToolBar::connectLink(bool connect) { + // No serial port yet present + // XXX magic number if (connect && LinkManager::instance()->getLinks().count() < 3) { MainWindow::instance()->addLink(); @@ -475,19 +536,6 @@ void QGCToolBar::connectLink(bool connect) } else if (!connect && LinkManager::instance()->getLinks().count() > 2) { LinkManager::instance()->getLinks().last()->disconnect(); } - - if (LinkManager::instance()->getLinks().count() > 2) { - if (LinkManager::instance()->getLinks().last()->isConnected()) - { - connectButton->setText(tr("Disconnect")); - } - else - { - connectButton->setText(tr("Connect")); - } - - } - } diff --git a/src/ui/QGCToolBar.h b/src/ui/QGCToolBar.h index 29b85f1..2fcdc3f 100644 --- a/src/ui/QGCToolBar.h +++ b/src/ui/QGCToolBar.h @@ -45,6 +45,12 @@ public: public slots: /** @brief Set the system that is currently displayed by this widget */ void setActiveUAS(UASInterface* active); + /** @brief Set the link which is currently handled with connecting / disconnecting */ + void addLink(LinkInterface* link); + /** @brief Remove link which is currently handled */ + void removeLink(LinkInterface* link); + /** @brief Update the link state */ + void updateLinkState(bool connected); /** @brief Set the system state */ void updateState(UASInterface* system, QString name, QString description); /** @brief Set the system mode */ @@ -112,6 +118,7 @@ protected: QTimer updateViewTimer; bool systemArmed; QString lastLogDirectory; + LinkInterface* currentLink; }; #endif // QGCTOOLBAR_H From 11006e551dc300773db6998a93e0c88ff01d9995 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Apr 2013 19:30:57 +0200 Subject: [PATCH 12/13] Fixed up first version of sensor level HIL, needs more validation, but operational --- src/comm/QGCXPlaneLink.cc | 76 +++++++++++++++++++++++++++-------------------- 1 file changed, 44 insertions(+), 32 deletions(-) diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc index 8529055..83f8347 100644 --- a/src/comm/QGCXPlaneLink.cc +++ b/src/comm/QGCXPlaneLink.cc @@ -474,14 +474,27 @@ void QGCXPlaneLink::readBytes() if (p.index == 4) { xacc = p.f[5] * 9.81f; - xacc = p.f[6] * 9.81f; + yacc = p.f[6] * 9.81f; zacc = -p.f[4] * 9.81f; + + Eigen::Vector3f g(0, 0, -9.81f); + + Eigen::Matrix3f R = euler_to_wRo(yaw, pitch, roll); + + Eigen::Vector3f gr = R.transpose().eval() * g; + + // TODO Add centrip. accel + + xacc = gr[0]; + yacc = gr[1]; + zacc = gr[2]; + fields_changed |= (1 << 0) | (1 << 1) | (1 << 2); } else if (p.index == 6 && xPlaneVersion == 10) { - // inHg to kPa - abs_pressure = p.f[0] * 3.3863886666718317f; + // inHg to hPa (hecto Pascal / millibar) + abs_pressure = p.f[0] * 33.863886666718317f; temperature = p.f[1]; fields_changed |= (1 << 9) | (1 << 12); } @@ -496,12 +509,12 @@ void QGCXPlaneLink::readBytes() // UAS* uas = dynamic_cast(mav); // if (uas) uas->setManualControlCommands(man_roll, man_pitch, man_yaw, 0.6); // } - else if (xPlaneVersion == 10 && p.index == 16) + else if ((xPlaneVersion == 10 && p.index == 16) || (xPlaneVersion == 9 && p.index == 17)) { - //qDebug() << "ANG VEL:" << p.f[0] << p.f[3] << p.f[7]; - rollspeed = p.f[2]; - pitchspeed = p.f[1]; - yawspeed = p.f[0]; + // Cross checked with XPlane flight + pitchspeed = p.f[0]; + rollspeed = p.f[1]; + yawspeed = p.f[2]; fields_changed |= (1 << 3) | (1 << 4) | (1 << 5); } else if ((xPlaneVersion == 10 && p.index == 17) || (xPlaneVersion == 9 && p.index == 18)) @@ -510,6 +523,9 @@ void QGCXPlaneLink::readBytes() pitch = p.f[0] / 180.0f * M_PI; roll = p.f[1] / 180.0f * M_PI; yaw = p.f[2] / 180.0f * M_PI; + + yaw = yaw; + // X-Plane expresses yaw as 0..2 PI if (yaw > M_PI) { yaw -= 2.0 * M_PI; @@ -518,7 +534,7 @@ void QGCXPlaneLink::readBytes() yaw += 2.0 * M_PI; } - float yawmag = p.f[3]; + float yawmag = p.f[3] / 180.0f * M_PI; if (yawmag > M_PI) { yawmag -= 2.0 * M_PI; @@ -527,20 +543,17 @@ void QGCXPlaneLink::readBytes() yawmag += 2.0 * M_PI; } - xmag = cos(yawmag) * 0.4f - sin(yawmag) * 0.0f + 0.0f; - ymag = sin(yawmag) * 0.4f - sin(yawmag) * 0.0f + 0.0f; - zmag = 0.0f + 0.0f + 1.0f * 0.4f; + // Normal rotation matrix, but since we rotate the + // vector [0.25 0 0.45]', we end up with these relevant + // matrix parts. + + xmag = cos(-yawmag) * 0.25f; + ymag = sin(-yawmag) * 0.25f; + zmag = 0.45f; fields_changed |= (1 << 6) | (1 << 7) | (1 << 8); emitUpdate = true; } - else if ((xPlaneVersion == 9 && p.index == 17)) - { - rollspeed = p.f[2]; - pitchspeed = p.f[1]; - yawspeed = p.f[0]; - fields_changed |= (1 << 3) | (1 << 4) | (1 << 5); - } // else if (p.index == 19) // { @@ -622,20 +635,19 @@ void QGCXPlaneLink::readBytes() emit sensorHilRawImuChanged(QGC::groundTimeUsecs(), xacc, yacc, zacc, rollspeed, pitchspeed, yawspeed, xmag, ymag, zmag, abs_pressure, diff_pressure, pressure_alt, temperature, fields_changed); - - int gps_fix_type = 3; - float eph = 0.3; - float epv = 0.6; - float vel = sqrt(vx*vx + vy*vy + vz*vz); - float cog = yaw; - int satellites = 8; - - emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, gps_fix_type, eph, epv, vel, cog, satellites); - } else { - emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed, - pitchspeed, yawspeed, lat, lon, alt, - vx, vy, vz, xacc, yacc, zacc); } + + int gps_fix_type = 3; + float eph = 0.3; + float epv = 0.6; + float vel = sqrt(vx*vx + vy*vy + vz*vz); + float cog = yaw; + int satellites = 8; + + emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, gps_fix_type, eph, epv, vel, cog, satellites); + emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed, + pitchspeed, yawspeed, lat, lon, alt, + vx, vy, vz, xacc, yacc, zacc); } if (!oldConnectionState && xPlaneConnected) From 0047d46b397d8935d5478355c83b553b5c88c1cb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Apr 2013 20:05:15 +0200 Subject: [PATCH 13/13] Minor modifications to sim value display, not optimal yet --- src/uas/UAS.cc | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 1eed69c..64623f4 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -2734,14 +2734,14 @@ void UAS::sendHilState(uint64_t time_us, float roll, float pitch, float yaw, flo { if (sensorHil) { // Emit attitude for cross-check - emit attitudeChanged(this, 201, roll, pitch, yaw, time_us/1000); - emit valueChanged(uasId, "roll sim", "rad", roll, time_us/1000); - emit valueChanged(uasId, "pitch sim", "rad", pitch, time_us/1000); - emit valueChanged(uasId, "yaw sim", "rad", yaw, time_us/1000); - - emit valueChanged(uasId, "roll rate sim", "rad/s", rollspeed, time_us/1000); - emit valueChanged(uasId, "pitch rate sim", "rad/s", pitchspeed, time_us/1000); - emit valueChanged(uasId, "yaw rate sim", "rad/s", yawspeed, time_us/1000); + emit attitudeChanged(this, 201, roll, pitch, yaw, getUnixTime()); + emit valueChanged(uasId, "roll sim", "rad", roll, getUnixTime()); + emit valueChanged(uasId, "pitch sim", "rad", pitch, getUnixTime()); + emit valueChanged(uasId, "yaw sim", "rad", yaw, getUnixTime()); + + emit valueChanged(uasId, "roll rate sim", "rad/s", rollspeed, getUnixTime()); + emit valueChanged(uasId, "pitch rate sim", "rad/s", pitchspeed, getUnixTime()); + emit valueChanged(uasId, "yaw rate sim", "rad/s", yawspeed, getUnixTime()); } else { mavlink_message_t msg; mavlink_msg_hil_state_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,