Merge branch 'release/wired-launch'

README.md

@@ -6,6 +6,7 @@
 
 ### Updates
 
+- *March 20, 2013*: Release related to company launch/Wired article
 - *October 23, 2012*: Create this seperate package
 
 ## Usage

launch/track.launch

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 -->
 
   <node name="ardrone_driver" pkg="ardrone_autonomy" type="ardrone_driver" >
-    <param name="realtime_navdata" value="true" />
+<!--    <param name="realtime_navdata" value="true" /> -->
   </node>
 
   <node name="ardrone_tracker" pkg="falkor_ardrone" type="ardrone_tracker.py"  output="screen" >

nodes/ardrone_follow.py

@@ -71,12 +71,16 @@ class ArdroneFollow:
         self.linearXlimit = 1.0
         self.linearZlimit = 2.0
 
-        self.xPid = pid.Pid( 0.020, 0.0, 0.0, self.angularZlimit )
-        self.yPid = pid.Pid( 0.020, 0.0, 0.0, self.linearZlimit )
+        # Increasing the P term for yaw
+        self.xPid = pid.Pid( 0.080, 0.0, 0.0, self.angularZlimit )
+        self.yPid = pid.Pid( 0.050, 0.0, 0.0, self.linearZlimit )
         self.zPid = pid.Pid( 0.050, 0.0, 0.0, self.linearXlimit )
 
-        self.xPid.setPointMin = 40
-        self.xPid.setPointMax = 60
+        # alpha for the ema filter on the found point
+        self.alpha = 0.5
+
+        self.xPid.setPointMin = 45
+        self.xPid.setPointMax = 55
 
         self.yPid.setPointMin = 40
         self.yPid.setPointMax = 60
@@ -207,7 +211,17 @@ class ArdroneFollow:
         self.reset_pub.publish( Empty() )
 
     def found_point_cb( self, data ):
-        self.found_point = data
+        if data.z != -1.0:
+            if self.found_point.z == -1.0:
+                self.found_point = data
+            else:
+                # update the ema found_point, if we didn't just re-acquire
+                self.found_point.x = self.found_point.x * self.alpha + data.x * ( 1.0 - self.alpha )
+                self.found_point.y = self.found_point.y * self.alpha + data.y * ( 1.0 - self.alpha )
+                self.found_point.z = self.found_point.z * self.alpha + data.z * ( 1.0 - self.alpha )
+        else:
+            self.found_point = data
+
         self.found_time = rospy.Time.now()
 
     def hover( self ):