Improving simulation

images/earth.html

@@ -71,6 +71,11 @@ function init()
 	google.earth.createInstance("map3d", initCallback, failureCallback);
 }
 
+function setFollowEnabled(enable)
+{
+	followEnabled = enable;
+}
+
 
 
 function setCurrAircraft(id)
@@ -214,8 +219,8 @@ function initCallback(object)
     ge.getWindow().setVisibility(true);
     ge.getOptions().setStatusBarVisibility(true); 
     ge.getOptions().setScaleLegendVisibility(true);
-    ge.getOptions().setFlyToSpeed(5.0); 
-    //ge.getOptions().setFlyToSpeed(ge.SPEED_TELEPORT); 
+    //ge.getOptions().setFlyToSpeed(5.0); 
+    ge.getOptions().setFlyToSpeed(ge.SPEED_TELEPORT); 
     ge.getNavigationControl().setVisibility(ge.VISIBILITY_AUTO);
         
     ge.getLayerRoot().enableLayerById(ge.LAYER_TERRAIN, true); 
@@ -233,11 +238,15 @@ function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw)
 		if (lastLat == 0)
 		{
 			lastLat = currLat;
-			lastLon = lastLon;
+			lastLon = currLon;
 		}
 		currLat = lat;
 		currLon = lon;
 		currAlt = alt;
+		// Interpolate between t-1 and t and set new states
+		lastLat = lastLat*0.8+currLat*0.2;
+		lastLon = lastLon*0.8+currLon*0.2;
+		lastAlt = lastAlt*0.8+currAlt*0.2;
 		//currFollowHeading = ((yaw/M_PI)+1.0)*360.0;
 	
 	
@@ -246,10 +255,14 @@ function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw)
 	planeOrient.setTilt(((pitch/M_PI)+1.0)*360.0);
 	planeOrient.setHeading(((yaw/M_PI)+1.0)*360.0);
 	
-    planeLoc.setLatitude(lat);
-    planeLoc.setLongitude(lon);
-    planeLoc.setAltitude(alt);
+	currFollowHeading = ((yaw/M_PI)+1.0)*360.0;
+    
+    planeLoc.setLatitude(lastLat);
+    planeLoc.setLongitude(lastLon);
+    planeLoc.setAltitude(lastAlt);
     planeModel.setLocation(planeLoc);
+    
+    if (followEnabled) updateFollowAircraft();
     }
 }
 
@@ -284,10 +297,6 @@ function setCurrentAircraft(id)
 function updateFollowAircraft()
 {
 	currView = ge.getView().copyAsLookAt(ge.ALTITUDE_ABSOLUTE);
-	// Interpolate between t-1 and t and set new states
-	lastLat = lastLat*0.7+currLat*0.3;
-	lastLon = lastLon*0.7+currLon*0.3;
-	lastAlt = lastAlt*0.7+currAlt*0.3;
 	currView.setLatitude(lastLat);
 	currView.setLongitude(lastLon);
 	currView.setAltitude(lastAlt);

src/comm/MAVLinkSimulationLink.cc

@@ -451,6 +451,11 @@ void MAVLinkSimulationLink::mainloop()
         memcpy(stream+streampointer,buffer, bufferlength);
         streampointer += bufferlength;
 
+        // ATTITUDE VEHICLE 2
+        mavlink_msg_attitude_pack(54, MAV_COMP_ID_IMU, &ret, 0, 0, 0, atan2((y/2)+0.3, (x+0.002)), 0, 0, 0);
+        sendMAVLinkMessage(&ret);
+
+
 //        // GLOBAL POSITION VEHICLE 3
 //        mavlink_msg_global_position_int_pack(60, componentId, &ret, (473780.28137103+(x/2+0.002))*1E3, (85489.9892510421+((y*2)+0.3))*1E3, (z+590.0)*1000.0, 0*100.0, 0*100.0, 0*100.0);
 //        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);

src/comm/MAVLinkSimulationMAV.cc

@@ -11,24 +11,24 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy
     timer25Hz(0),
     timer10Hz(0),
     timer1Hz(0),
-    latitude(0.0),
-    longitude(0.0),
+    latitude(47.376389),
+    longitude(8.548056),
     altitude(0.0),
-    x(0.0),
-    y(0.0),
-    z(0.0),
+    x(8.548056),
+    y(47.376389),
+    z(550),
     roll(0.0),
     pitch(0.0),
     yaw(0.0),
     globalNavigation(true),
     firstWP(false),
-    previousSPX(0.0),
-    previousSPY(0.0),
-    previousSPZ(0.0),
+    previousSPX(8.548056),
+    previousSPY(47.376389),
+    previousSPZ(550),
     previousSPYaw(0.0),
-    nextSPX(0.0),
-    nextSPY(0.0),
-    nextSPZ(0.0),
+    nextSPX(8.548056),
+    nextSPY(47.376389),
+    nextSPZ(550),
     nextSPYaw(0.0)
 {
     // Please note: The waypoint planner is running
@@ -46,38 +46,54 @@ void MAVLinkSimulationMAV::mainloop()
 
 //        double xNew = // (nextSPX - previousSPX)
 
-    if (!firstWP)
-    {
-        double xm = (nextSPX - x) * 0.01;
-        double ym = (nextSPY - y) * 0.01;
-        double zm = (nextSPZ - z) * 0.1;
-
-        x += xm;
-        y += ym;
-        z += zm;
-
-        //if (xm < 0.001) xm
-    }
-    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);
+        planner.handleMessage(msg);
         timer1Hz = 50;
     }
 
     // 10 Hz execution
     if (timer10Hz <= 0)
     {
+        if (!firstWP)
+        {
+            double radPer100ms = 0.0002;
+            double altPer100ms = 0.1;
+            double xm = (nextSPX - x);
+            double ym = (nextSPY - y);
+            double zm = (nextSPZ - z);
+
+            float zsign = (zm < 0) ? -1.0f : 1.0f;
+
+            //float trueyaw = atan2f(xm, ym);
+
+            yaw = yaw*0.9 + 0.1*atan2f(xm, ym);
+
+            qDebug() << "SIMULATION MAV: x:" << xm << "y:" << ym << "z:" << zm << "yaw:" << yaw;
+
+            if (sqrt(xm*xm+ym*ym) > 0.0001)
+            {
+                x += cos(yaw)*radPer100ms;
+                y += sin(yaw)*radPer100ms;
+                z += altPer100ms*zsign;
+            }
+
+            //if (xm < 0.001) xm
+        }
+        else
+        {
+            x = nextSPX;
+            y = nextSPY;
+            z = nextSPZ;
+            firstWP = false;
+            qDebug() << "INIT STEP";
+        }
+
+
         mavlink_message_t msg;
         mavlink_global_position_int_t pos;
         pos.alt = z*1000.0;
@@ -88,6 +104,16 @@ void MAVLinkSimulationMAV::mainloop()
         pos.vz = 0;
         mavlink_msg_global_position_int_encode(systemid, MAV_COMP_ID_IMU, &msg, &pos);
         link->sendMAVLinkMessage(&msg);
+        planner.handleMessage(msg);
+        mavlink_attitude_t attitude;
+        attitude.roll = 0.0f;
+        attitude.pitch = 0.0f;
+        attitude.yaw = yaw;
+
+        qDebug() << "YAW" << yaw;
+
+        mavlink_msg_attitude_encode(systemid, MAV_COMP_ID_IMU, &msg, &attitude);
+        link->sendMAVLinkMessage(&msg);
         timer10Hz = 5;
     }
 
@@ -127,9 +153,9 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)
                 //nextSPYaw = sp.yaw;
 
                 // Airplane
-                yaw = atan2(previousSPX-nextSPX, previousSPY-nextSPY);
+                //yaw = atan2(previousSPX-nextSPX, previousSPY-nextSPY);
 
-                if (!firstWP) firstWP = true;
+                //if (!firstWP) firstWP = true;
             }
             //qDebug() << "UPDATED SP:" << "X" << nextSPX << "Y" << nextSPY << "Z" << nextSPZ;
         }

src/comm/MAVLinkSimulationWaypointPlanner.cc

@@ -437,6 +437,20 @@ float MAVLinkSimulationWaypointPlanner::distanceToPoint(uint16_t seq, float x, f
         return -1.f;
 }
 
+float MAVLinkSimulationWaypointPlanner::distanceToPoint(uint16_t seq, float x, float y)
+{
+        if (seq < waypoints->size())
+        {
+                mavlink_waypoint_t *cur = waypoints->at(seq);
+
+                const PxVector3 A(cur->x, cur->y, 0);
+                const PxVector3 C(x, y, 0);
+
+                return (C-A).length();
+        }
+        return -1.f;
+}
+
 void MAVLinkSimulationWaypointPlanner::handleMessage(const mavlink_message_t& msg)
 {
     mavlink_handler(&msg);
@@ -502,6 +516,10 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                                                 if (att.yaw - yaw_tolerance < wp->yaw || wp->yaw < upperBound)
                                                         yawReached = true;
                                         }
+
+                                        // FIXME HACK: Ignore yaw:
+
+                                        yawReached = true;
                                 }
                         }
                         break;
@@ -543,6 +561,50 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                         break;
                 }
 
+        case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
+        {
+                        qDebug() << "GOT GLOBAL POS" << "sys:" << msg->sysid << "wpid" << current_active_wp_id << "wpsize" << waypoints->size();
+                if(msg->sysid == systemid && current_active_wp_id < waypoints->size())
+                {
+                        mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id);
+
+                        if(wp->frame == 0)
+                        {
+                                mavlink_global_position_int_t pos;
+                                mavlink_msg_global_position_int_decode(msg, &pos);
+
+                                float x = static_cast<double>(pos.lon)/1E7;
+                                float y = static_cast<double>(pos.lat)/1E7;
+                                float z = static_cast<double>(pos.alt)/1000;
+
+                                qDebug() << "Received new position: x:" << x << "| y:" << y << "| z:" << z;
+
+                                posReached = false;
+                                yawReached = true;
+
+                                // FIXME big hack for simulation!
+                                //float oneDegreeOfLatMeters = 111131.745f;
+                                float orbit = 0.0001;
+
+                                // compare current position (given in message) with current waypoint
+                                //float orbit = wp->param1;
+
+                                // Convert to degrees
+
+
+                                float dist;
+                                dist = distanceToPoint(current_active_wp_id, x, y);
+
+                                if (dist >= 0.f && dist <= orbit && yawReached)
+                                {
+                                        posReached = true;
+                                        qDebug() << "WP PLANNER: REACHED POSITION";
+                                }
+                        }
+                }
+                break;
+        }
+
                 case MAVLINK_MSG_ID_ACTION: // special action from ground station
                 {
                         mavlink_action_t action;

src/comm/MAVLinkSimulationWaypointPlanner.h

@@ -68,6 +68,7 @@ protected:
     void send_waypoint_reached(uint16_t seq);
     float distanceToSegment(uint16_t seq, float x, float y, float z);
     float distanceToPoint(uint16_t seq, float x, float y, float z);
+    float distanceToPoint(uint16_t seq, float x, float y);
     void mavlink_handler(const mavlink_message_t* msg);
 
 };

src/uas/UASWaypointManager.cc

@@ -138,9 +138,9 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
 
         if(wp->seq == current_wp_id)
         {
-            qDebug() << "Got WP: " << wp->seq << wp->x <<  wp->y << wp->z << wp->yaw << wp->autocontinue << wp->current << wp->param1 << wp->param2 << (MAV_FRAME) wp->frame << (MAV_ACTION) wp->action;
+            qDebug() << "Got WP: " << wp->seq << wp->x <<  wp->y << wp->z << wp->yaw << "auto:" << wp->autocontinue << "curr:" << wp->current << wp->param1 << wp->param2 << (MAV_FRAME) wp->frame << (MAV_ACTION) wp->action;
             Waypoint *lwp = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->param1, wp->param2, (MAV_FRAME) wp->frame, (MAV_ACTION) wp->action);
-            addWaypoint(lwp);
+            addWaypoint(lwp, false);
 
             //get next waypoint
             current_wp_id++;
@@ -320,11 +320,15 @@ int UASWaypointManager::setCurrentWaypoint(quint16 seq)
     return -1;
 }
 
-void UASWaypointManager::addWaypoint(Waypoint *wp)
+/**
+ * @param enforceFirstActive Enforces that the first waypoint is set as active
+ */
+void UASWaypointManager::addWaypoint(Waypoint *wp, bool enforceFirstActive)
 {
     if (wp)
     {
         wp->setId(waypoints.size());
+        if (enforceFirstActive && waypoints.size() == 0) wp->setCurrent(true);
         waypoints.insert(waypoints.size(), wp);
         connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChange(Waypoint*)));
 
@@ -517,6 +521,7 @@ void UASWaypointManager::writeWaypoints()
 {
     if (current_state == WP_IDLE)
     {
+        // Send clear all if count == 0
         if (waypoints.count() > 0)
         {
             protocol_timer.start(PROTOCOL_TIMEOUT_MS);
@@ -529,7 +534,9 @@ void UASWaypointManager::writeWaypoints()
             current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER;
 
             //clear local buffer
-            //TODO: Why not replace with waypoint_buffer.clear() ? - because this will lead to memory leaks, the waypoint-structs have to be deleted, clear() would only delete the pointers.
+            // Why not replace with waypoint_buffer.clear() ?
+            // because this will lead to memory leaks, the waypoint-structs
+            // have to be deleted, clear() would only delete the pointers.
             while(!waypoint_buffer.empty())
             {
                 delete waypoint_buffer.back();

src/uas/UASWaypointManager.h

@@ -114,7 +114,7 @@ public slots:
     void timeout();                                 ///< Called by the timer if a response times out. Handles send retries.
     /** @name Waypoint list operations */
     /*@{*/
-    void addWaypoint(Waypoint *wp);                 ///< adds a new waypoint to the end of the list and changes its sequence number accordingly
+    void addWaypoint(Waypoint *wp, bool enforceFirstActive=true);                 ///< adds a new waypoint to the end of the list and changes its sequence number accordingly
     int removeWaypoint(quint16 seq);                       ///< locally remove the specified waypoint from the storage
     void moveWaypoint(quint16 cur_seq, quint16 new_seq);   ///< locally move a waypoint from its current position cur_seq to a new position new_seq
     void saveWaypoints(const QString &saveFile);           ///< saves the local waypoint list to saveFile

src/ui/MapWidget.cc

@@ -94,9 +94,9 @@ MapWidget::MapWidget(QWidget *parent) :
 
     // SET INITIAL POSITION AND ZOOM
     // Set default zoom level
-    mc->setZoom(17);
+    mc->setZoom(16);
     // Zurich, ETH
-    mc->setView(QPointF(8.548056,47.376389));
+    mc->setView(QPointF(8.548056,47.376889));
 
     // Veracruz Mexico
     //mc->setView(QPointF(-96.105208,19.138955));
@@ -411,7 +411,7 @@ void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordina
 
         if (mav)
         {
-            mav->getWaypointManager()->addWaypoint(new Waypoint(mav->getWaypointManager()->getWaypointList().count(), coordinate.x(), coordinate.y()));
+            mav->getWaypointManager()->addWaypoint(new Waypoint(mav->getWaypointManager()->getWaypointList().count(), coordinate.x(), coordinate.y(), 0.0f, 0.0f, true));
         }
         else
         {
@@ -824,7 +824,8 @@ mc->updateRequest(p->boundingBox().toRect());
 
     //mc->updateRequestNew();//(uasTrails.value(uas->getUASID())->boundingBox().toRect());
 
-
+    if (uas->getUASID() == this->mav->getUASID())
+{
     // Limit the position update rate
     quint64 currTime = MG::TIME::getGroundTimeNow();
     if (currTime - lastUpdate > 120)
@@ -842,6 +843,7 @@ mc->updateRequest(p->boundingBox().toRect());
         }
     }
 }
+}
 
 /**
  * Center the view on this position

src/ui/WaypointList.cc

@@ -507,29 +507,6 @@ void WaypointList::on_clearWPListButton_clicked()
     }
 }
 
-/** @brief Add a waypoint by mouse click over the map */
-void WaypointList::addWaypointMouse(QPointF coordinate)
-{
-    if (uas)
-    {
-        const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
-        if (waypoints.size() > 0)
-        {
-            Waypoint *last = waypoints.at(waypoints.size()-1);
-            Waypoint *wp = new Waypoint(0, coordinate.x(), coordinate.y(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime());
-            uas->getWaypointManager()->addWaypoint(wp);
-        }
-        else
-        {
-            Waypoint *wp = new Waypoint(0, coordinate.x(), coordinate.y(), -0.8, 0.0, true, true, 0.15, 2000);
-            uas->getWaypointManager()->addWaypoint(wp);
-        }
-
-
-    }
-
-}
-
 /** @brief The MapWidget informs that a waypoint global was changed on the map */
 
 void WaypointList::waypointGlobalChanged(QPointF coordinate, int indexWP)

src/ui/WaypointList.h

@@ -75,10 +75,6 @@ public slots:
     /** @brief Add a waypoint at the current MAV position */
     void addCurrentPositonWaypoint();
     /** @brief Add a waypoint by mouse click over the map */
-    void addWaypointMouse(QPointF coordinate);
-    /** @brief it notifies that a global waypoint goes to do created */
-    //void setIsWPGlobal(bool value, QPointF centerCoordinate);
-
 
     //Update events
     /** @brief sets statusLabel string */

src/ui/WaypointView.cc

@@ -170,13 +170,13 @@ void WaypointView::changedCurrent(int state)
 {
     if (state == 0)
     {
-        //m_ui->selectedBox->setChecked(true);
-        //m_ui->selectedBox->setCheckState(Qt::Checked);
-        //wp->setCurrent(false);
+        m_ui->selectedBox->setChecked(true);
+        m_ui->selectedBox->setCheckState(Qt::Checked);
+        wp->setCurrent(false);
     }
     else
     {
-        //wp->setCurrent(true);
+        wp->setCurrent(true);
         emit changeCurrentWaypoint(wp->getId());   //the slot changeCurrentWaypoint() in WaypointList sets all other current flags to false
     }
 }
@@ -255,8 +255,14 @@ void WaypointView::updateValues()
     {
         m_ui->yawSpinBox->setValue(wp->getYaw()/M_PI*180.);
     }
-    m_ui->selectedBox->setChecked(wp->getCurrent());
-    m_ui->autoContinue->setChecked(wp->getAutoContinue());
+    if (m_ui->selectedBox->isChecked() != wp->getCurrent())
+    {
+        m_ui->selectedBox->setChecked(wp->getCurrent());
+    }
+    if (m_ui->autoContinue->isChecked() != wp->getAutoContinue())
+    {
+        m_ui->autoContinue->setChecked(wp->getAutoContinue());
+    }
     m_ui->idLabel->setText(QString("%1").arg(wp->getId()));
     if (m_ui->orbitSpinBox->value() != wp->getOrbit())
     {

src/ui/map/MAV2DIcon.cc

@@ -42,17 +42,18 @@ void MAV2DIcon::setPen(QPen* pen)
  */
 void MAV2DIcon::setYaw(float yaw)
 {
-    float diff = fabs(yaw - this->yaw);
-    while (diff > M_PI)
-    {
-        diff -= M_PI;
-    }
+    //qDebug() << "MAV2Icon" << yaw;
+    //float diff = fabs(yaw - this->yaw);
+//    while (diff > M_PI)
+//    {
+//        diff -= M_PI;
+//    }
 
-    if (diff > 0.25)
-    {
+//    if (diff > 0.25)
+//    {
         this->yaw = yaw;
         drawIcon(mypen);
-    }
+//    }
 }
 
 void MAV2DIcon::drawIcon(QPen* pen)

src/ui/map3D/QGCGoogleEarthView.cc

@@ -185,6 +185,7 @@ void QGCGoogleEarthView::follow(bool follow)
 {
     ui->followAirplaneCheckbox->setChecked(follow);
     followCamera = follow;
+    if (gEarthInitialized) javaScript(QString("setFollowEnabled(%1)").arg(follow));
 }
 
 void QGCGoogleEarthView::goHome()
@@ -353,6 +354,8 @@ void QGCGoogleEarthView::initializeGoogleEarth()
             // Start update timer
             updateTimer->start(refreshRateMs);
 
+            follow(this->followCamera);
+
             gEarthInitialized = true;
         }
     }
@@ -395,11 +398,6 @@ void QGCGoogleEarthView::updateState()
                        .arg(pitch, 0, 'f', 9)
                        .arg(yaw, 0, 'f', 9));
         }
-
-        if (followCamera)
-        {
-            javaScript(QString("updateFollowAircraft()"));
-        }
     }
 }