Skip to content

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

Enabled offset message, fixed line charts for swarm use

parent d5c30100
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 481 additions and 81 deletions
src/input/JoystickInput.cc
View file @ b302c815
......@@ -95,7 +95,7 @@ void JoystickInput::init()
// Wait for joysticks if none is connected
while (numJoysticks == 0)
{
MG::SLEEP::msleep(800);
QGC::SLEEP::msleep(800);
// INITIALIZE SDL Joystick support
if (SDL_InitSubSystem(SDL_INIT_JOYSTICK | SDL_INIT_NOPARACHUTE) < 0)
{
......
src/uas/UAS.cc
View file @ b302c815
......@@ -94,7 +94,9 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
simulation(new QGCFlightGearLink(this)),
isLocalPositionKnown(false),
isGlobalPositionKnown(false),
systemIsArmed(false)
systemIsArmed(false),
nedPosGlobalOffset(0,0,0),
nedAttGlobalOffset(0,0,0)
{
for (unsigned int i = 0; i<255;++i)
{
......@@ -440,7 +442,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
if ((startVoltage > 0.0f) && (tickLowpassVoltage < tickVoltage) && (fabs(lastTickVoltageValue - tickLowpassVoltage) > 0.1f)
&& (lpVoltage < tickVoltage))
{
GAudioOutput::instance()->say(QString("voltage warning: %1 volt").arg(lpVoltage, 2, 'f', 0, QChar(' ')));
GAudioOutput::instance()->say(QString("voltage warning: %1 volt").arg(lpVoltage, 0, 'f', 1, QChar(' ')));
lastTickVoltageValue = tickLowpassVoltage;
}
......@@ -528,6 +530,18 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
}
}
break;
case MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET:
{
mavlink_local_position_ned_system_global_offset_t offset;
mavlink_msg_local_position_ned_system_global_offset_decode(&message, &offset);
nedPosGlobalOffset.setX(offset.x);
nedPosGlobalOffset.setY(offset.y);
nedPosGlobalOffset.setZ(offset.z);
nedAttGlobalOffset.setX(offset.roll);
nedAttGlobalOffset.setY(offset.pitch);
nedAttGlobalOffset.setZ(offset.yaw);
}
break;
case MAVLINK_MSG_ID_HIL_CONTROLS:
{
mavlink_hil_controls_t hil;
......
src/uas/UAS.h
View file @ b302c815
......@@ -33,8 +33,8 @@ This file is part of the QGROUNDCONTROL project
#define _UAS_H_
#include "UASInterface.h"
#include "MG.h"
#include <MAVLinkProtocol.h>
#include <QVector3D>
#include "QGCMAVLink.h"
#include "QGCFlightGearLink.h"
......@@ -53,7 +53,8 @@ public:
UAS(MAVLinkProtocol* protocol, int id = 0);
~UAS();
enum BatteryType {
enum BatteryType
{
NICD = 0,
NIMH = 1,
LIION = 2,
......@@ -80,7 +81,8 @@ public:
/** @brief Get the unique system id */
int getUASID() const;
/** @brief Get the airframe */
int getAirframe() const {
int getAirframe() const
{
return airframe;
}
/** @brief Get the components */
......@@ -107,13 +109,16 @@ public:
{
return localZ;
}
double getLatitude() const {
double getLatitude() const
{
return latitude;
}
double getLongitude() const {
double getLongitude() const
{
return longitude;
}
double getAltitude() const {
double getAltitude() const
{
return altitude;
}
virtual bool localPositionKnown() const
......@@ -125,24 +130,39 @@ public:
return isGlobalPositionKnown;
}
double getRoll() const {
double getRoll() const
{
return roll;
}
double getPitch() const {
double getPitch() const
{
return pitch;
}
double getYaw() const {
double getYaw() const
{
return yaw;
}
bool getSelected() const;
QVector3D getNedPosGlobalOffset() const
{
return nedPosGlobalOffset;
}
QVector3D getNedAttGlobalOffset() const
{
return nedAttGlobalOffset;
}
#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
px::GLOverlay getOverlay() {
px::GLOverlay getOverlay()
{
QMutexLocker locker(&overlayMutex);
return overlay;
}
px::GLOverlay getOverlay(qreal& receivedTimestamp) {
px::GLOverlay getOverlay(qreal& receivedTimestamp)
{
receivedTimestamp = receivedOverlayTimestamp;
QMutexLocker locker(&overlayMutex);
return overlay;
......@@ -315,6 +335,8 @@ protected: //COMMENTS FOR TEST UNIT
bool isLocalPositionKnown; ///< If the local position has been received for this MAV
bool isGlobalPositionKnown; ///< If the global position has been received for this MAV
bool systemIsArmed; ///< If the system is armed
QVector3D nedPosGlobalOffset; ///< Offset between the system's NED position measurements and the swarm / global 0/0/0 origin
QVector3D nedAttGlobalOffset; ///< Offset between the system's NED position measurements and the swarm / global 0/0/0 origin
public:
/** @brief Set the current battery type */
......
src/ui/HUD.cc
View file @ b302c815
......@@ -279,7 +279,7 @@ void HUD::setActiveUAS(UASInterface* uas)
// Now connect the new UAS
// Setup communication
connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*, double, double, double, quint64)));
connect(this->uas, SIGNAL(attitudeChanged(UASInterface*,int,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,int,double, double, double, quint64)));
connect(uas, SIGNAL(attitudeChanged(UASInterface*,int,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,int,double, double, double, quint64)));
connect(uas, SIGNAL(batteryChanged(UASInterface*, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, int)));
connect(uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString)));
connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
......@@ -854,6 +854,8 @@ void HUD::paintHUD()
// PITCH
paintPitchLines(att.y(), &painter);
painter.translate(0, -(-att.y()/(float)M_PI)* -180.0f * refToScreenY(1.8f));
painter.rotate(-(att.x()/M_PI)* -180.0f);
}
painter.end();
......
src/ui/Linechart.ui
View file @ b302c815
......@@ -99,8 +99,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>232</width>
<height>516</height>
<width>219</width>
<height>475</height>
</rect>
</property>
</widget>
......@@ -109,7 +109,7 @@
</layout>
</widget>
</item>
<item row="0" column="4" rowspan="2">
<item row="0" column="4" rowspan="4">
<widget class="QGroupBox" name="diagramGroupBox">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
......@@ -134,21 +134,21 @@
</property>
</widget>
</item>
<item row="1" column="0">
<item row="3" column="0">
<widget class="QCheckBox" name="shortNameCheckBox">
<property name="text">
<string>Short Names</string>
</property>
</widget>
</item>
<item row="1" column="2" colspan="2">
<item row="3" column="2" colspan="2">
<widget class="QPushButton" name="recolorButton">
<property name="text">
<string>Recolor</string>
</property>
</widget>
</item>
<item row="1" column="1">
<item row="3" column="1">
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
......@@ -161,6 +161,15 @@
</property>
</spacer>
</item>
<item row="2" column="0" colspan="4">
<widget class="QComboBox" name="uasSelectionBox">
<item>
<property name="text">
<string>All MAVs</string>
</property>
</item>
</widget>
</item>
</layout>
</widget>
<resources/>
......
src/ui/linechart/LinechartWidget.cc
View file @ b302c815
......@@ -146,6 +146,7 @@ LinechartWidget::LinechartWidget(int systemid, QWidget *parent) : QWidget(parent
updateTimer->setInterval(updateInterval);
connect(updateTimer, SIGNAL(timeout()), this, SLOT(refresh()));
connect(ui.uasSelectionBox, SIGNAL(currentIndexChanged(int)), this, SLOT(selectActiveSystem(int)));
readSettings();
}
......@@ -159,6 +160,17 @@ LinechartWidget::~LinechartWidget()
listedCurves = NULL;
}
void LinechartWidget::selectActiveSystem(int mav)
{
// -1: Unitialized, 0: all
if (mav != selectedMAV && (selectedMAV != -1))
{
// Delete all curves
// FIXME
}
selectedMAV = mav;
}
void LinechartWidget::selectAllCurves(bool all)
{
QMap<QString, QLabel*>::iterator i;
......
src/ui/linechart/LinechartWidget.h
View file @ b302c815
......@@ -105,11 +105,8 @@ public slots:
/** @brief Stop automatic updates once hidden */
void hideEvent(QHideEvent* event);
void setActive(bool active);
void setActiveSystem(int systemid)
{
selectedMAV = systemid;
}
/** @brief Select one MAV for curve display */
void selectActiveSystem(int mav);
/** @brief Set the number of values to average over */
void setAverageWindow(int windowSize);
/** @brief Start logging to file */
......
src/ui/linechart/Linecharts.cc
View file @ b302c815
......@@ -63,36 +63,40 @@ void Linecharts::hideEvent(QHideEvent* event)
void Linecharts::selectSystem(int systemid)
{
QWidget* prevWidget = currentWidget();
if (prevWidget)
{
LinechartWidget* chart = dynamic_cast<LinechartWidget*>(prevWidget);
if (chart)
{
chart->setActive(false);
chart->setActiveSystem(systemid);
}
}
QWidget* widget = plots.value(systemid, NULL);
if (widget)
{
setCurrentWidget(widget);
LinechartWidget* chart = dynamic_cast<LinechartWidget*>(widget);
if (chart)
{
chart->setActive(true);
chart->setActiveSystem(systemid);
}
}
// QWidget* prevWidget = currentWidget();
// if (prevWidget)
// {
// LinechartWidget* chart = dynamic_cast<LinechartWidget*>(prevWidget);
// if (chart)
// {
// chart->setActive(false);
// chart->setActiveSystem(systemid);
// }
// }
// QWidget* widget = plots.value(systemid, NULL);
// if (widget)
// {
// setCurrentWidget(widget);
// LinechartWidget* chart = dynamic_cast<LinechartWidget*>(widget);
// if (chart)
// {
// chart->setActive(true);
// chart->setActiveSystem(systemid);
// }
// }
}
void Linecharts::addSystem(UASInterface* uas)
{
if (!plots.contains(uas->getUASID()))
// FIXME Add removeSystem() call
// Compatibility hack
int uasid = 0; /*uas->getUASID()*/
if (!plots.contains(uasid))
{
LinechartWidget* widget = new LinechartWidget(uas->getUASID(), this);
LinechartWidget* widget = new LinechartWidget(uasid, this);
addWidget(widget);
plots.insert(uas->getUASID(), widget);
plots.insert(uasid, widget);
// Connect valueChanged signals
connect(uas, SIGNAL(valueChanged(int,QString,QString,quint8,quint64)), widget, SLOT(appendData(int,QString,QString,quint8,quint64)));
......@@ -107,10 +111,10 @@ void Linecharts::addSystem(UASInterface* uas)
connect(widget, SIGNAL(logfileWritten(QString)), this, SIGNAL(logfileWritten(QString)));
// Set system active if this is the only system
if (active)
{
if (plots.size() == 1)
{
// if (active)
// {
// if (plots.size() == 1)
// {
// FIXME XXX HACK
// Connect generic sources
for (int i = 0; i < genericSources.count(); ++i)
......@@ -126,10 +130,11 @@ void Linecharts::addSystem(UASInterface* uas)
connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,double,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,double,quint64)));
}
// Select system
selectSystem(uas->getUASID());
widget->setActive(true);
//widget->selectActiveSystem(0);
}
}
}
// }
// }
}
void Linecharts::addSource(QObject* obj)
......@@ -149,4 +154,4 @@ void Linecharts::addSource(QObject* obj)
connect(obj, SIGNAL(valueChanged(int,QString,QString,qint64,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,qint64,quint64)));
connect(obj, SIGNAL(valueChanged(int,QString,QString,double,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,double,quint64)));
}
}
\ No newline at end of file
}
src/ui/map3D/Pixhawk3DWidget.cc
View file @ b302c815
......@@ -280,6 +280,14 @@ Pixhawk3DWidget::localPositionChanged(UASInterface* uas,
return;
}
// Add offset
UAS* mav = qobject_cast<UAS*>(uas);
float offX = mav->getNedPosGlobalOffset().x();
float offY = mav->getNedPosGlobalOffset().y();
float offZ = mav->getNedPosGlobalOffset().z();
float offYaw = mav->getNedAttGlobalOffset().z();
// update system position
m3DWidget->systemGroup(systemId)->position()->setPosition(osg::Vec3d(y, x, -z));
}
......
thirdParty/mavlink/include/common/common.h
View file @ b302c815
This diff is collapsed.
thirdParty/mavlink/include/common/testsuite.h
View file @ b302c815
......@@ -3123,6 +3123,61 @@ static void mavlink_test_command_ack(uint8_t system_id, uint8_t component_id, ma
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
static void mavlink_test_local_position_ned_system_global_offset(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_message_t msg;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
uint16_t i;
mavlink_local_position_ned_system_global_offset_t packet_in = {
963497464,
45.0,
73.0,
101.0,
129.0,
157.0,
185.0,
};
mavlink_local_position_ned_system_global_offset_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
packet1.time_boot_ms = packet_in.time_boot_ms;
packet1.x = packet_in.x;
packet1.y = packet_in.y;
packet1.z = packet_in.z;
packet1.roll = packet_in.roll;
packet1.pitch = packet_in.pitch;
packet1.yaw = packet_in.yaw;
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_local_position_ned_system_global_offset_encode(system_id, component_id, &msg, &packet1);
mavlink_msg_local_position_ned_system_global_offset_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_local_position_ned_system_global_offset_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw );
mavlink_msg_local_position_ned_system_global_offset_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_local_position_ned_system_global_offset_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw );
mavlink_msg_local_position_ned_system_global_offset_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_to_send_buffer(buffer, &msg);
for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
comm_send_ch(MAVLINK_COMM_0, buffer[i]);
}
mavlink_msg_local_position_ned_system_global_offset_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_local_position_ned_system_global_offset_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw );
mavlink_msg_local_position_ned_system_global_offset_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
static void mavlink_test_hil_state(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_message_t msg;
......@@ -3336,17 +3391,17 @@ static void mavlink_test_optical_flow(uint8_t system_id, uint8_t component_id, m
mavlink_optical_flow_t packet_in = {
93372036854775807ULL,
73.0,
17859,
17963,
53,
120,
101.0,
129.0,
65,
132,
};
mavlink_optical_flow_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
packet1.time_usec = packet_in.time_usec;
packet1.ground_distance = packet_in.ground_distance;
packet1.flow_x = packet_in.flow_x;
packet1.flow_y = packet_in.flow_y;
packet1.ground_distance = packet_in.ground_distance;
packet1.sensor_id = packet_in.sensor_id;
packet1.quality = packet_in.quality;
......@@ -3942,6 +3997,7 @@ static void mavlink_test_common(uint8_t system_id, uint8_t component_id, mavlink
mavlink_test_vfr_hud(system_id, component_id, last_msg);
mavlink_test_command_long(system_id, component_id, last_msg);
mavlink_test_command_ack(system_id, component_id, last_msg);
mavlink_test_local_position_ned_system_global_offset(system_id, component_id, last_msg);
mavlink_test_hil_state(system_id, component_id, last_msg);
mavlink_test_hil_controls(system_id, component_id, last_msg);
mavlink_test_hil_rc_inputs_raw(system_id, component_id, last_msg);
......
thirdParty/mavlink/include/common/version.h
View file @ b302c815
......@@ -5,8 +5,8 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Thu Feb 9 16:35:38 2012"
#define MAVLINK_BUILD_DATE "Sat Feb 25 13:54:47 2012"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101
#endif // MAVLINK_VERSION_H
thirdParty/mavlink/include/mavlink_types.h
View file @ b302c815
......@@ -15,21 +15,10 @@
#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length
#if (defined __GNUC__) || (defined _MSC_VER) || (defined __MINGW32__) || (defined __WATCOMC__) || (defined __CMB__) || (defined __BORLANDC__) || (defined __clang__)
#define MAVLINK_EXTENDED_MESSAGES_ENABLED
#endif
// EXTENDED message definition - the extended message set is compatible to standard MAVLink message passing
// but does NOT have to be supported by the platform. The extended message set will NOT consume
// any memory if the messages are not explicitely used
#ifdef MAVLINK_EXTENDED_MESSAGES_ENABLED
#define MAVLINK_MSG_ID_EXTENDED_MESSAGE 255
#define MAVLINK_EXTENDED_HEADER_LEN 14
#define MAVLINK_MAX_EXTENDED_PACKET_LEN 65507
#define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES)
#endif
typedef struct param_union {
union {
......@@ -62,13 +51,12 @@ typedef struct __mavlink_message {
uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
} mavlink_message_t;
#ifdef MAVLINK_EXTENDED_MESSAGES_ENABLED
typedef struct __mavlink_extended_message {
mavlink_message_t base_msg;
int32_t extended_payload_len; ///< Length of extended payload if any
uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN];
} mavlink_extended_message_t;
#endif
typedef enum {
......
thirdParty/mavlink/message_definitions/common.xml
View file @ b302c815
......@@ -1367,6 +1367,16 @@
<field type="uint16_t" name="command">Command ID, as defined by MAV_CMD enum.</field>
<field type="uint8_t" name="result">See MAV_RESULT enum</field>
</message>
<message id="80" name="LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET">
<description>The offset in X, Y, Z and yaw between the LOCAL_POSITION_NED messages of MAV X and the global coordinate frame in NED coordinates. Coordinate frame is right-handed, Z-axis down (aeronautical frame, NED / north-east-down convention)</description>
<field type="uint32_t" name="time_boot_ms">Timestamp (milliseconds since system boot)</field>
<field type="float" name="x">X Position</field>
<field type="float" name="y">Y Position</field>
<field type="float" name="z">Z Position</field>
<field type="float" name="roll">Roll</field>
<field type="float" name="pitch">Pitch</field>
<field type="float" name="yaw">Yaw</field>
</message>
<!--
<message id="80" name="MISSION_ITEM">
<description>One mission element following the MAV_CMD enumeration</description>
......
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