Skip to content

Q
qgroundcontrol
Commit 38d83048 authored by pixhawk's avatar pixhawk
Browse files
Options

Enabled waypoints in simulation, implemented factory class for UAS objects

parent fbeddd82
Hide whitespace changes
Inline Side-by-side
Showing with 402 additions and 143 deletions
qgcunittest.pro
View file @ 38d83048
...@@ -114,7 +114,8 @@ SOURCES += src/uas/UAS.cc \ ...@@ -114,7 +114,8 @@ SOURCES += src/uas/UAS.cc \
src/comm/SerialLink.cc \ src/comm/SerialLink.cc \
$$TESTDIR/SlugsMavUnitTest.cc \ $$TESTDIR/SlugsMavUnitTest.cc \
$$TESTDIR/testSuite.cc \ $$TESTDIR/testSuite.cc \
$$TESTDIR/UASUnitTest.cc $$TESTDIR/UASUnitTest.cc \
src/uas/QGCMAVLinkUASFactory.cc
HEADERS += src/uas/UASInterface.h \ HEADERS += src/uas/UASInterface.h \
...@@ -136,7 +137,8 @@ HEADERS += src/uas/UASInterface.h \ ...@@ -136,7 +137,8 @@ HEADERS += src/uas/UASInterface.h \
src/comm/SerialLink.h \ src/comm/SerialLink.h \
$$TESTDIR//SlugsMavUnitTest.h \ $$TESTDIR//SlugsMavUnitTest.h \
$$TESTDIR/AutoTest.h \ $$TESTDIR/AutoTest.h \
$$TESTDIR/UASUnitTest.h $$TESTDIR/UASUnitTest.h \
src/uas/QGCMAVLinkUASFactory.h
......
qgroundcontrol.pro
View file @ 38d83048
...@@ -262,7 +262,8 @@ HEADERS += src/MG.h \ ...@@ -262,7 +262,8 @@ HEADERS += src/MG.h \
src/ui/designer/QGCToolWidgetItem.h \ src/ui/designer/QGCToolWidgetItem.h \
src/ui/QGCMAVLinkLogPlayer.h \ src/ui/QGCMAVLinkLogPlayer.h \
src/comm/MAVLinkSimulationWaypointPlanner.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 # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler
macx|win32-msvc2008: { macx|win32-msvc2008: {
...@@ -385,7 +386,8 @@ SOURCES += src/main.cc \ ...@@ -385,7 +386,8 @@ SOURCES += src/main.cc \
src/ui/designer/QGCToolWidgetItem.cc \ src/ui/designer/QGCToolWidgetItem.cc \
src/ui/QGCMAVLinkLogPlayer.cc \ src/ui/QGCMAVLinkLogPlayer.cc \
src/comm/MAVLinkSimulationWaypointPlanner.cc \ src/comm/MAVLinkSimulationWaypointPlanner.cc \
src/comm/MAVLinkSimulationMAV.cc src/comm/MAVLinkSimulationMAV.cc \
src/uas/QGCMAVLinkUASFactory.cc \
macx|win32-msvc2008: { macx|win32-msvc2008: {
SOURCES += src/ui/map3D/QGCGoogleEarthView.cc SOURCES += src/ui/map3D/QGCGoogleEarthView.cc
......
src/GAudioOutput.cc
View file @ 38d83048
...@@ -153,7 +153,7 @@ void GAudioOutput::mute(bool mute) ...@@ -153,7 +153,7 @@ void GAudioOutput::mute(bool mute)
{ {
this->muted = mute; this->muted = mute;
QSettings settings; QSettings settings;
settings.setValue(QGC_GAUDIOOUTPUT_KEY+"muted", muted); settings.setValue(QGC_GAUDIOOUTPUT_KEY+"muted", this->muted);
settings.sync(); settings.sync();
} }
......
src/Waypoint.cc
View file @ 38d83048
...@@ -55,7 +55,12 @@ Waypoint::~Waypoint() ...@@ -55,7 +55,12 @@ Waypoint::~Waypoint()
void Waypoint::save(QTextStream &saveStream) 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) bool Waypoint::load(QTextStream &loadStream)
......
src/comm/MAVLinkProtocol.cc
View file @ 38d83048
...@@ -27,7 +27,8 @@ ...@@ -27,7 +27,8 @@
#include "configuration.h" #include "configuration.h"
#include "LinkManager.h" #include "LinkManager.h"
//#include "MainWindow.h" //#include "MainWindow.h"
#include <QGCMAVLink.h> #include "QGCMAVLink.h"
#include "QGCMAVLinkUASFactory.h"
#include "QGC.h" #include "QGC.h"
/** /**
...@@ -169,63 +170,8 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) ...@@ -169,63 +170,8 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
continue; continue;
} }
switch (heartbeat.autopilot) // Create a new UAS object
{ uas = QGCMAVLinkUASFactory::createUAS(this, link, message.sysid, &heartbeat);
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);
} }
// Only count message if UAS exists for this message // Only count message if UAS exists for this message
......
src/comm/MAVLinkSimulationLink.cc
View file @ 38d83048
...@@ -153,14 +153,14 @@ void MAVLinkSimulationLink::sendMAVLinkMessage(const mavlink_message_t* msg) ...@@ -153,14 +153,14 @@ void MAVLinkSimulationLink::sendMAVLinkMessage(const mavlink_message_t* msg)
unsigned int bufferlength = mavlink_msg_to_send_buffer(buf, msg); unsigned int bufferlength = mavlink_msg_to_send_buffer(buf, msg);
// Pack to link buffer // Pack to link buffer
readyBufferMutex.lock(); // readyBufferMutex.lock();
for (unsigned int i = 0; i < bufferlength; i++) for (unsigned int i = 0; i < bufferlength; i++)
{ {
readyBuffer.enqueue(*(buf + 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) void MAVLinkSimulationLink::enqueue(uint8_t* stream, uint8_t* index, mavlink_message_t* msg)
......
src/comm/MAVLinkSimulationMAV.cc
View file @ 38d83048
#include <QDebug> #include <QDebug>
#include <cmath>
#include "MAVLinkSimulationMAV.h" #include "MAVLinkSimulationMAV.h"
...@@ -6,28 +7,126 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy ...@@ -6,28 +7,126 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy
QObject(parent), QObject(parent),
link(parent), link(parent),
planner(parent, systemid), 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())); 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))); connect(link, SIGNAL(messageReceived(mavlink_message_t)), this, SLOT(handleMessage(mavlink_message_t)));
mainloopTimer.start(20);
mainloop(); mainloop();
} }
void MAVLinkSimulationMAV::mainloop() void MAVLinkSimulationMAV::mainloop()
{ {
mavlink_message_t msg; // Calculate new simulator values
mavlink_msg_heartbeat_pack(systemid, MAV_COMP_ID_IMU, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_PIXHAWK); // double maxSpeed = 0.0001; // rad/s in earth coordinate frame
link->sendMAVLinkMessage(&msg);
// 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) 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) switch(msg.msgid)
{ {
case MAVLINK_MSG_ID_ATTITUDE: case MAVLINK_MSG_ID_ATTITUDE:
break; 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;
} }
} }
src/comm/MAVLinkSimulationMAV.h
View file @ 38d83048
...@@ -24,7 +24,31 @@ protected: ...@@ -24,7 +24,31 @@ protected:
MAVLinkSimulationWaypointPlanner planner; MAVLinkSimulationWaypointPlanner planner;
int systemid; int systemid;
QTimer mainloopTimer; 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;
}; };
......
src/comm/MAVLinkSimulationWaypointPlanner.cc
View file @ 38d83048
...@@ -34,6 +34,7 @@ This file is part of the PIXHAWK project ...@@ -34,6 +34,7 @@ This file is part of the PIXHAWK project
#include "MAVLinkSimulationWaypointPlanner.h" #include "MAVLinkSimulationWaypointPlanner.h"
#include "QGC.h" #include "QGC.h"
#include <QDebug>
#ifndef M_PI #ifndef M_PI
#define M_PI 3.14159265358979323846 #define M_PI 3.14159265358979323846
...@@ -225,7 +226,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint_ack(uint8_t target_systemid ...@@ -225,7 +226,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint_ack(uint8_t target_systemid
if (verbose) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); if (verbose) qDebug("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system);
} }
/* /*
...@@ -253,7 +254,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint_current(uint16_t seq) ...@@ -253,7 +254,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint_current(uint16_t seq)
if (verbose) printf("Broadcasted new current waypoint %u\n", wpc.seq); if (verbose) qDebug("Broadcasted new current waypoint %u\n", wpc.seq);
} }
} }
...@@ -291,10 +292,20 @@ void MAVLinkSimulationWaypointPlanner::send_setpoint(uint16_t seq) ...@@ -291,10 +292,20 @@ void MAVLinkSimulationWaypointPlanner::send_setpoint(uint16_t seq)
} }
else else
{ {
if (verbose) printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq); //if (verbose) qDebug("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq);
PControlSetPoint.target_system = systemid;
PControlSetPoint.target_component = MAV_COMP_ID_IMU;
PControlSetPoint.x = cur->x;
PControlSetPoint.y = cur->y;
PControlSetPoint.z = cur->z;
PControlSetPoint.yaw = cur->yaw;
mavlink_msg_local_position_setpoint_set_encode(systemid, compid, &msg, &PControlSetPoint);
link->sendMAVLinkMessage(&msg);
emit messageSent(msg);
} }
uint64_t now = QGC::groundTimeUsecs(); uint64_t now = QGC::groundTimeUsecs()/1000;
timestamp_last_send_setpoint = now; timestamp_last_send_setpoint = now;
} }
} }
...@@ -311,7 +322,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint_count(uint8_t target_system ...@@ -311,7 +322,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint_count(uint8_t target_system
mavlink_msg_waypoint_count_encode(systemid, compid, &msg, &wpc); mavlink_msg_waypoint_count_encode(systemid, compid, &msg, &wpc);
link->sendMAVLinkMessage(&msg); link->sendMAVLinkMessage(&msg);
if (verbose) printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); if (verbose) qDebug("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system);
} }
...@@ -325,17 +336,17 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint(uint8_t target_systemid, ui ...@@ -325,17 +336,17 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint(uint8_t target_systemid, ui
wp->target_system = target_systemid; wp->target_system = target_systemid;
wp->target_component = target_compid; wp->target_component = target_compid;
if (verbose) printf("Sent waypoint %u (%u / %u / %u / %u / %u / %f / %u / %f / %f / %u / %f / %f / %f / %f / %u)\n", wp->seq, wp->target_system, wp->target_component, wp->seq, wp->frame, wp->action, wp->orbit, wp->orbit_direction, wp->param1, wp->param2, wp->current, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue); if (verbose) qDebug("Sent waypoint %u (%u / %u / %u / %u / %u / %f / %u / %f / %f / %u / %f / %f / %f / %f / %u)\n", wp->seq, wp->target_system, wp->target_component, wp->seq, wp->frame, wp->action, wp->orbit, wp->orbit_direction, wp->param1, wp->param2, wp->current, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue);
mavlink_msg_waypoint_encode(systemid, compid, &msg, wp); mavlink_msg_waypoint_encode(systemid, compid, &msg, wp);
link->sendMAVLinkMessage(&msg); link->sendMAVLinkMessage(&msg);
if (verbose) printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); if (verbose) qDebug("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system);
} }
else else
{ {
if (verbose) printf("ERROR: index out of bounds\n"); if (verbose) qDebug("ERROR: index out of bounds\n");
} }
} }
...@@ -348,7 +359,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint_request(uint8_t target_syst ...@@ -348,7 +359,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint_request(uint8_t target_syst
wpr.seq = seq; wpr.seq = seq;
mavlink_msg_waypoint_request_encode(systemid, compid, &msg, &wpr); mavlink_msg_waypoint_request_encode(systemid, compid, &msg, &wpr);
link->sendMAVLinkMessage(&msg); link->sendMAVLinkMessage(&msg);
if (verbose) printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); if (verbose) qDebug("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system);
} }
...@@ -370,7 +381,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint_reached(uint16_t seq) ...@@ -370,7 +381,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint_reached(uint16_t seq)
mavlink_msg_waypoint_reached_encode(systemid, compid, &msg, &wp_reached); mavlink_msg_waypoint_reached_encode(systemid, compid, &msg, &wp_reached);
link->sendMAVLinkMessage(&msg); link->sendMAVLinkMessage(&msg);
if (verbose) printf("Sent waypoint %u reached message\n", wp_reached.seq); if (verbose) qDebug("Sent waypoint %u reached message\n", wp_reached.seq);
} }
...@@ -438,12 +449,12 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* ...@@ -438,12 +449,12 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
//check for timed-out operations //check for timed-out operations
printf("MAV: %d WAYPOINTPLANNER GOT MESSAGE\n", systemid); qDebug() << "MAV: %d WAYPOINTPLANNER GOT MESSAGE" << systemid;
uint64_t now = QGC::groundTimeUsecs(); uint64_t now = QGC::groundTimeUsecs()/1000;
if (now-protocol_timestamp_lastaction > protocol_timeout && current_state != PX_WPP_IDLE) if (now-protocol_timestamp_lastaction > protocol_timeout && current_state != PX_WPP_IDLE)
{ {
if (verbose) printf("Last operation (state=%u) timed out, changing state to PX_WPP_IDLE\n", current_state); if (verbose) qDebug() << "Last operation (state=%u) timed out, changing state to PX_WPP_IDLE" << current_state;
current_state = PX_WPP_IDLE; current_state = PX_WPP_IDLE;
protocol_current_count = 0; protocol_current_count = 0;
protocol_current_partner_systemid = 0; protocol_current_partner_systemid = 0;
...@@ -506,7 +517,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* ...@@ -506,7 +517,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
{ {
mavlink_local_position_t pos; mavlink_local_position_t pos;
mavlink_msg_local_position_decode(msg, &pos); mavlink_msg_local_position_decode(msg, &pos);
if (debug) printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); qDebug() << "Received new position: x:" << pos.x << "| y:" << pos.y << "| z:" << pos.z;
posReached = false; posReached = false;
...@@ -538,7 +549,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* ...@@ -538,7 +549,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
mavlink_msg_action_decode(msg, &action); mavlink_msg_action_decode(msg, &action);
if(action.target == systemid) if(action.target == systemid)
{ {
if (verbose) printf("Waypoint: received message with action %d\n", action.action); if (verbose) qDebug("Waypoint: received message with action %d\n", action.action);
switch (action.action) switch (action.action)
{ {
// case MAV_ACTION_LAUNCH: // case MAV_ACTION_LAUNCH:
...@@ -585,7 +596,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* ...@@ -585,7 +596,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
{ {
if (protocol_current_wp_id == waypoints->size()-1) if (protocol_current_wp_id == waypoints->size()-1)
{ {
if (verbose) printf("Received Ack after having sent last waypoint, going to state PX_WPP_IDLE\n"); if (verbose) qDebug("Received Ack after having sent last waypoint, going to state PX_WPP_IDLE\n");
current_state = PX_WPP_IDLE; current_state = PX_WPP_IDLE;
protocol_current_wp_id = 0; protocol_current_wp_id = 0;
} }
...@@ -607,7 +618,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* ...@@ -607,7 +618,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
{ {
if (wpc.seq < waypoints->size()) if (wpc.seq < waypoints->size())
{ {
if (verbose) printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n"); if (verbose) qDebug("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n");
current_active_wp_id = wpc.seq; current_active_wp_id = wpc.seq;
uint32_t i; uint32_t i;
for(i = 0; i < waypoints->size(); i++) for(i = 0; i < waypoints->size(); i++)
...@@ -621,7 +632,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* ...@@ -621,7 +632,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
waypoints->at(i)->current = false; waypoints->at(i)->current = false;
} }
} }
if (verbose) printf("New current waypoint %u\n", current_active_wp_id); if (verbose) qDebug("New current waypoint %u\n", current_active_wp_id);
yawReached = false; yawReached = false;
posReached = false; posReached = false;
send_waypoint_current(current_active_wp_id); send_waypoint_current(current_active_wp_id);
...@@ -630,7 +641,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* ...@@ -630,7 +641,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
} }
else else
{ {
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n"); if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n");
} }
} }
} }
...@@ -649,8 +660,8 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* ...@@ -649,8 +660,8 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
{ {
if (waypoints->size() > 0) if (waypoints->size() > 0)
{ {
if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to PX_WPP_SENDLIST\n", msg->sysid); if (verbose && current_state == PX_WPP_IDLE) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to PX_WPP_SENDLIST\n", msg->sysid);
if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state PX_WPP_SENDLIST\n", msg->sysid); if (verbose && current_state == PX_WPP_SENDLIST) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state PX_WPP_SENDLIST\n", msg->sysid);
current_state = PX_WPP_SENDLIST; current_state = PX_WPP_SENDLIST;
protocol_current_wp_id = 0; protocol_current_wp_id = 0;
protocol_current_partner_systemid = msg->sysid; protocol_current_partner_systemid = msg->sysid;
...@@ -658,19 +669,19 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* ...@@ -658,19 +669,19 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
} }
else else
{ {
if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); if (verbose) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid);
} }
protocol_current_count = waypoints->size(); protocol_current_count = waypoints->size();
send_waypoint_count(msg->sysid,msg->compid, protocol_current_count); send_waypoint_count(msg->sysid,msg->compid, protocol_current_count);
} }
else else
{ {
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", current_state); if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", current_state);
} }
} }
else else
{ {
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because not my systemid or compid.\n"); if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because not my systemid or compid.\n");
} }
break; break;
} }
...@@ -686,9 +697,9 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* ...@@ -686,9 +697,9 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
//ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
if ((current_state == PX_WPP_SENDLIST && wpr.seq == 0) || (current_state == PX_WPP_SENDLIST_SENDWPS && (wpr.seq == protocol_current_wp_id || wpr.seq == protocol_current_wp_id + 1) && wpr.seq < waypoints->size())) if ((current_state == PX_WPP_SENDLIST && wpr.seq == 0) || (current_state == PX_WPP_SENDLIST_SENDWPS && (wpr.seq == protocol_current_wp_id || wpr.seq == protocol_current_wp_id + 1) && wpr.seq < waypoints->size()))
{ {
if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); if (verbose && current_state == PX_WPP_SENDLIST) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id + 1) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id + 1) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
current_state = PX_WPP_SENDLIST_SENDWPS; current_state = PX_WPP_SENDLIST_SENDWPS;
protocol_current_wp_id = wpr.seq; protocol_current_wp_id = wpr.seq;
...@@ -698,17 +709,17 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* ...@@ -698,17 +709,17 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
{ {
if (verbose) if (verbose)
{ {
if (!(current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", current_state); break; } if (!(current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)) { qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", current_state); break; }
else if (current_state == PX_WPP_SENDLIST) else if (current_state == PX_WPP_SENDLIST)
{ {
if (wpr.seq != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); if (wpr.seq != 0) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq);
} }
else if (current_state == PX_WPP_SENDLIST_SENDWPS) else if (current_state == PX_WPP_SENDLIST_SENDWPS)
{ {
if (wpr.seq != protocol_current_wp_id && wpr.seq != protocol_current_wp_id + 1) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, protocol_current_wp_id, protocol_current_wp_id+1); if (wpr.seq != protocol_current_wp_id && wpr.seq != protocol_current_wp_id + 1) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, protocol_current_wp_id, protocol_current_wp_id+1);
else if (wpr.seq >= waypoints->size()) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); else if (wpr.seq >= waypoints->size()) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq);
} }
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n"); else qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n");
} }
} }
} }
...@@ -717,7 +728,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* ...@@ -717,7 +728,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
//we we're target but already communicating with someone else //we we're target but already communicating with someone else
if((wpr.target_system == systemid && wpr.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid)) if((wpr.target_system == systemid && wpr.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid))
{ {
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, protocol_current_partner_systemid); if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, protocol_current_partner_systemid);
} }
} }
break; break;
...@@ -735,8 +746,8 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* ...@@ -735,8 +746,8 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
{ {
if (wpc.count > 0) if (wpc.count > 0)
{ {
if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to PX_WPP_GETLIST\n", wpc.count, msg->sysid); if (verbose && current_state == PX_WPP_IDLE) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to PX_WPP_GETLIST\n", wpc.count, msg->sysid);
if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid); if (verbose && current_state == PX_WPP_GETLIST) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid);
current_state = PX_WPP_GETLIST; current_state = PX_WPP_GETLIST;
protocol_current_wp_id = 0; protocol_current_wp_id = 0;
...@@ -744,7 +755,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* ...@@ -744,7 +755,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
protocol_current_partner_compid = msg->compid; protocol_current_partner_compid = msg->compid;
protocol_current_count = wpc.count; protocol_current_count = wpc.count;
printf("clearing receive buffer and readying for receiving waypoints\n"); qDebug("clearing receive buffer and readying for receiving waypoints\n");
while(waypoints_receive_buffer->size() > 0) while(waypoints_receive_buffer->size() > 0)
{ {
delete waypoints_receive_buffer->back(); delete waypoints_receive_buffer->back();
...@@ -755,14 +766,14 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* ...@@ -755,14 +766,14 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
} }
else else
{ {
if (verbose) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count); if (verbose) qDebug("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count);
} }
} }
else else
{ {
if (verbose && !(current_state == PX_WPP_IDLE || current_state == PX_WPP_GETLIST)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", current_state); if (verbose && !(current_state == PX_WPP_IDLE || current_state == PX_WPP_GETLIST)) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", current_state);
else if (verbose && current_state == PX_WPP_GETLIST && protocol_current_wp_id != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", protocol_current_wp_id); else if (verbose && current_state == PX_WPP_GETLIST && protocol_current_wp_id != 0) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", protocol_current_wp_id);
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n"); else qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n");
} }
} }
break; break;
...@@ -780,9 +791,9 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* ...@@ -780,9 +791,9 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
//ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids
if ((current_state == PX_WPP_GETLIST && wp.seq == 0) || (current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id && wp.seq < protocol_current_count)) if ((current_state == PX_WPP_GETLIST && wp.seq == 0) || (current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id && wp.seq < protocol_current_count))
{ {
if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to PX_WPP_GETLIST_GETWPS\n", wp.seq, msg->sysid); if (verbose && current_state == PX_WPP_GETLIST) qDebug("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to PX_WPP_GETLIST_GETWPS\n", wp.seq, msg->sysid);
if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid); if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id) qDebug("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid);
if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq-1 == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid); if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq-1 == protocol_current_wp_id) qDebug("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid);
current_state = PX_WPP_GETLIST_GETWPS; current_state = PX_WPP_GETLIST_GETWPS;
protocol_current_wp_id = wp.seq + 1; protocol_current_wp_id = wp.seq + 1;
...@@ -792,7 +803,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* ...@@ -792,7 +803,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
if(protocol_current_wp_id == protocol_current_count && current_state == PX_WPP_GETLIST_GETWPS) if(protocol_current_wp_id == protocol_current_count && current_state == PX_WPP_GETLIST_GETWPS)
{ {
if (verbose) printf("Got all %u waypoints, changing state to PX_WPP_IDLE\n", protocol_current_count); if (verbose) qDebug("Got all %u waypoints, changing state to PX_WPP_IDLE\n", protocol_current_count);
send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0); send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0);
...@@ -813,7 +824,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* ...@@ -813,7 +824,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
if (waypoints->at(i)->current == 1) if (waypoints->at(i)->current == 1)
{ {
current_active_wp_id = i; current_active_wp_id = i;
//if (verbose) printf("New current waypoint %u\n", current_active_wp_id); //if (verbose) qDebug("New current waypoint %u\n", current_active_wp_id);
yawReached = false; yawReached = false;
posReached = false; posReached = false;
send_waypoint_current(current_active_wp_id); send_waypoint_current(current_active_wp_id);
...@@ -844,36 +855,36 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* ...@@ -844,36 +855,36 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
{ {
//we're done receiving waypoints, answer with ack. //we're done receiving waypoints, answer with ack.
send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0); send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0);
printf("Received MAVLINK_MSG_ID_WAYPOINT while state=PX_WPP_IDLE, answered with WAYPOINT_ACK.\n"); qDebug("Received MAVLINK_MSG_ID_WAYPOINT while state=PX_WPP_IDLE, answered with WAYPOINT_ACK.\n");
} }
if (verbose) if (verbose)
{ {
if (!(current_state == PX_WPP_GETLIST || current_state == PX_WPP_GETLIST_GETWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, current_state); break; } if (!(current_state == PX_WPP_GETLIST || current_state == PX_WPP_GETLIST_GETWPS)) { qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, current_state); break; }
else if (current_state == PX_WPP_GETLIST) else if (current_state == PX_WPP_GETLIST)
{ {
if(!(wp.seq == 0)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq); if(!(wp.seq == 0)) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq);
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); else qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
} }
else if (current_state == PX_WPP_GETLIST_GETWPS) else if (current_state == PX_WPP_GETLIST_GETWPS)
{ {
if (!(wp.seq == protocol_current_wp_id)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, protocol_current_wp_id); if (!(wp.seq == protocol_current_wp_id)) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, protocol_current_wp_id);
else if (!(wp.seq < protocol_current_count)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq); else if (!(wp.seq < protocol_current_count)) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq);
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); else qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
} }
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); else qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
} }
} }
} }
else else
{ {
//we we're target but already communicating with someone else // We're target but already communicating with someone else
if((wp.target_system == systemid && wp.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && current_state != PX_WPP_IDLE) if((wp.target_system == systemid && wp.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && current_state != PX_WPP_IDLE)
{ {
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, protocol_current_partner_systemid); if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, protocol_current_partner_systemid);
} }
else if(wp.target_system == systemid && wp.target_component == compid) else if(wp.target_system == systemid && wp.target_component == compid)
{ {
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid);
} }
} }
break; break;
...@@ -888,7 +899,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* ...@@ -888,7 +899,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
{ {
protocol_timestamp_lastaction = now; protocol_timestamp_lastaction = now;
if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); if (verbose) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid);
while(waypoints->size() > 0) while(waypoints->size() > 0)
{ {
delete waypoints->back(); delete waypoints->back();
...@@ -898,14 +909,14 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* ...@@ -898,14 +909,14 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
} }
else if (wpca.target_system == systemid && wpca.target_component == compid && current_state != PX_WPP_IDLE) else if (wpca.target_system == systemid && wpca.target_component == compid && current_state != PX_WPP_IDLE)
{ {
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, current_state); if (verbose) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, current_state);
} }
break; break;
} }
default: default:
{ {
if (debug) printf("Waypoint: received message of unknown type\n"); if (debug) qDebug("Waypoint: received message of unknown type\n");
break; break;
} }
} }
...@@ -920,7 +931,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* ...@@ -920,7 +931,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
if (timestamp_firstinside_orbit == 0) if (timestamp_firstinside_orbit == 0)
{ {
// Announce that last waypoint was reached // Announce that last waypoint was reached
if (verbose) printf("*** Reached waypoint %u ***\n", cur_wp->seq); if (verbose) qDebug("*** Reached waypoint %u ***\n", cur_wp->seq);
send_waypoint_reached(cur_wp->seq); send_waypoint_reached(cur_wp->seq);
timestamp_firstinside_orbit = now; timestamp_firstinside_orbit = now;
} }
...@@ -950,7 +961,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* ...@@ -950,7 +961,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
waypoints->at(current_active_wp_id)->current = true; waypoints->at(current_active_wp_id)->current = true;
posReached = false; posReached = false;
//yawReached = false; //yawReached = false;
if (verbose) printf("Set new waypoint (%u)\n", current_active_wp_id); if (verbose) qDebug("Set new waypoint (%u)\n", current_active_wp_id);
} }
} }
} }
......
src/comm/MAVLinkSimulationWaypointPlanner.h
View file @ 38d83048
...@@ -24,6 +24,7 @@ public: ...@@ -24,6 +24,7 @@ public:
explicit MAVLinkSimulationWaypointPlanner(MAVLinkSimulationLink *parent, int systemid); explicit MAVLinkSimulationWaypointPlanner(MAVLinkSimulationLink *parent, int systemid);
signals: signals:
void messageSent(const mavlink_message_t& msg);
public slots: public slots:
void handleMessage(const mavlink_message_t& msg); void handleMessage(const mavlink_message_t& msg);
......
src/uas/QGCMAVLinkUASFactory.cc 0 → 100644
View file @ 38d83048
#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;
}
src/uas/QGCMAVLinkUASFactory.h 0 → 100644
View file @ 38d83048
#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
src/uas/UAS.cc
View file @ 38d83048
...@@ -149,6 +149,18 @@ void UAS::setSelected() ...@@ -149,6 +149,18 @@ void UAS::setSelected()
UASManager::instance()->setActiveUAS(this); 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) void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
{ {
if (!link) return; if (!link) return;
......
src/uas/UAS.h
View file @ 38d83048
...@@ -172,10 +172,12 @@ public: ...@@ -172,10 +172,12 @@ public:
UASWaypointManager &getWaypointManager(void) { return waypointManager; } UASWaypointManager &getWaypointManager(void) { return waypointManager; }
int getSystemType(); int getSystemType();
int getAutopilotType() {return autopilot;} int getAutopilotType() {return autopilot;}
void setAutopilotType(int apType) { autopilot = apType;}
public slots: 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 **/ /** @brief Set a new name **/
void setUASName(const QString& name); void setUASName(const QString& name);
/** @brief Executes an action **/ /** @brief Executes an action **/
...@@ -307,6 +309,10 @@ signals: ...@@ -307,6 +309,10 @@ signals:
void writeSettings(); void writeSettings();
/** @brief Read settings from disk */ /** @brief Read settings from disk */
void readSettings(); void readSettings();
// MESSAGE RECEPTION
/** @brief Receive a named value message */
void receiveMessageNamedValue(const mavlink_message_t& message);
}; };
......
src/uas/UASWaypointManager.cc
View file @ 38d83048
...@@ -123,6 +123,10 @@ void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, qui ...@@ -123,6 +123,10 @@ void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, qui
qDebug() << "No waypoints on UAS " << systemId; 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) void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_waypoint_t *wp)
...@@ -165,9 +169,13 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ ...@@ -165,9 +169,13 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
} }
else 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) void UASWaypointManager::handleWaypointAck(quint8 systemId, quint8 compId, mavlink_waypoint_ack_t *wpa)
...@@ -210,6 +218,10 @@ void UASWaypointManager::handleWaypointRequest(quint8 systemId, quint8 compId, m ...@@ -210,6 +218,10 @@ void UASWaypointManager::handleWaypointRequest(quint8 systemId, quint8 compId, m
//TODO: Error message or something //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) void UASWaypointManager::handleWaypointReached(quint8 systemId, quint8 compId, mavlink_waypoint_reached_t *wpr)
......
src/ui/MainWindow.cc
View file @ 38d83048
...@@ -1155,6 +1155,10 @@ void MainWindow::connectCommonActions() ...@@ -1155,6 +1155,10 @@ void MainWindow::connectCommonActions()
// Custom widget actions // Custom widget actions
connect(ui.actionNewCustomWidget, SIGNAL(triggered()), this, SLOT(createCustomWidget())); 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() void MainWindow::connectPxActions()
...@@ -1388,14 +1392,14 @@ void MainWindow::UASCreated(UASInterface* uas) ...@@ -1388,14 +1392,14 @@ void MainWindow::UASCreated(UASInterface* uas)
} }
break; break;
loadOperatorView(); loadOperatorView();
} }
// Change the view only if this is the first UAS // Change the view only if this is the first UAS
// If this is the first connected UAS, it is both created as well as // If this is the first connected UAS, it is both created as well as
// the currently active UAS // 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"; //qDebug() << "UPDATING THE VIEW SINCE THIS IS THE FIRST CONNECTED SYSTEM";
...@@ -1410,8 +1414,7 @@ void MainWindow::UASCreated(UASInterface* uas) ...@@ -1410,8 +1414,7 @@ void MainWindow::UASCreated(UASInterface* uas)
if (settings.contains(getWindowStateKey())) if (settings.contains(getWindowStateKey()))
{ {
restoreState(settings.value(getWindowStateKey()).toByteArray(), QGC::applicationVersion()); restoreState(settings.value(getWindowStateKey()).toByteArray(), QGC::applicationVersion());
} }
} }
else else
{ {
......
src/ui/SlugsHilSim.cc
View file @ 38d83048
...@@ -252,7 +252,8 @@ uint16_t SlugsHilSim::getUint16FromDatagram (const QByteArray* datagram, unsigne ...@@ -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 #ifdef MAVLINK_ENABLED_SLUGS
// HIL code to go here... // HIL code to go here...
//hilLink = linksAvailable //hilLink = linksAvailable
...@@ -260,6 +261,8 @@ void SlugsHilSim::linkSelected(int cbIndex){ ...@@ -260,6 +261,8 @@ void SlugsHilSim::linkSelected(int cbIndex){
hilLink =linksAvailable.value(cbIndex); hilLink =linksAvailable.value(cbIndex);
#else
Q_UNUSED(cbIndex)
#endif #endif
} }
......
src/ui/uas/UASView.cc
View file @ 38d83048
...@@ -137,6 +137,8 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : ...@@ -137,6 +137,8 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
{ {
m_ui->positionLabel->hide(); m_ui->positionLabel->hide();
} }
setSystemType(uas, uas->getSystemType());
} }
UASView::~UASView() 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