Skip to content

Q
qgroundcontrol
Commit f01ad4a2 authored by LM's avatar LM
Browse files
Options

Runtime fixes for multi-IMU systems

parent 921870e4
Hide whitespace changes
Inline Side-by-side
Showing with 57 additions and 82 deletions
src/comm/MAVLinkProtocol.cc
View file @ f01ad4a2
...@@ -372,7 +372,8 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) ...@@ -372,7 +372,8 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
{ {
lostMessages += 256; lostMessages += 256;
} }
qDebug() << QString("Lost %1 messages: expected sequence ID %2 but received %3.").arg(lostMessages).arg(expectedIndex).arg(message.seq); // Console generates excessive load at high loss rates, needs better GUI visualization
//qDebug() << QString("Lost %1 messages for comp %4: expected sequence ID %2 but received %3.").arg(lostMessages).arg(expectedIndex).arg(message.seq).arg(message.compid);
totalLossCounter += lostMessages; totalLossCounter += lostMessages;
currLossCounter += lostMessages; currLossCounter += lostMessages;
......
src/uas/UAS.cc
View file @ f01ad4a2
...@@ -360,7 +360,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -360,7 +360,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
{ {
emit navModeChanged(uasId, state.custom_mode, getNavModeText(state.custom_mode)); emit navModeChanged(uasId, state.custom_mode, getNavModeText(state.custom_mode));
navMode = state.custom_mode; navMode = state.custom_mode;
navModeAudio = tr(" changed nav mode to ") + tr("FIXME"); //navModeAudio = tr(" changed nav mode to ") + tr("FIXME");
} }
// AUDIO // AUDIO
...@@ -517,9 +517,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -517,9 +517,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
// Emit position always with component ID // Emit position always with component ID
emit localPositionChanged(this, message.compid, pos.x, pos.y, pos.z, time); emit localPositionChanged(this, message.compid, pos.x, pos.y, pos.z, time);
if (!wrongComponent) if (!wrongComponent)
{ {
localX = pos.x; localX = pos.x;
localY = pos.y; localY = pos.y;
localZ = pos.z; localZ = pos.z;
......
src/ui/HSIDisplay.cc
View file @ f01ad4a2
...@@ -85,7 +85,7 @@ HSIDisplay::HSIDisplay(QWidget *parent) : ...@@ -85,7 +85,7 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
bodyYawSet(0.0f), bodyYawSet(0.0f),
uiXSetCoordinate(0.0f), uiXSetCoordinate(0.0f),
uiYSetCoordinate(0.0f), uiYSetCoordinate(0.0f),
uiZSetCoordinate(-0.51f), uiZSetCoordinate(0.0f),
uiYawSet(0.0f), uiYawSet(0.0f),
metricWidth(4.0), metricWidth(4.0),
positionLock(false), positionLock(false),
...@@ -106,7 +106,9 @@ HSIDisplay::HSIDisplay(QWidget *parent) : ...@@ -106,7 +106,9 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
actionPending(false), actionPending(false),
directSending(false), directSending(false),
userSetPointSet(false), userSetPointSet(false),
userXYSetPointSet(false) userXYSetPointSet(false),
userZSetPointSet(false),
userYawSetPointSet(false)
{ {
refreshTimer->setInterval(updateInterval); refreshTimer->setInterval(updateInterval);
...@@ -580,6 +582,8 @@ void HSIDisplay::mouseDoubleClickEvent(QMouseEvent * event) ...@@ -580,6 +582,8 @@ void HSIDisplay::mouseDoubleClickEvent(QMouseEvent * event)
void HSIDisplay::mouseReleaseEvent(QMouseEvent * event) void HSIDisplay::mouseReleaseEvent(QMouseEvent * event)
{ {
// FIXME hardcode yaw to current value
setBodySetpointCoordinateYaw(0);
if (mouseHasMoved) if (mouseHasMoved)
{ {
if (event->type() == QMouseEvent::MouseButtonRelease && event->button() == Qt::RightButton) if (event->type() == QMouseEvent::MouseButtonRelease && event->button() == Qt::RightButton)
...@@ -648,106 +652,74 @@ void HSIDisplay::keyPressEvent(QKeyEvent* event) ...@@ -648,106 +652,74 @@ void HSIDisplay::keyPressEvent(QKeyEvent* event)
statusClearTimer.start(); statusClearTimer.start();
sendBodySetPointCoordinates(); sendBodySetPointCoordinates();
} }
else
if ((event->key() == Qt::Key_W))
{ {
if (!directSending) // FIXME hardcode yaw to current value
setBodySetpointCoordinateYaw(0);
// Reset setpoints to current position / orientation
// if not initialized
if (!userYawSetPointSet)
{ {
setBodySetpointCoordinateZ(uas->getLocalZ()); setBodySetpointCoordinateYaw(0);
setBodySetpointCoordinateYaw(uas->getYaw());
setBodySetpointCoordinateXY(1.0, 0);
} }
else
if (!userZSetPointSet)
{ {
setBodySetpointCoordinateXY(qBound(-1.5, bodySP.x()+0.2, +1.5), bodySP.y()); setBodySetpointCoordinateZ(0);
} }
} if (!userXYSetPointSet)
if ((event->key() == Qt::Key_S))
{
if (!directSending)
{ {
setBodySetpointCoordinateZ(uas->getLocalZ()); setBodySetpointCoordinateXY(0, 0);
setBodySetpointCoordinateYaw(uas->getYaw());
setBodySetpointCoordinateXY(-1.0, 0);
} }
else
if ((event->key() == Qt::Key_W))
{ {
setBodySetpointCoordinateXY(qBound(-1.5, bodySP.x()-0.2, +1.5), bodySP.y()); setBodySetpointCoordinateXY(qBound(-1.5, bodySP.x()+0.2, +1.5), bodySP.y());
} }
}
if ((event->key() == Qt::Key_A)) if ((event->key() == Qt::Key_S))
{
if (!directSending)
{ {
setBodySetpointCoordinateZ(uas->getLocalZ()); setBodySetpointCoordinateXY(qBound(-1.5, bodySP.x()-0.2, +1.5), bodySP.y());
setBodySetpointCoordinateYaw(uas->getYaw());
setBodySetpointCoordinateXY(0, -1.0);
} }
else
if ((event->key() == Qt::Key_A))
{ {
setBodySetpointCoordinateXY(bodySP.x(), qBound(-1.5, bodySP.y()-0.2, +1.5)); setBodySetpointCoordinateXY(bodySP.x(), qBound(-1.5, bodySP.y()-0.2, +1.5));
} }
}
if ((event->key() == Qt::Key_D)) if ((event->key() == Qt::Key_D))
{
if (!directSending)
{
setBodySetpointCoordinateZ(uas->getLocalZ());
setBodySetpointCoordinateYaw(uas->getYaw());
setBodySetpointCoordinateXY(0, 1.0);
}
else
{ {
setBodySetpointCoordinateXY(bodySP.x(), qBound(-1.5, bodySP.y()+0.2, +1.5)); setBodySetpointCoordinateXY(bodySP.x(), qBound(-1.5, bodySP.y()+0.2, +1.5));
} }
}
if ((event->key() == Qt::Key_Up)) if ((event->key() == Qt::Key_Up))
{
if (!directSending)
{ {
setBodySetpointCoordinateXY(0, 0); setBodySetpointCoordinateZ(-0.5);
setBodySetpointCoordinateYaw(uas->getYaw());
} }
setBodySetpointCoordinateZ(-0.5);
}
if ((event->key() == Qt::Key_Down)) if ((event->key() == Qt::Key_Down))
{
if (!directSending)
{ {
setBodySetpointCoordinateXY(0, 0); setBodySetpointCoordinateZ(+0.5);
setBodySetpointCoordinateYaw(uas->getYaw());
} }
setBodySetpointCoordinateZ(+0.5);
}
if ((event->key() == Qt::Key_Left)) if ((event->key() == Qt::Key_Left))
{
if (!directSending)
{ {
setBodySetpointCoordinateXY(0, 0); setBodySetpointCoordinateYaw(-0.2);
setBodySetpointCoordinateZ(uas->getLocalZ());
} }
setBodySetpointCoordinateYaw(-0.2);
}
if ((event->key() == Qt::Key_Right)) if ((event->key() == Qt::Key_Right))
{
if (!directSending)
{ {
setBodySetpointCoordinateXY(0, 0); setBodySetpointCoordinateYaw(0.2);
setBodySetpointCoordinateZ(uas->getLocalZ());
} }
setBodySetpointCoordinateYaw(0.2);
} }
if ((event->key() == Qt::Key_Escape)) // Overwrite any existing non-zero body setpoints
// for escape
if ((event->key() == Qt::Key_Escape) || (event->key() == Qt::Key_R))
{ {
setBodySetpointCoordinateXY(0, 0); setBodySetpointCoordinateXY(0, 0);
setBodySetpointCoordinateZ(0); setBodySetpointCoordinateZ(0);
...@@ -877,6 +849,7 @@ void HSIDisplay::setBodySetpointCoordinateZ(double z) ...@@ -877,6 +849,7 @@ void HSIDisplay::setBodySetpointCoordinateZ(double z)
if (uas) if (uas)
{ {
userSetPointSet = true; userSetPointSet = true;
userZSetPointSet = true;
// Set coordinates and send them out to MAV // Set coordinates and send them out to MAV
uiZSetCoordinate = z+uas->getLocalZ(); uiZSetCoordinate = z+uas->getLocalZ();
statusMessage = "Z SET, PRESS <ENTER> TO SEND"; statusMessage = "Z SET, PRESS <ENTER> TO SEND";
...@@ -902,6 +875,7 @@ void HSIDisplay::setBodySetpointCoordinateYaw(double yaw) ...@@ -902,6 +875,7 @@ void HSIDisplay::setBodySetpointCoordinateYaw(double yaw)
uiYSetCoordinate = coord.y(); uiYSetCoordinate = coord.y();
} }
userSetPointSet = true; userSetPointSet = true;
userYawSetPointSet = true;
// Set coordinates and send them out to MAV // Set coordinates and send them out to MAV
uiYawSet = atan2(sin(yaw+uas->getYaw()), cos(yaw+uas->getYaw())); uiYawSet = atan2(sin(yaw+uas->getYaw()), cos(yaw+uas->getYaw()));
statusMessage = "YAW SET, PRESS <ENTER> TO SEND"; statusMessage = "YAW SET, PRESS <ENTER> TO SEND";
...@@ -919,15 +893,10 @@ void HSIDisplay::sendBodySetPointCoordinates() ...@@ -919,15 +893,10 @@ void HSIDisplay::sendBodySetPointCoordinates()
double dx = uiXSetCoordinate - uas->getLocalX(); double dx = uiXSetCoordinate - uas->getLocalX();
double dy = uiYSetCoordinate - uas->getLocalY(); double dy = uiYSetCoordinate - uas->getLocalY();
double dz = uiZSetCoordinate - uas->getLocalZ(); double dz = uiZSetCoordinate - uas->getLocalZ();
bool valid = (sqrt(dx*dx + dy*dy + dz*dz) < 2.0);//UASManager::instance()->isInLocalNEDSafetyLimits(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate); bool valid = (sqrt(dx*dx + dy*dy + dz*dz) < 200.0);//UASManager::instance()->isInLocalNEDSafetyLimits(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate);
if (valid) // if (valid)
{ // {
uas->setLocalPositionSetpoint(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet); uas->setLocalPositionSetpoint(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet);
}
else
{
setStatusMessage("REJECTED NEW SETPOINT: OUT OF BOUNDS");
}
} }
} }
...@@ -958,6 +927,8 @@ void HSIDisplay::updateUserPositionSetpoints(int uasid, float xDesired, float yD ...@@ -958,6 +927,8 @@ void HSIDisplay::updateUserPositionSetpoints(int uasid, float xDesired, float yD
uiZSetCoordinate = zDesired; uiZSetCoordinate = zDesired;
uiYawSet = yawDesired; uiYawSet = yawDesired;
userXYSetPointSet = true; userXYSetPointSet = true;
userZSetPointSet = true;
userYawSetPointSet = true;
userSetPointSet = true; userSetPointSet = true;
} }
......
src/ui/HSIDisplay.h
View file @ f01ad4a2
...@@ -294,6 +294,8 @@ protected: ...@@ -294,6 +294,8 @@ protected:
bool positionSetPointKnown; ///< Position setpoint known status flag bool positionSetPointKnown; ///< Position setpoint known status flag
bool userSetPointSet; ///< User set X, Y and Z bool userSetPointSet; ///< User set X, Y and Z
bool userXYSetPointSet; ///< User set the X/Y position already bool userXYSetPointSet; ///< User set the X/Y position already
bool userZSetPointSet; ///< User set the Z position already
bool userYawSetPointSet; ///< User set the YAW position already
private: private:
}; };
......
src/ui/map3D/Pixhawk3DWidget.cc
View file @ f01ad4a2
...@@ -149,9 +149,6 @@ Pixhawk3DWidget::localPositionChanged(UASInterface* uas, int component, ...@@ -149,9 +149,6 @@ Pixhawk3DWidget::localPositionChanged(UASInterface* uas, int component,
SystemContainer& systemData = mSystemContainerMap[systemId]; SystemContainer& systemData = mSystemContainerMap[systemId];
// update system position
m3DWidget->systemGroup(systemId)->position()->setPosition(osg::Vec3d(y, x, -z));
// update trail data // update trail data
if (!systemData.trailMap().contains(component)) if (!systemData.trailMap().contains(component))
{ {
...@@ -167,6 +164,10 @@ Pixhawk3DWidget::localPositionChanged(UASInterface* uas, int component, ...@@ -167,6 +164,10 @@ Pixhawk3DWidget::localPositionChanged(UASInterface* uas, int component,
systemData.trailNode()->addDrawable(createTrail(color)); systemData.trailNode()->addDrawable(createTrail(color));
} }
// update system position
// FIXME
if (component == 201) m3DWidget->systemGroup(systemId)->position()->setPosition(osg::Vec3d(y, x, -z));
QVector<osg::Vec3d>& trail = systemData.trailMap()[component]; QVector<osg::Vec3d>& trail = systemData.trailMap()[component];
bool addToTrail = false; bool addToTrail = false;
...@@ -685,7 +686,6 @@ Pixhawk3DWidget::update(void) ...@@ -685,7 +686,6 @@ Pixhawk3DWidget::update(void)
{ {
double x = 0.0, y = 0.0, z = 0.0; double x = 0.0, y = 0.0, z = 0.0;
getPosition(mActiveUAS, frame, x, y, z); getPosition(mActiveUAS, frame, x, y, z);
m3DWidget->recenterCamera(y, x, -z); m3DWidget->recenterCamera(y, x, -z);
mCameraPos = QVector3D(x, y, z); mCameraPos = QVector3D(x, y, z);
...@@ -1465,6 +1465,7 @@ Pixhawk3DWidget::resizeHUD(int width, int height) ...@@ -1465,6 +1465,7 @@ Pixhawk3DWidget::resizeHUD(int width, int height)
void void
Pixhawk3DWidget::updateHUD(UASInterface* uas, MAV_FRAME frame) Pixhawk3DWidget::updateHUD(UASInterface* uas, MAV_FRAME frame)
{ {
if (!uas) return;
// display pose of current system // display pose of current system
double x = 0.0; double x = 0.0;
double y = 0.0; double y = 0.0;
......
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