Commit ae152e73 authored by dogmaphobic's avatar dogmaphobic

Fixing merge conflicts.

Merge remote-tracking branch 'mavlink/master' into toggleMapButtons

* mavlink/master:
  UAS: Improve HIL inputs
  More work on waypoint lines

Conflicts:
	src/MissionEditor/MissionEditor.qml
parents 81908093 2ba8a0f3
......@@ -52,7 +52,7 @@ MissionEditor::MissionEditor(QWidget *parent)
_newMissionItemsAvailable();
} else {
_missionItems = new QmlObjectListModel(this);
connect(_missionItems, &QmlObjectListModel::dirtyChanged, this, &MissionEditor::_missionListDirtyChanged);
_initAllMissionItems();
}
setContextPropertyObject("controller", this);
......@@ -67,6 +67,7 @@ MissionEditor::~MissionEditor()
void MissionEditor::_newMissionItemsAvailable(void)
{
if (_missionItems) {
_deinitAllMissionItems();
_missionItems->deleteLater();
}
......@@ -74,14 +75,8 @@ void MissionEditor::_newMissionItemsAvailable(void)
_canEdit = missionManager->canEdit();
_missionItems = missionManager->copyMissionItems();
_reSequence();
_missionItems->setDirty(false);
connect(_missionItems, &QmlObjectListModel::dirtyChanged, this, &MissionEditor::_missionListDirtyChanged);
_rebuildWaypointLines();
emit missionItemsChanged();
emit canEditChanged(_canEdit);
_initAllMissionItems();
}
void MissionEditor::getMissionItems(void)
......@@ -97,10 +92,11 @@ void MissionEditor::getMissionItems(void)
void MissionEditor::setMissionItems(void)
{
// FIXME: Need to pull out home position
Vehicle* activeVehicle = MultiVehicleManager::instance()->activeVehicle();
if (activeVehicle) {
activeVehicle->missionManager()->writeMissionItems(*_missionItems);
activeVehicle->missionManager()->writeMissionItems(*_missionItems, true /* skipFirstItem */);
_missionItems->setDirty(false);
}
}
......@@ -112,23 +108,19 @@ int MissionEditor::addMissionItem(QGeoCoordinate coordinate)
}
MissionItem * newItem = new MissionItem(this, _missionItems->count(), coordinate, MAV_CMD_NAV_WAYPOINT);
_initMissionItem(newItem);
newItem->setAltitude(30);
if (_missionItems->count() == 0) {
if (_missionItems->count() == 1) {
newItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF);
}
qDebug() << "MissionItem" << newItem->coordinate();
_missionItems->append(newItem);
_recalcAll();
return _missionItems->count() - 1;
}
void MissionEditor::_reSequence(void)
{
for (int i=0; i<_missionItems->count(); i++) {
qobject_cast<MissionItem*>(_missionItems->get(i))->setSequenceNumber(i);
}
}
void MissionEditor::removeMissionItem(int index)
{
if (!_canEdit) {
......@@ -136,8 +128,11 @@ void MissionEditor::removeMissionItem(int index)
return;
}
_missionItems->removeAt(index);
_reSequence();
MissionItem* item = qobject_cast<MissionItem*>(_missionItems->removeAt(index));
_deinitMissionItem(item);
_recalcAll();
}
void MissionEditor::moveUp(int index)
......@@ -160,7 +155,7 @@ void MissionEditor::moveUp(int index)
_missionItems->insert(index - 1, new MissionItem(item2, _missionItems));
_missionItems->insert(index, new MissionItem(item1, _missionItems));
_reSequence();
_recalcAll();
}
void MissionEditor::moveDown(int index)
......@@ -183,7 +178,7 @@ void MissionEditor::moveDown(int index)
_missionItems->insert(index, new MissionItem(item2, _missionItems));
_missionItems->insert(index + 1, new MissionItem(item1, _missionItems));
_reSequence();
_recalcAll();
}
void MissionEditor::loadMissionFromFile(void)
......@@ -196,6 +191,7 @@ void MissionEditor::loadMissionFromFile(void)
}
if (_missionItems) {
_deinitAllMissionItems();
_missionItems->deleteLater();
}
_missionItems = new QmlObjectListModel(this);
......@@ -236,11 +232,7 @@ void MissionEditor::loadMissionFromFile(void)
_missionItems->clear();
}
_missionItems->setDirty(false);
emit canEditChanged(_canEdit);
connect(_missionItems, &QmlObjectListModel::dirtyChanged, this, &MissionEditor::_missionListDirtyChanged);
_rebuildWaypointLines();
_initAllMissionItems();
}
void MissionEditor::saveMissionToFile(void)
......@@ -269,20 +261,131 @@ void MissionEditor::saveMissionToFile(void)
_missionItems->setDirty(false);
}
void MissionEditor::_rebuildWaypointLines(void)
void MissionEditor::_recalcWaypointLines(void)
{
bool firstCoordinateItem = true;
MissionItem* lastCoordinateItem = qobject_cast<MissionItem*>(_missionItems->get(0));
_waypointLines.clear();
for (int i=1; i<_missionItems->count(); i++) {
MissionItem* item1 = qobject_cast<MissionItem*>(_missionItems->get(i-1));
MissionItem* item2 = qobject_cast<MissionItem*>(_missionItems->get(i));
MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(i));
_waypointLines.append(new CoordinateVector(item1->coordinate(), item2->coordinate()));
if (item->specifiesCoordinate()) {
if (firstCoordinateItem) {
if (item->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF) {
// The first coordinate we hit is a takeoff command so link back to home position
_waypointLines.append(new CoordinateVector(qobject_cast<MissionItem*>(_missionItems->get(0))->coordinate(), item->coordinate()));
} else {
// First coordiante is not a takeoff command, it does not link backwards to anything
}
firstCoordinateItem = false;
} else {
// Subsequent coordinate items link to last coordinate item
_waypointLines.append(new CoordinateVector(lastCoordinateItem->coordinate(), item->coordinate()));
}
lastCoordinateItem = item;
}
}
emit waypointLinesChanged();
}
void MissionEditor::_missionListDirtyChanged(bool dirty)
// This will update the sequence numbers to be sequential starting from 0
void MissionEditor::_recalcSequence(void)
{
MissionItem* currentParentItem = qobject_cast<MissionItem*>(_missionItems->get(0));
currentParentItem->childItems()->clear();
for (int i=0; i<_missionItems->count(); i++) {
MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(i));
// Setup ascending sequence numbers
item->setSequenceNumber(i);
}
}
// This will update the child item hierarchy
void MissionEditor::_recalcChildItems(void)
{
MissionItem* currentParentItem = qobject_cast<MissionItem*>(_missionItems->get(0));
currentParentItem->childItems()->clear();
for (int i=1; i<_missionItems->count(); i++) {
MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(i));
// Set up non-coordinate item child hierarchy
if (item->specifiesCoordinate()) {
item->childItems()->clear();
currentParentItem = item;
} else {
currentParentItem->childItems()->append(item);
}
}
}
void MissionEditor::_recalcAll(void)
{
_recalcSequence();
_recalcChildItems();
_recalcWaypointLines();
}
/// Initializes a new set of mission items which may have come from the vehicle or have been loaded from a file
void MissionEditor::_initAllMissionItems(void)
{
// Add the home position item to the front
MissionItem* homeItem = new MissionItem(this);
homeItem->setHomePositionSpecialCase(true);
homeItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_WAYPOINT);
_missionItems->insert(0, homeItem);
for (int i=0; i<_missionItems->count(); i++) {
_initMissionItem(qobject_cast<MissionItem*>(_missionItems->get(i)));
}
_recalcSequence();
_recalcChildItems();
_recalcWaypointLines();
emit missionItemsChanged();
emit canEditChanged(_canEdit);
_missionItems->setDirty(false);
}
void MissionEditor::_deinitAllMissionItems(void)
{
for (int i=0; i<_missionItems->count(); i++) {
_deinitMissionItem(qobject_cast<MissionItem*>(_missionItems->get(i)));
}
}
void MissionEditor::_initMissionItem(MissionItem* item)
{
_missionItems->setDirty(false);
connect(item, &MissionItem::commandChanged, this, &MissionEditor::_itemCommandChanged);
connect(item, &MissionItem::coordinateChanged, this, &MissionEditor::_itemCoordinateChanged);
}
void MissionEditor::_deinitMissionItem(MissionItem* item)
{
disconnect(item, &MissionItem::commandChanged, this, &MissionEditor::_itemCommandChanged);
disconnect(item, &MissionItem::coordinateChanged, this, &MissionEditor::_itemCoordinateChanged);
}
void MissionEditor::_itemCoordinateChanged(const QGeoCoordinate& coordinate)
{
Q_UNUSED(coordinate);
_recalcWaypointLines();
}
void MissionEditor::_itemCommandChanged(MavlinkQmlSingleton::Qml_MAV_CMD command)
{
Q_UNUSED(dirty);
_rebuildWaypointLines();
Q_UNUSED(command);;
_recalcChildItems();
_recalcWaypointLines();
}
......@@ -61,12 +61,19 @@ signals:
private slots:
void _newMissionItemsAvailable();
void _missionListDirtyChanged(bool dirty);
void _itemCoordinateChanged(const QGeoCoordinate& coordinate);
void _itemCommandChanged(MavlinkQmlSingleton::Qml_MAV_CMD command);
private:
void _reSequence(void);
void _rebuildWaypointLines(void);
void _recalcSequence(void);
void _recalcWaypointLines(void);
void _recalcChildItems(void);
void _recalcAll(void);
void _initAllMissionItems(void);
void _deinitAllMissionItems(void);
void _initMissionItem(MissionItem* item);
void _deinitMissionItem(MissionItem* item);
private:
QmlObjectListModel* _missionItems;
QmlObjectListModel _waypointLines;
......
This diff is collapsed.
......@@ -84,6 +84,7 @@ MissionItem::MissionItem(QObject* parent,
, _reachedTime(0)
, _yawRadiansFact(NULL)
,_dirty(false)
, _homePositionSpecialCase(false)
{
_latitudeFact = new Fact(0, "Latitude:", FactMetaData::valueTypeDouble, this);
_longitudeFact = new Fact(0, "Longitude:", FactMetaData::valueTypeDouble, this);
......@@ -219,6 +220,7 @@ const MissionItem& MissionItem::operator=(const MissionItem& other)
_reachedTime = other._reachedTime;
_altitudeRelativeToHomeFact = other._altitudeRelativeToHomeFact;
_dirty = other._dirty;
_homePositionSpecialCase = other._homePositionSpecialCase;
*_latitudeFact = *other._latitudeFact;
*_longitudeFact = *other._longitudeFact;
......@@ -664,9 +666,11 @@ QmlObjectListModel* MissionItem::textFieldFacts(void)
model->append(_latitudeFact);
model->append(_longitudeFact);
model->append(_altitudeFact);
model->append(_yawRadiansFact);
model->append(_param2Fact);
model->append(_param1Fact);
if (!_homePositionSpecialCase) {
model->append(_yawRadiansFact);
model->append(_param2Fact);
model->append(_param1Fact);
}
break;
case MAV_CMD_NAV_LOITER_UNLIM:
model->append(_latitudeFact);
......@@ -736,7 +740,9 @@ QmlObjectListModel* MissionItem::checkboxFacts(void)
switch ((MAV_CMD)_command) {
case MAV_CMD_NAV_WAYPOINT:
model->append(_altitudeRelativeToHomeFact);
if (!_homePositionSpecialCase) {
model->append(_altitudeRelativeToHomeFact);
}
break;
case MAV_CMD_NAV_LOITER_UNLIM:
model->append(_altitudeRelativeToHomeFact);
......
......@@ -36,6 +36,7 @@
#include "QmlObjectListModel.h"
#include "Fact.h"
#include "QGCLoggingCategory.h"
#include "QmlObjectListModel.h"
Q_DECLARE_LOGGING_CATEGORY(MissionItemLog)
......@@ -76,6 +77,7 @@ public:
Q_PROPERTY(QmlObjectListModel* textFieldFacts READ textFieldFacts NOTIFY commandChanged)
Q_PROPERTY(QmlObjectListModel* checkboxFacts READ checkboxFacts NOTIFY commandChanged)
Q_PROPERTY(MavlinkQmlSingleton::Qml_MAV_CMD command READ command WRITE setCommand NOTIFY commandChanged)
Q_PROPERTY(QmlObjectListModel* childItems READ childItems CONSTANT)
// Property accesors
......@@ -111,6 +113,8 @@ public:
bool dirty(void) { return _dirty; }
void setDirty(bool dirty);
QmlObjectListModel* childItems(void) { return &_childItems; }
// C++ only methods
/// Returns true if this item can be edited in the ui
......@@ -184,6 +188,8 @@ public:
void save(QTextStream &saveStream);
bool load(QTextStream &loadStream);
void setHomePositionSpecialCase(bool homePositionSpecialCase) { _homePositionSpecialCase = homePositionSpecialCase; }
signals:
void sequenceNumberChanged(int sequenceNumber);
void isCurrentItemChanged(bool isCurrentItem);
......@@ -265,6 +271,11 @@ private:
bool _dirty;
bool _homePositionSpecialCase; ///< true: this item is being used as a ui home position indicator
/// This is used to reference any subsequent mission items which do not specify a coordinate.
QmlObjectListModel _childItems;
static const int _cMavCmd2Name = 9;
static const MavCmd2Name_t _rgMavCmd2Name[_cMavCmd2Name];
};
......
......@@ -53,13 +53,24 @@ MissionManager::~MissionManager()
}
void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems)
void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems, bool skipFirstItem)
{
_retryCount = 0;
_missionItems.clear();
for (int i=0; i<missionItems.count(); i++) {
for (int i=skipFirstItem ? 1: 0; i<missionItems.count(); i++) {
_missionItems.append(new MissionItem(*qobject_cast<const MissionItem*>(missionItems[i])));
}
if (skipFirstItem) {
for (int i=0; i<_missionItems.count(); i++) {
MissionItem* item = qobject_cast<MissionItem*>(_missionItems[i]);
if (item->command() == MavlinkQmlSingleton::MAV_CMD_CONDITION_DELAY) {
item->setParam1((int)item->param1() - 1);
}
}
}
qCDebug(MissionManagerLog) << "writeMissionItems count:" << _missionItems.count();
......@@ -370,24 +381,24 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message)
mavlink_msg_mission_ack_decode(&message, &missionAck);
qCDebug(MissionManagerLog) << "_handleMissionAck type:" << missionAck.type;
qCDebug(MissionManagerLog) << "_handleMissionAck type:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type);
switch (savedRetryAck) {
case AckNone:
// State machine is idle. Vehicle is confused.
qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack while state machine is idle: error:" << missionAck.type;
qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack while state machine is idle: error:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type);
_sendError(VehicleError, QString("Vehicle sent unexpected MISSION_ACK message, error: %1").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
break;
case AckMissionCount:
// MISSION_COUNT message expected
qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack when MISSION_COUNT expected: error:" << missionAck.type;
qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack when MISSION_COUNT expected: error:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type);
if (!_retrySequence(AckMissionCount)) {
_sendError(VehicleError, QString("Vehicle returned error: %1. Partial list of mission items may have been returned.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
}
break;
case AckMissionItem:
// MISSION_ITEM expected
qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack when MISSION_ITEM expected: error:" << missionAck.type;
qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack when MISSION_ITEM expected: error:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type);
if (!_retrySequence(AckMissionItem)) {
_sendError(VehicleError, QString("Vehicle returned error: %1. Partial list of mission items may have been returned.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
}
......@@ -405,7 +416,7 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message)
}
}
} else {
qCDebug(MissionManagerLog) << "_handleMissionAck ack error:" << missionAck.type;
qCDebug(MissionManagerLog) << "_handleMissionAck ack error:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type);
if (!_retrySequence(AckMissionRequest)) {
_sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle only has partial list of mission items.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type)));
}
......
......@@ -62,7 +62,9 @@ public:
void requestMissionItems(void);
/// Writes the specified set of mission items to the vehicle
void writeMissionItems(const QmlObjectListModel& missionItems);
/// @oaram missionItems Items to send to vehicle
/// @param skipFirstItem true: Don't send first item
void writeMissionItems(const QmlObjectListModel& missionItems, bool skipFirstItem);
/// Returns a copy of the current set of mission items. Caller is responsible for
/// freeing returned object.
......
......@@ -158,7 +158,7 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f
}
// Send the items to the vehicle
_missionManager->writeMissionItems(*list);
_missionManager->writeMissionItems(*list, false /* skipFirstItem */);
// writeMissionItems should emit inProgressChanged signal before returning so no need to wait for it
QVERIFY(_missionManager->inProgress());
......
......@@ -53,7 +53,7 @@ Rectangle {
id: label
anchors.verticalCenter: commandPicker.verticalCenter
isCurrentItem: missionItem.isCurrentItem
label: missionItem.sequenceNumber
label: missionItem.sequenceNumber == 0 ? "H" : missionItem.sequenceNumber
}
MouseArea {
......@@ -71,10 +71,26 @@ Rectangle {
anchors.right: parent.right
currentIndex: missionItem.commandByIndex
model: missionItem.commandNames
visible: missionItem.sequenceNumber != 0
onActivated: missionItem.commandByIndex = index
}
Rectangle {
anchors.fill: commandPicker
color: qgcPal.button
visible: missionItem.sequenceNumber == 0
QGCLabel {
id: homeLabel
anchors.leftMargin: ScreenTools.defaultFontPixelWidth
anchors.fill: parent
verticalAlignment: Text.AlignVCenter
text: "Home"
color: qgcPal.buttonText
}
}
Rectangle {
anchors.topMargin: _margin
anchors.top: commandPicker.bottom
......
......@@ -35,6 +35,7 @@ const int QmlObjectListModel::TextRole = Qt::UserRole + 1;
QmlObjectListModel::QmlObjectListModel(QObject* parent)
: QAbstractListModel(parent)
, _dirty(false)
, _skipDirtyFirstItem(false)
{
}
......@@ -147,16 +148,22 @@ void QmlObjectListModel::clear(void)
}
}
void QmlObjectListModel::removeAt(int i)
QObject* QmlObjectListModel::removeAt(int i)
{
QObject* removedObject = _objectList[i];
// Look for a dirtyChanged signal on the object
if (_objectList[i]->metaObject()->indexOfSignal(QMetaObject::normalizedSignature("dirtyChanged(bool)")) != -1) {
QObject::disconnect(_objectList[i], SIGNAL(dirtyChanged(bool)), this, SLOT(_childDirtyChanged(bool)));
if (!_skipDirtyFirstItem || i != 0) {
QObject::disconnect(_objectList[i], SIGNAL(dirtyChanged(bool)), this, SLOT(_childDirtyChanged(bool)));
}
}
removeRows(i, 1);
setDirty(true);
return removedObject;
}
void QmlObjectListModel::insert(int i, QObject* object)
......@@ -169,7 +176,9 @@ void QmlObjectListModel::insert(int i, QObject* object)
// Look for a dirtyChanged signal on the object
if (object->metaObject()->indexOfSignal(QMetaObject::normalizedSignature("dirtyChanged(bool)")) != -1) {
QObject::connect(object, SIGNAL(dirtyChanged(bool)), this, SLOT(_childDirtyChanged(bool)));
if (!_skipDirtyFirstItem || i != 0) {
QObject::connect(object, SIGNAL(dirtyChanged(bool)), this, SLOT(_childDirtyChanged(bool)));
}
}
_objectList.insert(i, object);
......
......@@ -51,7 +51,7 @@ public:
void append(QObject* object);
void clear(void);
void removeAt(int i);
QObject* removeAt(int i);
void insert(int i, QObject* object);
QObject* operator[](int i);
const QObject* operator[](int i) const;
......@@ -79,6 +79,7 @@ private:
QList<QObject*> _objectList;
bool _dirty;
bool _skipDirtyFirstItem;
static const int ObjectRole;
static const int TextRole;
......
......@@ -1960,15 +1960,15 @@ void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObj
}
float noise_scaler = 0.05f;
xacc_var = noise_scaler * 1.2914f;
yacc_var = noise_scaler * 0.7048f;
zacc_var = noise_scaler * 1.9577f;
rollspeed_var = noise_scaler * 0.8126f;
pitchspeed_var = noise_scaler * 0.6145f;
yawspeed_var = noise_scaler * 0.5852f;
xmag_var = noise_scaler * 0.4786f;
ymag_var = noise_scaler * 0.4566f;
zmag_var = noise_scaler * 0.3333f;
xacc_var = noise_scaler * 0.2914f;
yacc_var = noise_scaler * 0.2914f;
zacc_var = noise_scaler * 0.9577f;
rollspeed_var = noise_scaler * 0.1f * 0.8126f;
pitchspeed_var = noise_scaler * 0.1f * 0.6145f;
yawspeed_var = noise_scaler * 0.1f * 0.5852f;
xmag_var = noise_scaler * 0.0786f;
ymag_var = noise_scaler * 0.0566f;
zmag_var = noise_scaler * 0.0333f;
abs_pressure_var = noise_scaler * 1.1604f;
diff_pressure_var = noise_scaler * 0.6604f;
pressure_alt_var = noise_scaler * 1.1604f;
......@@ -2034,16 +2034,16 @@ void UAS::enableHilXPlane(bool enable)
}
simulation = new QGCXPlaneLink(this);
float noise_scaler = 0.05f;
xacc_var = noise_scaler * 1.2914f;
yacc_var = noise_scaler * 0.7048f;
zacc_var = noise_scaler * 1.9577f;
rollspeed_var = noise_scaler * 0.8126f;
pitchspeed_var = noise_scaler * 0.6145f;
yawspeed_var = noise_scaler * 0.5852f;
xmag_var = noise_scaler * 0.4786f;
ymag_var = noise_scaler * 0.4566f;
zmag_var = noise_scaler * 0.3333f;
float noise_scaler = 0.02f;
xacc_var = noise_scaler * 0.2914f;
yacc_var = noise_scaler * 0.2914f;
zacc_var = noise_scaler * 0.9577f;
rollspeed_var = noise_scaler * 0.15f * 0.8126f;
pitchspeed_var = noise_scaler * 0.15f * 0.6145f;
yawspeed_var = noise_scaler * 0.15f * 0.5852f;
xmag_var = noise_scaler * 0.0786f;
ymag_var = noise_scaler * 0.0566f;
zmag_var = noise_scaler * 0.0333f;
abs_pressure_var = noise_scaler * 1.1604f;
diff_pressure_var = noise_scaler * 0.6604f;
pressure_alt_var = noise_scaler * 1.1604f;
......
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