Commit 38d83048 authored by pixhawk's avatar pixhawk

Enabled waypoints in simulation, implemented factory class for UAS objects

parent fbeddd82
......@@ -114,7 +114,8 @@ SOURCES += src/uas/UAS.cc \
src/comm/SerialLink.cc \
$$TESTDIR/SlugsMavUnitTest.cc \
$$TESTDIR/testSuite.cc \
$$TESTDIR/UASUnitTest.cc
$$TESTDIR/UASUnitTest.cc \
src/uas/QGCMAVLinkUASFactory.cc
HEADERS += src/uas/UASInterface.h \
......@@ -136,7 +137,8 @@ HEADERS += src/uas/UASInterface.h \
src/comm/SerialLink.h \
$$TESTDIR//SlugsMavUnitTest.h \
$$TESTDIR/AutoTest.h \
$$TESTDIR/UASUnitTest.h
$$TESTDIR/UASUnitTest.h \
src/uas/QGCMAVLinkUASFactory.h
......
......@@ -262,7 +262,8 @@ HEADERS += src/MG.h \
src/ui/designer/QGCToolWidgetItem.h \
src/ui/QGCMAVLinkLogPlayer.h \
src/comm/MAVLinkSimulationWaypointPlanner.h \
src/comm/MAVLinkSimulationMAV.h
src/comm/MAVLinkSimulationMAV.h \
src/uas/QGCMAVLinkUASFactory.h
# Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler
macx|win32-msvc2008: {
......@@ -385,7 +386,8 @@ SOURCES += src/main.cc \
src/ui/designer/QGCToolWidgetItem.cc \
src/ui/QGCMAVLinkLogPlayer.cc \
src/comm/MAVLinkSimulationWaypointPlanner.cc \
src/comm/MAVLinkSimulationMAV.cc
src/comm/MAVLinkSimulationMAV.cc \
src/uas/QGCMAVLinkUASFactory.cc \
macx|win32-msvc2008: {
SOURCES += src/ui/map3D/QGCGoogleEarthView.cc
......
......@@ -153,7 +153,7 @@ void GAudioOutput::mute(bool mute)
{
this->muted = mute;
QSettings settings;
settings.setValue(QGC_GAUDIOOUTPUT_KEY+"muted", muted);
settings.setValue(QGC_GAUDIOOUTPUT_KEY+"muted", this->muted);
settings.sync();
}
......
......@@ -55,7 +55,12 @@ Waypoint::~Waypoint()
void Waypoint::save(QTextStream &saveStream)
{
saveStream << this->getId() << "\t" << this->getFrame() << "\t" << this->getAction() << "\t" << this->getOrbit() << "\t" << /*Orbit Direction*/ 0 << "\t" << this->getOrbit() << "\t" << this->getHoldTime() << "\t" << this->getCurrent() << "\t" << this->getX() << "\t" << this->getY() << "\t" << this->getZ() << "\t" << this->getYaw() << "\t" << this->getAutoContinue() << "\r\n";
QString position("%1\t%2\t%3\t%4");
position = position.arg(x, 0, 'g', 18);
position = position.arg(y, 0, 'g', 18);
position = position.arg(z, 0, 'g', 18);
position = position.arg(yaw, 0, 'g', 8);
saveStream << this->getId() << "\t" << this->getFrame() << "\t" << this->getAction() << "\t" << this->getOrbit() << "\t" << /*Orbit Direction*/ 0 << "\t" << this->getOrbit() << "\t" << this->getHoldTime() << "\t" << this->getCurrent() << "\t" << position << "\t" << this->getAutoContinue() << "\r\n";
}
bool Waypoint::load(QTextStream &loadStream)
......
......@@ -27,7 +27,8 @@
#include "configuration.h"
#include "LinkManager.h"
//#include "MainWindow.h"
#include <QGCMAVLink.h>
#include "QGCMAVLink.h"
#include "QGCMAVLinkUASFactory.h"
#include "QGC.h"
/**
......@@ -169,63 +170,8 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
continue;
}
switch (heartbeat.autopilot)
{
case MAV_AUTOPILOT_GENERIC:
uas = new UAS(this, message.sysid);
// Connect this robot to the UAS object
connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), uas, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
break;
case MAV_AUTOPILOT_PIXHAWK:
{
// Fixme differentiate between quadrotor and coaxial here
PxQuadMAV* mav = new PxQuadMAV(this, message.sysid);
// Connect this robot to the UAS object
// it is IMPORTANT here to use the right object type,
// else the slot of the parent object is called (and thus the special
// packets never reach their goal)
connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
uas = mav;
}
break;
case MAV_AUTOPILOT_SLUGS:
{
SlugsMAV* mav = new SlugsMAV(this, message.sysid);
// Connect this robot to the UAS object
// it is IMPORTANT here to use the right object type,
// else the slot of the parent object is called (and thus the special
// packets never reach their goal)
connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
uas = mav;
}
break;
case MAV_AUTOPILOT_ARDUPILOTMEGA:
{
ArduPilotMegaMAV* mav = new ArduPilotMegaMAV(this, message.sysid);
// Connect this robot to the UAS object
// it is IMPORTANT here to use the right object type,
// else the slot of the parent object is called (and thus the special
// packets never reach their goal)
connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
uas = mav;
}
break;
default:
uas = new UAS(this, message.sysid);
break;
}
// Set the autopilot type
uas->setAutopilotType((int)heartbeat.autopilot);
// Make UAS aware that this link can be used to communicate with the actual robot
uas->addLink(link);
// Now add UAS to "official" list, which makes the whole application aware of it
UASManager::instance()->addUAS(uas);
// Create a new UAS object
uas = QGCMAVLinkUASFactory::createUAS(this, link, message.sysid, &heartbeat);
}
// Only count message if UAS exists for this message
......
......@@ -153,14 +153,14 @@ void MAVLinkSimulationLink::sendMAVLinkMessage(const mavlink_message_t* msg)
unsigned int bufferlength = mavlink_msg_to_send_buffer(buf, msg);
// Pack to link buffer
readyBufferMutex.lock();
// readyBufferMutex.lock();
for (unsigned int i = 0; i < bufferlength; i++)
{
readyBuffer.enqueue(*(buf + i));
}
readyBufferMutex.unlock();
// readyBufferMutex.unlock();
//qDebug() << "SENT MAVLINK MESSAGE FROM SYSTEM" << msg->sysid << "COMP" << msg->compid;
qDebug() << "SENT MAVLINK MESSAGE FROM SYSTEM" << msg->sysid << "COMP" << msg->compid;
}
void MAVLinkSimulationLink::enqueue(uint8_t* stream, uint8_t* index, mavlink_message_t* msg)
......
#include <QDebug>
#include <cmath>
#include "MAVLinkSimulationMAV.h"
......@@ -6,28 +7,126 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy
QObject(parent),
link(parent),
planner(parent, systemid),
systemid(systemid)
systemid(systemid),
timer25Hz(0),
timer10Hz(0),
timer1Hz(0),
latitude(0.0),
longitude(0.0),
altitude(0.0),
x(0.0),
y(0.0),
z(0.0),
roll(0.0),
pitch(0.0),
yaw(0.0),
globalNavigation(true),
firstWP(false),
previousSPX(0.0),
previousSPY(0.0),
previousSPZ(0.0),
previousSPYaw(0.0),
nextSPX(0.0),
nextSPY(0.0),
nextSPZ(0.0),
nextSPYaw(0.0)
{
// Please note: The waypoint planner is running
connect(&mainloopTimer, SIGNAL(timeout()), this, SLOT(mainloop()));
mainloopTimer.start(1000);
connect(&planner, SIGNAL(messageSent(mavlink_message_t)), this, SLOT(handleMessage(mavlink_message_t)));
connect(link, SIGNAL(messageReceived(mavlink_message_t)), this, SLOT(handleMessage(mavlink_message_t)));
mainloopTimer.start(20);
mainloop();
}
void MAVLinkSimulationMAV::mainloop()
{
mavlink_message_t msg;
mavlink_msg_heartbeat_pack(systemid, MAV_COMP_ID_IMU, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_PIXHAWK);
link->sendMAVLinkMessage(&msg);
// Calculate new simulator values
// double maxSpeed = 0.0001; // rad/s in earth coordinate frame
// double xNew = // (nextSPX - previousSPX)
if (!firstWP)
{
x += (nextSPX - previousSPX) * 0.01;
y += (nextSPY - previousSPY) * 0.01;
z += (nextSPZ - previousSPZ) * 0.1;
}
else
{
x = nextSPX;
y = nextSPY;
z = nextSPZ;
firstWP = false;
}
// 1 Hz execution
if (timer1Hz <= 0)
{
mavlink_message_t msg;
mavlink_msg_heartbeat_pack(systemid, MAV_COMP_ID_IMU, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_PIXHAWK);
link->sendMAVLinkMessage(&msg);
timer1Hz = 50;
}
// 10 Hz execution
if (timer10Hz <= 0)
{
mavlink_message_t msg;
mavlink_global_position_int_t pos;
pos.alt = z*1000.0;
pos.lat = y*1E7;
pos.lon = x*1E7;
pos.vx = 0;
pos.vy = 0;
pos.vz = 0;
mavlink_msg_global_position_int_encode(systemid, MAV_COMP_ID_IMU, &msg, &pos);
link->sendMAVLinkMessage(&msg);
timer10Hz = 5;
}
// 25 Hz execution
if (timer25Hz <= 0)
{
timer25Hz = 2;
}
timer1Hz--;
timer10Hz--;
timer25Hz--;
}
void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)
{
qDebug() << "MAV:" << systemid << "RECEIVED MESSAGE FROM" << msg.sysid << "COMP" << msg.compid;
//qDebug() << "MAV:" << systemid << "RECEIVED MESSAGE FROM" << msg.sysid << "COMP" << msg.compid;
switch(msg.msgid)
{
case MAVLINK_MSG_ID_ATTITUDE:
break;
case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET:
{
mavlink_local_position_setpoint_set_t sp;
mavlink_msg_local_position_setpoint_set_decode(&msg, &sp);
if (sp.target_system == this->systemid)
{
previousSPX = nextSPX;
previousSPY = nextSPY;
previousSPZ = nextSPZ;
nextSPX = sp.x;
nextSPY = sp.y;
nextSPZ = sp.z;
// Rotary wing
//nextSPYaw = sp.yaw;
// Airplane
yaw = atan2(previousSPX-nextSPX, previousSPY-nextSPY);
if (!firstWP) firstWP = true;
}
//qDebug() << "UPDATED SP:" << "X" << nextSPX << "Y" << nextSPY << "Z" << nextSPZ;
}
break;
}
}
......@@ -24,7 +24,31 @@ protected:
MAVLinkSimulationWaypointPlanner planner;
int systemid;
QTimer mainloopTimer;
int timer25Hz;
int timer10Hz;
int timer1Hz;
double latitude;
double longitude;
double altitude;
double x;
double y;
double z;
double roll;
double pitch;
double yaw;
bool globalNavigation;
bool firstWP;
double previousSPX;
double previousSPY;
double previousSPZ;
double previousSPYaw;
double nextSPX;
double nextSPY;
double nextSPZ;
double nextSPYaw;
};
......
......@@ -24,6 +24,7 @@ public:
explicit MAVLinkSimulationWaypointPlanner(MAVLinkSimulationLink *parent, int systemid);
signals:
void messageSent(const mavlink_message_t& msg);
public slots:
void handleMessage(const mavlink_message_t& msg);
......
#include "QGCMAVLinkUASFactory.h"
#include "UASManager.h"
QGCMAVLinkUASFactory::QGCMAVLinkUASFactory(QObject *parent) :
QObject(parent)
{
}
UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInterface* link, int sysid, mavlink_heartbeat_t* heartbeat, QObject* parent)
{
QPointer<QObject> p;
if (parent != NULL)
{
p = parent;
}
else
{
p = mavlink;
}
UASInterface* uas;
switch (heartbeat->autopilot)
{
case MAV_AUTOPILOT_GENERIC:
{
UAS* mav = new UAS(mavlink, sysid);
// Set the system type
mav->setSystemType((int)heartbeat->type);
// Connect this robot to the UAS object
connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
uas = mav;
}
break;
case MAV_AUTOPILOT_PIXHAWK:
{
PxQuadMAV* mav = new PxQuadMAV(mavlink, sysid);
// Set the system type
mav->setSystemType((int)heartbeat->type);
// Connect this robot to the UAS object
// it is IMPORTANT here to use the right object type,
// else the slot of the parent object is called (and thus the special
// packets never reach their goal)
connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
uas = mav;
}
break;
case MAV_AUTOPILOT_SLUGS:
{
SlugsMAV* mav = new SlugsMAV(mavlink, sysid);
// Set the system type
mav->setSystemType((int)heartbeat->type);
// Connect this robot to the UAS object
// it is IMPORTANT here to use the right object type,
// else the slot of the parent object is called (and thus the special
// packets never reach their goal)
connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
uas = mav;
}
break;
case MAV_AUTOPILOT_ARDUPILOTMEGA:
{
ArduPilotMegaMAV* mav = new ArduPilotMegaMAV(mavlink, sysid);
// Set the system type
mav->setSystemType((int)heartbeat->type);
// Connect this robot to the UAS object
// it is IMPORTANT here to use the right object type,
// else the slot of the parent object is called (and thus the special
// packets never reach their goal)
connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
uas = mav;
}
break;
default:
{
UAS* mav = new UAS(mavlink, sysid);
mav->setSystemType((int)heartbeat->type);
// Connect this robot to the UAS object
// it is IMPORTANT here to use the right object type,
// else the slot of the parent object is called (and thus the special
// packets never reach their goal)
connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
uas = mav;
}
break;
}
// Set the autopilot type
uas->setAutopilotType((int)heartbeat->autopilot);
// Make UAS aware that this link can be used to communicate with the actual robot
uas->addLink(link);
// Now add UAS to "official" list, which makes the whole application aware of it
UASManager::instance()->addUAS(uas);
return uas;
}
#ifndef QGCMAVLINKUASFACTORY_H
#define QGCMAVLINKUASFACTORY_H
#include <QObject>
#include "QGCMAVLink.h"
#include "MAVLinkProtocol.h"
#include "UASInterface.h"
#include "LinkInterface.h"
// INCLUDE ALL MAV/UAS CLASSES USING MAVLINK
#include "UAS.h"
#include "SlugsMAV.h"
#include "PxQuadMAV.h"
#include "ArduPilotMegaMAV.h"
class QGCMAVLinkUASFactory : public QObject
{
Q_OBJECT
public:
explicit QGCMAVLinkUASFactory(QObject *parent = 0);
/** @brief Create a new UAS object using MAVLink as protocol */
static UASInterface* createUAS(MAVLinkProtocol* mavlink, LinkInterface* link, int sysid, mavlink_heartbeat_t* heartbeat, QObject* parent=NULL);
signals:
public slots:
};
#endif // QGCMAVLINKUASFACTORY_H
......@@ -149,6 +149,18 @@ void UAS::setSelected()
UASManager::instance()->setActiveUAS(this);
}
void UAS::receiveMessageNamedValue(const mavlink_message_t& message)
{
if (message.msgid == MAVLINK_MSG_ID_NAMED_VALUE_FLOAT)
{
}
else if (message.msgid == MAVLINK_MSG_ID_NAMED_VALUE_INT)
{
}
}
void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
if (!link) return;
......
......@@ -172,10 +172,12 @@ public:
UASWaypointManager &getWaypointManager(void) { return waypointManager; }
int getSystemType();
int getAutopilotType() {return autopilot;}
void setAutopilotType(int apType) { autopilot = apType;}
public slots:
/** @brief Set the autopilot type */
void setAutopilotType(int apType) { autopilot = apType; }
/** @brief Set the type of airframe */
void setSystemType(int systemType) { type = systemType; }
/** @brief Set a new name **/
void setUASName(const QString& name);
/** @brief Executes an action **/
......@@ -307,6 +309,10 @@ signals:
void writeSettings();
/** @brief Read settings from disk */
void readSettings();
// MESSAGE RECEPTION
/** @brief Receive a named value message */
void receiveMessageNamedValue(const mavlink_message_t& message);
};
......
......@@ -123,6 +123,10 @@ void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, qui
qDebug() << "No waypoints on UAS " << systemId;
}
}
else
{
qDebug("Rejecting message, check mismatch: current_state: %d == %d, system id %d == %d, comp id %d == %d", current_state, WP_GETLIST, current_partner_systemid, systemId, current_partner_compid, compId);
}
}
void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_waypoint_t *wp)
......@@ -165,9 +169,13 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
}
else
{
//MainWindow::instance()->showStatusMessage(tr("Waypoint ID mismatch, rejecting waypoint"));
emit updateStatusString(tr("Waypoint ID mismatch, rejecting waypoint"));
}
}
else
{
qDebug("Rejecting message, check mismatch: current_state: %d == %d, system id %d == %d, comp id %d == %d", current_state, WP_GETLIST, current_partner_systemid, systemId, current_partner_compid, compId);
}
}
void UASWaypointManager::handleWaypointAck(quint8 systemId, quint8 compId, mavlink_waypoint_ack_t *wpa)
......@@ -210,6 +218,10 @@ void UASWaypointManager::handleWaypointRequest(quint8 systemId, quint8 compId, m
//TODO: Error message or something
}
}
else
{
qDebug("Rejecting message, check mismatch: current_state: %d == %d, system id %d == %d, comp id %d == %d", current_state, WP_GETLIST, current_partner_systemid, systemId, current_partner_compid, compId);
}
}
void UASWaypointManager::handleWaypointReached(quint8 systemId, quint8 compId, mavlink_waypoint_reached_t *wpr)
......
......@@ -1155,6 +1155,10 @@ void MainWindow::connectCommonActions()
// Custom widget actions
connect(ui.actionNewCustomWidget, SIGNAL(triggered()), this, SLOT(createCustomWidget()));
// Audio output
ui.actionMuteAudioOutput->setChecked(GAudioOutput::instance()->isMuted());
connect(ui.actionMuteAudioOutput, SIGNAL(triggered(bool)), GAudioOutput::instance(), SLOT(mute(bool)));
}
void MainWindow::connectPxActions()
......@@ -1388,14 +1392,14 @@ void MainWindow::UASCreated(UASInterface* uas)
}
break;
loadOperatorView();
loadOperatorView();
}
// Change the view only if this is the first UAS
// If this is the first connected UAS, it is both created as well as
// the currently active UAS
if (UASManager::instance()->getActiveUAS() == uas)
if (UASManager::instance()->getUASList().size() == 1)
{
//qDebug() << "UPDATING THE VIEW SINCE THIS IS THE FIRST CONNECTED SYSTEM";
......@@ -1410,8 +1414,7 @@ void MainWindow::UASCreated(UASInterface* uas)
if (settings.contains(getWindowStateKey()))
{
restoreState(settings.value(getWindowStateKey()).toByteArray(), QGC::applicationVersion());
}
}
}
else
{
......
......@@ -252,7 +252,8 @@ uint16_t SlugsHilSim::getUint16FromDatagram (const QByteArray* datagram, unsigne
}
void SlugsHilSim::linkSelected(int cbIndex){
void SlugsHilSim::linkSelected(int cbIndex)
{
#ifdef MAVLINK_ENABLED_SLUGS
// HIL code to go here...
//hilLink = linksAvailable
......@@ -260,6 +261,8 @@ void SlugsHilSim::linkSelected(int cbIndex){
hilLink =linksAvailable.value(cbIndex);
#else
Q_UNUSED(cbIndex)
#endif
}
......
......@@ -137,6 +137,8 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
{
m_ui->positionLabel->hide();
}
setSystemType(uas, uas->getSystemType());
}
UASView::~UASView()
......
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