Commit 777a7031 authored by lm's avatar lm

Merge branch 'master' of git@pixhawk.ethz.ch:qgroundcontrol

parents 413e8f8e 0303d346
......@@ -428,6 +428,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
mavlink_waypoint_t wp;
mavlink_msg_waypoint_decode(&message, &wp);
qDebug() << "got waypoint (" << wp.seq << ") from ID " << message.sysid << " x=" << wp.x << " y=" << wp.y << " z=" << wp.z;
if(wp.target_system == mavlink->getSystemId() && wp.target_component == mavlink->getComponentId())
{
waypointManager.handleWaypoint(message.sysid, message.compid, &wp);
......
......@@ -37,7 +37,7 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
{
if (systemId == current_partner_systemid && compId == current_partner_compid && current_state == WP_GETLIST_GETWPS && wp->seq == current_wp_id)
{
qDebug() << "got waypoint (" << wp->seq << ") from ID " << systemId;
qDebug() << "got waypoint (" << wp->seq << ") from ID " << systemId << " x=" << wp->x << " y=" << wp->y << " z=" << wp->z;
if(wp->seq == current_wp_id)
{
......@@ -170,6 +170,8 @@ void UASWaypointManager::sendWaypoints(const QVector<Waypoint*> &list)
cur_d->autocontinue = cur_s->getAutoContinue();
cur_d->current = cur_s->getCurrent();
cur_d->orbit = 0.1f; //FIXME
cur_d->type = 1; //FIXME
cur_d->seq = i;
cur_d->x = cur_s->getX();
cur_d->y = cur_s->getY();
......
......@@ -40,7 +40,7 @@ public slots:
signals:
void waypointUpdated(int,quint16,double,double,double,double,bool,bool); ///< Adds a waypoint to the waypoint list widget
void updateStatusString(const QString &); ///< updates the current status string
void updateStatusString(const QString &); ///< updates the current status string
private:
UAS &uas; ///< Reference to the corresponding UAS
......
......@@ -160,6 +160,7 @@ void WaypointList::read()
removeWaypoint(waypoints[0]);
}
redrawList();
emit requestWaypoints();
}
......@@ -189,13 +190,6 @@ void WaypointList::add()
void WaypointList::addWaypoint(Waypoint* wp)
{
if (waypoints.contains(wp))
{
removeWaypoint(wp);
}
waypoints.insert(wp->getId(), wp);
if (!wpViews.contains(wp))
{
WaypointView* wpview = new WaypointView(wp, this);
......@@ -205,7 +199,7 @@ void WaypointList::addWaypoint(Waypoint* wp)
connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(Waypoint*)));
connect(wpview, SIGNAL(removeWaypoint(Waypoint*)), this, SLOT(removeWaypoint(Waypoint*)));
connect(wpview, SIGNAL(setCurrentWaypoint(Waypoint*)), this, SLOT(setCurrentWaypoint(Waypoint*)));
connect(wpview, SIGNAL(waypointUpdated(Waypoint*)), this, SIGNAL(waypointChanged(Waypoint*)));
//connect(wpview, SIGNAL(waypointUpdated(Waypoint*)), this, SIGNAL(waypointChanged(Waypoint*)));
}
}
......@@ -280,7 +274,8 @@ void WaypointList::moveDown(Waypoint* wp)
void WaypointList::removeWaypoint(Waypoint* wp)
{
// Delete from list
if (wp != NULL){
if (wp != NULL)
{
waypoints.remove(wp->getId());
for(int i = wp->getId(); i < waypoints.size(); i++)
{
......
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