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 \
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
......
qgroundcontrol.pro
View file @ 38d83048
......@@ -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
......
src/GAudioOutput.cc
View file @ 38d83048
......@@ -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();
}
......
src/Waypoint.cc
View file @ 38d83048
......@@ -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)
......
src/comm/MAVLinkProtocol.cc
View file @ 38d83048
......@@ -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
......
src/comm/MAVLinkSimulationLink.cc
View file @ 38d83048
......@@ -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)
......
src/comm/MAVLinkSimulationMAV.cc
View file @ 38d83048
#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;
}
}
src/comm/MAVLinkSimulationMAV.h
View file @ 38d83048
......@@ -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;
};
......
src/comm/MAVLinkSimulationWaypointPlanner.cc
View file @ 38d83048
......@@ -34,6 +34,7 @@ This file is part of the PIXHAWK project
#include "MAVLinkSimulationWaypointPlanner.h"
#include "QGC.h"
#include <QDebug>
#ifndef M_PI
#define M_PI 3.14159265358979323846
......@@ -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)
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)
}
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;
}
}
......@@ -311,7 +322,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint_count(uint8_t target_system
mavlink_msg_waypoint_count_encode(systemid, compid, &msg, &wpc);
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
wp->target_system = target_systemid;
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);
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
{
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
wpr.seq = seq;
mavlink_msg_waypoint_request_encode(systemid, compid, &msg, &wpr);
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)
mavlink_msg_waypoint_reached_encode(systemid, compid, &msg, &wp_reached);
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*
//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 (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;
protocol_current_count = 0;
protocol_current_partner_systemid = 0;
......@@ -506,7 +517,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
{
mavlink_local_position_t 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;
......@@ -538,7 +549,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
mavlink_msg_action_decode(msg, &action);
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)
{
// case MAV_ACTION_LAUNCH:
......@@ -585,7 +596,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
{
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;
protocol_current_wp_id = 0;
}
......@@ -607,7 +618,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
{
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;
uint32_t i;
for(i = 0; i < waypoints->size(); i++)
......@@ -621,7 +632,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
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;
posReached = false;
send_waypoint_current(current_active_wp_id);
......@@ -630,7 +641,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
}
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*
{
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_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_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) 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;
protocol_current_wp_id = 0;
protocol_current_partner_systemid = msg->sysid;
......@@ -658,19 +669,19 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
}
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();
send_waypoint_count(msg->sysid,msg->compid, protocol_current_count);
}
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
{
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;
}
......@@ -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)
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_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) 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) 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) 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) 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;
protocol_current_wp_id = wpr.seq;
......@@ -698,17 +709,17 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
{
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)
{
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)
{
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);
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);
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()) 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*
//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 (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;
......@@ -735,8 +746,8 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
{
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_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\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) qDebug("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid);
current_state = PX_WPP_GETLIST;
protocol_current_wp_id = 0;
......@@ -744,7 +755,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
protocol_current_partner_compid = msg->compid;
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)
{
delete waypoints_receive_buffer->back();
......@@ -755,14 +766,14 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
}
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
{
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);
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 printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n");
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) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", protocol_current_wp_id);
else qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n");
}
}
break;
......@@ -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
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_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-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) 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) 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) qDebug("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid);
current_state = PX_WPP_GETLIST_GETWPS;
protocol_current_wp_id = wp.seq + 1;
......@@ -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 (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);
......@@ -813,7 +824,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
if (waypoints->at(i)->current == 1)
{
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;
posReached = false;
send_waypoint_current(current_active_wp_id);
......@@ -844,36 +855,36 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
{
//we're done receiving waypoints, answer with ack.
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 (!(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)
{
if(!(wp.seq == 0)) printf("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);
if(!(wp.seq == 0)) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\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)
{
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);
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 printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
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)) qDebug("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\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
{
//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 (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)
{
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;
......@@ -888,7 +899,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
{
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)
{
delete waypoints->back();
......@@ -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)
{
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;
}
default:
{
if (debug) printf("Waypoint: received message of unknown type\n");
if (debug) qDebug("Waypoint: received message of unknown type\n");
break;
}
}
......@@ -920,7 +931,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
if (timestamp_firstinside_orbit == 0)
{
// 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);
timestamp_firstinside_orbit = now;
}
......@@ -950,7 +961,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
waypoints->at(current_active_wp_id)->current = true;
posReached = 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:
explicit MAVLinkSimulationWaypointPlanner(MAVLinkSimulationLink *parent, int systemid);
signals:
void messageSent(const mavlink_message_t& msg);
public slots:
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()
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;
......
src/uas/UAS.h
View file @ 38d83048
......@@ -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);
};
......
src/uas/UASWaypointManager.cc
View file @ 38d83048
......@@ -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)
......
src/ui/MainWindow.cc
View file @ 38d83048
......@@ -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
{
......
src/ui/SlugsHilSim.cc
View file @ 38d83048
......@@ -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
}
......
src/ui/uas/UASView.cc
View file @ 38d83048
......@@ -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