Merge branch 'master' of https://code.google.com/p/ardupilot-mega

ArduPlane/GCS_Mavlink.pde

@@ -264,6 +264,9 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
             mode 		= MAV_MODE_UNINIT;
             nav_mode 	= MAV_NAV_GROUNDED;
             break;
+        case CIRCLE:
+            mode        = MAV_MODE_TEST3;
+            break;
         }
 
         uint8_t status 		= MAV_STATE_ACTIVE;

Tools/ArdupilotMegaPlanner/CommsSerialInterface.cs

@@ -27,6 +27,8 @@ namespace ArdupilotMega
          //void Write(char[] buffer, int offset, int count);
          void WriteLine(string text);
 
+         void toggleDTR();
+
         // Properties
          //Stream BaseStream { get; }
          int BaudRate { get; set; }

Tools/ArdupilotMegaPlanner/CommsSerialPort.cs

@@ -9,5 +9,12 @@ namespace ArdupilotMega
     class SerialPort : System.IO.Ports.SerialPort,ICommsSerial
     {
 
+
+        public void toggleDTR()
+        {
+            DtrEnable = true;
+            System.Threading.Thread.Sleep(100);
+            DtrEnable = false;
+        }
     }
 }

Tools/ArdupilotMegaPlanner/CommsTCPSerial.cs

@@ -35,6 +35,10 @@ namespace System.IO.Ports
             Port = "5760";
         }
 
+        public void toggleDTR()
+        {
+        }
+
         public string Port { get; set; }
 
         public int ReadTimeout

Tools/ArdupilotMegaPlanner/CommsUdpSerial.cs

@@ -35,6 +35,10 @@ namespace System.IO.Ports
             Port = "14550";
         }
 
+        public void toggleDTR()
+        {
+        }
+
         public string Port { get; set; }
 
         public int ReadTimeout

Tools/ArdupilotMegaPlanner/CurrentState.cs

@@ -461,7 +461,7 @@ namespace ArdupilotMega
                             }
                             break;
                         case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST3:
-                            mode = "FBW B";
+                            mode = "Circle";
                             break;
                         case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_AUTO:
                             switch (sysstatus.nav_mode)

Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs

@@ -693,7 +693,7 @@ namespace ArdupilotMega.GCSViews
                         }
                     }
 
-                    lbl_status.Text = "Write Done... Waiting (90 sec)";
+                    lbl_status.Text = "Write Done... Waiting (17 sec)";
                 }
                 else
                 {
@@ -714,7 +714,7 @@ namespace ArdupilotMega.GCSViews
 
                 DateTime startwait = DateTime.Now;
 
-                while ((DateTime.Now - startwait).TotalSeconds < 75)
+                while ((DateTime.Now - startwait).TotalSeconds < 17)
                 {
                     try
                     {
@@ -722,7 +722,7 @@ namespace ArdupilotMega.GCSViews
                     }
                     catch { }
                     System.Threading.Thread.Sleep(1000);
-                    progress.Value = (int)((DateTime.Now - startwait).TotalSeconds / 75 * 100);
+                    progress.Value = (int)((DateTime.Now - startwait).TotalSeconds / 17 * 100);
                     progress.Refresh();
                 }
                 try

Tools/ArdupilotMegaPlanner/GCSViews/Terminal.cs

@@ -172,14 +172,14 @@ namespace ArdupilotMega.GCSViews
                 if (comPort.IsOpen)
                     comPort.Close();
 
-                comPort.DtrEnable = true;
-
                 comPort.ReadBufferSize = 1024 * 1024;
 
                 comPort.PortName = MainV2.comportname;
 
                 comPort.Open();
 
+                comPort.toggleDTR();
+
                 System.Threading.Thread t11 = new System.Threading.Thread(delegate()
                 {
                     threadrun = true;

Tools/ArdupilotMegaPlanner/Log.cs

@@ -69,9 +69,7 @@ namespace ArdupilotMega
             //comPort.ReadBufferSize = 1024 * 1024;
             try
             {
-                comPort.DtrEnable = false;
+                comPort.toggleDTR();
-                System.Threading.Thread.Sleep(100);
-                comPort.DtrEnable = true;
                 //comPort.Open();
             }
             catch (Exception)

Tools/ArdupilotMegaPlanner/MAVLink.cs

@@ -98,10 +98,7 @@ namespace ArdupilotMega
 
                     BaseStream.DiscardInBuffer();
 
-                    System.Threading.Thread.Sleep(200); // allow reset to work
+                    BaseStream.toggleDTR();
-
-                    if (BaseStream.DtrEnable)
-                        BaseStream.DtrEnable = false;
 
                     // allow 2560 connect timeout on usb
                     System.Threading.Thread.Sleep(1000);

Tools/ArdupilotMegaPlanner/MainV2.cs

@@ -685,7 +685,7 @@ namespace ArdupilotMega
                 comPort.BaseStream.DtrEnable = false;
 
                 if (config["CHK_resetapmonconnect"] == null || bool.Parse(config["CHK_resetapmonconnect"].ToString()) == true)
-                    comPort.BaseStream.DtrEnable = true;
+                    comPort.BaseStream.toggleDTR();
 
                 try
                 {
@@ -1047,6 +1047,8 @@ namespace ArdupilotMega
                                 {
                                     this.MenuConnect.BackgroundImage = global::ArdupilotMega.Properties.Resources.disconnect;
                                     this.MenuConnect.BackgroundImage.Tag = "Disconnect";
+                                    CMB_baudrate.Enabled = false;
+                                    CMB_serialport.Enabled = false;
                                 });
                             }
                         }
@@ -1058,6 +1060,8 @@ namespace ArdupilotMega
                                 {
                                     this.MenuConnect.BackgroundImage = global::ArdupilotMega.Properties.Resources.connect;
                                     this.MenuConnect.BackgroundImage.Tag = "Connect";
+                                    CMB_baudrate.Enabled = true;
+                                    CMB_serialport.Enabled = true;
                                 });
                             }
                         }

Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs

@@ -34,5 +34,5 @@ using System.Resources;
 // by using the '*' as shown below:
 // [assembly: AssemblyVersion("1.0.*")]
 [assembly: AssemblyVersion("1.0.0.0")]
-[assembly: AssemblyFileVersion("1.1.14")]
+[assembly: AssemblyFileVersion("1.1.15")]
 [assembly: NeutralResourcesLanguageAttribute("")]

Tools/ArdupilotMegaPlanner/Setup/Setup.cs

@@ -811,11 +811,11 @@ namespace ArdupilotMega.Setup
             }
             catch (Exception ex) { MainV2.givecomport = false; MessageBox.Show("Invalid Comport Settings : " + ex.Message); return; }
 
-            BUT_reset.Text = "Rebooting (75 sec)";
+            BUT_reset.Text = "Rebooting (17 sec)";
             BUT_reset.Refresh();
             Application.DoEvents();
 
-            Sleep(75000, comPortT); // wait for boot/reset
+            Sleep(17000, comPortT); // wait for boot/reset
 
             comPortT.DtrEnable = false;
 

Tools/autotest/pymavlink/mavextra.py

@@ -32,3 +32,7 @@ def TrueHeading(SERVO_OUTPUT_RAW):
 def kmh(mps):
     '''convert m/s to Km/h'''
     return mps*3.6
+
+def altitude(press_abs, ground_press=955.0, ground_temp=30):
+    '''calculate barometric altitude'''
+    return log(ground_press/press_abs)*(ground_temp+273.15)*29271.267*0.001

Tools/autotest/pymavlink/mavlink.py

@@ -156,6 +156,18 @@ MAV_CMD_PREFLIGHT_STORAGE = 245 # Request storage of different parameter values
                         # will be only accepted if in pre-flight mode.
 MAV_CMD_ENUM_END = 246 # 
 
+# FENCE_ACTION
+FENCE_ACTION_NONE = 0 # Disable fenced mode
+FENCE_ACTION_GUIDED = 1 # Switched to guided mode to return point (fence point 0)
+FENCE_ACTION_ENUM_END = 2 # 
+
+# FENCE_BREACH
+FENCE_BREACH_NONE = 0 # No last fence breach
+FENCE_BREACH_MINALT = 1 # Breached minimum altitude
+FENCE_BREACH_MAXALT = 2 # Breached minimum altitude
+FENCE_BREACH_BOUNDARY = 3 # Breached fence boundary
+FENCE_BREACH_ENUM_END = 4 # 
+
 # MAV_DATA_STREAM
 MAV_DATA_STREAM_ALL = 0 # Enable all data streams
 MAV_DATA_STREAM_RAW_SENSORS = 1 # Enable IMU_RAW, GPS_RAW, GPS_STATUS packets.
@@ -188,6 +200,9 @@ MAVLINK_MSG_ID_DIGICAM_CONTROL = 155
 MAVLINK_MSG_ID_MOUNT_CONFIGURE = 156
 MAVLINK_MSG_ID_MOUNT_CONTROL = 157
 MAVLINK_MSG_ID_MOUNT_STATUS = 158
+MAVLINK_MSG_ID_FENCE_POINT = 160
+MAVLINK_MSG_ID_FENCE_FETCH_POINT = 161
+MAVLINK_MSG_ID_FENCE_STATUS = 162
 MAVLINK_MSG_ID_HEARTBEAT = 0
 MAVLINK_MSG_ID_BOOT = 1
 MAVLINK_MSG_ID_SYSTEM_TIME = 2
@@ -423,6 +438,54 @@ class MAVLink_mount_status_message(MAVLink_message):
         def pack(self, mav):
                 return MAVLink_message.pack(self, mav, 233, struct.pack('>BBiii', self.target_system, self.target_component, self.pointing_a, self.pointing_b, self.pointing_c))
 
+class MAVLink_fence_point_message(MAVLink_message):
+        '''
+        A fence point. Used to set a point when from               GCS
+        -> MAV. Also used to return a point from MAV -> GCS
+        '''
+        def __init__(self, target_system, target_component, idx, count, lat, lng):
+                MAVLink_message.__init__(self, MAVLINK_MSG_ID_FENCE_POINT, 'FENCE_POINT')
+                self._fieldnames = ['target_system', 'target_component', 'idx', 'count', 'lat', 'lng']
+                self.target_system = target_system
+                self.target_component = target_component
+                self.idx = idx
+                self.count = count
+                self.lat = lat
+                self.lng = lng
+
+        def pack(self, mav):
+                return MAVLink_message.pack(self, mav, 18, struct.pack('>BBBBff', self.target_system, self.target_component, self.idx, self.count, self.lat, self.lng))
+
+class MAVLink_fence_fetch_point_message(MAVLink_message):
+        '''
+        Request a current fence point from MAV
+        '''
+        def __init__(self, target_system, target_component, idx):
+                MAVLink_message.__init__(self, MAVLINK_MSG_ID_FENCE_FETCH_POINT, 'FENCE_FETCH_POINT')
+                self._fieldnames = ['target_system', 'target_component', 'idx']
+                self.target_system = target_system
+                self.target_component = target_component
+                self.idx = idx
+
+        def pack(self, mav):
+                return MAVLink_message.pack(self, mav, 68, struct.pack('>BBB', self.target_system, self.target_component, self.idx))
+
+class MAVLink_fence_status_message(MAVLink_message):
+        '''
+        Status of geo-fencing. Sent in extended             status
+        stream when fencing enabled
+        '''
+        def __init__(self, breach_status, breach_count, breach_type, breach_time):
+                MAVLink_message.__init__(self, MAVLINK_MSG_ID_FENCE_STATUS, 'FENCE_STATUS')
+                self._fieldnames = ['breach_status', 'breach_count', 'breach_type', 'breach_time']
+                self.breach_status = breach_status
+                self.breach_count = breach_count
+                self.breach_type = breach_type
+                self.breach_time = breach_time
+
+        def pack(self, mav):
+                return MAVLink_message.pack(self, mav, 136, struct.pack('>BHBI', self.breach_status, self.breach_count, self.breach_type, self.breach_time))
+
 class MAVLink_heartbeat_message(MAVLink_message):
         '''
         The heartbeat message shows that a system is present and
@@ -1718,6 +1781,9 @@ mavlink_map = {
         MAVLINK_MSG_ID_MOUNT_CONFIGURE : ( '>BBBBBB', MAVLink_mount_configure_message, [0, 1, 2, 3, 4, 5], 19 ),
         MAVLINK_MSG_ID_MOUNT_CONTROL : ( '>BBiiiB', MAVLink_mount_control_message, [0, 1, 2, 3, 4, 5], 97 ),
         MAVLINK_MSG_ID_MOUNT_STATUS : ( '>BBiii', MAVLink_mount_status_message, [0, 1, 2, 3, 4], 233 ),
+        MAVLINK_MSG_ID_FENCE_POINT : ( '>BBBBff', MAVLink_fence_point_message, [0, 1, 2, 3, 4, 5], 18 ),
+        MAVLINK_MSG_ID_FENCE_FETCH_POINT : ( '>BBB', MAVLink_fence_fetch_point_message, [0, 1, 2], 68 ),
+        MAVLINK_MSG_ID_FENCE_STATUS : ( '>BHBI', MAVLink_fence_status_message, [0, 1, 2, 3], 136 ),
         MAVLINK_MSG_ID_HEARTBEAT : ( '>BBB', MAVLink_heartbeat_message, [0, 1, 2], 72 ),
         MAVLINK_MSG_ID_BOOT : ( '>I', MAVLink_boot_message, [0], 39 ),
         MAVLINK_MSG_ID_SYSTEM_TIME : ( '>Q', MAVLink_system_time_message, [0], 190 ),
@@ -2267,6 +2333,90 @@ class MAVLink(object):
                 '''
                 return self.send(self.mount_status_encode(target_system, target_component, pointing_a, pointing_b, pointing_c))
             
+        def fence_point_encode(self, target_system, target_component, idx, count, lat, lng):
+                '''
+                A fence point. Used to set a point when from               GCS -> MAV.
+                Also used to return a point from MAV -> GCS
+
+                target_system             : System ID (uint8_t)
+                target_component          : Component ID (uint8_t)
+                idx                       : point index (first point is 1, 0 is for return point) (uint8_t)
+                count                     : total number of points (for sanity checking) (uint8_t)
+                lat                       : Latitude of point (float)
+                lng                       : Longitude of point (float)
+
+                '''
+                msg = MAVLink_fence_point_message(target_system, target_component, idx, count, lat, lng)
+                msg.pack(self)
+                return msg
+            
+        def fence_point_send(self, target_system, target_component, idx, count, lat, lng):
+                '''
+                A fence point. Used to set a point when from               GCS -> MAV.
+                Also used to return a point from MAV -> GCS
+
+                target_system             : System ID (uint8_t)
+                target_component          : Component ID (uint8_t)
+                idx                       : point index (first point is 1, 0 is for return point) (uint8_t)
+                count                     : total number of points (for sanity checking) (uint8_t)
+                lat                       : Latitude of point (float)
+                lng                       : Longitude of point (float)
+
+                '''
+                return self.send(self.fence_point_encode(target_system, target_component, idx, count, lat, lng))
+            
+        def fence_fetch_point_encode(self, target_system, target_component, idx):
+                '''
+                Request a current fence point from MAV
+
+                target_system             : System ID (uint8_t)
+                target_component          : Component ID (uint8_t)
+                idx                       : point index (first point is 1, 0 is for return point) (uint8_t)
+
+                '''
+                msg = MAVLink_fence_fetch_point_message(target_system, target_component, idx)
+                msg.pack(self)
+                return msg
+            
+        def fence_fetch_point_send(self, target_system, target_component, idx):
+                '''
+                Request a current fence point from MAV
+
+                target_system             : System ID (uint8_t)
+                target_component          : Component ID (uint8_t)
+                idx                       : point index (first point is 1, 0 is for return point) (uint8_t)
+
+                '''
+                return self.send(self.fence_fetch_point_encode(target_system, target_component, idx))
+            
+        def fence_status_encode(self, breach_status, breach_count, breach_type, breach_time):
+                '''
+                Status of geo-fencing. Sent in extended             status stream when
+                fencing enabled
+
+                breach_status             : 0 if currently inside fence, 1 if outside (uint8_t)
+                breach_count              : number of fence breaches (uint16_t)
+                breach_type               : last breach type (see FENCE_BREACH_* enum) (uint8_t)
+                breach_time               : time of last breach in milliseconds since boot (uint32_t)
+
+                '''
+                msg = MAVLink_fence_status_message(breach_status, breach_count, breach_type, breach_time)
+                msg.pack(self)
+                return msg
+            
+        def fence_status_send(self, breach_status, breach_count, breach_type, breach_time):
+                '''
+                Status of geo-fencing. Sent in extended             status stream when
+                fencing enabled
+
+                breach_status             : 0 if currently inside fence, 1 if outside (uint8_t)
+                breach_count              : number of fence breaches (uint16_t)
+                breach_type               : last breach type (see FENCE_BREACH_* enum) (uint8_t)
+                breach_time               : time of last breach in milliseconds since boot (uint32_t)
+
+                '''
+                return self.send(self.fence_status_encode(breach_status, breach_count, breach_type, breach_time))
+            
         def heartbeat_encode(self, type, autopilot, mavlink_version=2):
                 '''
                 The heartbeat message shows that a system is present and responding.
@@ -4578,3 +4728,4 @@ class MAVLink(object):
 
                 '''
                 return self.send(self.debug_encode(ind, value))
+            

Tools/autotest/pymavlink/mavutil.py

@@ -359,8 +359,6 @@ class mavlogfile(mavfile):
         self.planner_format = planner_format
         self.notimestamps = notimestamps
         self._two64 = math.pow(2.0, 63)
-        if planner_format is None and self.filename.endswith(".tlog"):
-            self.planner_format = True
         mode = 'rb'
         if self.writeable:
             if append:

Tools/autotest/pymavlink/mavwp.py

@@ -2,7 +2,12 @@
 module for loading/saving waypoints
 '''
 
-import mavlink
+import os
+
+if os.getenv('MAVLINK10'):
+    import mavlinkv10 as mavlink
+else:
+    import mavlink
 
 class MAVWPError(Exception):
         '''MAVLink WP error class'''
@@ -133,3 +138,63 @@ class MAVWPLoader(object):
                 w.param1, w.param2, w.param3, w.param4,
                 w.x, w.y, w.z, w.autocontinue))
         f.close()
+
+
+class MAVFenceError(Exception):
+        '''MAVLink fence error class'''
+        def __init__(self, msg):
+            Exception.__init__(self, msg)
+            self.message = msg
+
+class MAVFenceLoader(object):
+    '''MAVLink geo-fence loader'''
+    def __init__(self, target_system=0, target_component=0):
+        self.points = []
+        self.target_system = target_system
+        self.target_component = target_component
+
+    def count(self):
+        '''return number of points'''
+        return len(self.points)
+
+    def point(self, i):
+        '''return a point'''
+        return self.points[i]
+
+    def add(self, p):
+        '''add a point'''
+        self.points.append(p)
+
+    def clear(self):
+        '''clear point list'''
+        self.points = []
+
+    def load(self, filename):
+        '''load points from a file.
+        returns number of points loaded'''
+        f = open(filename, mode='r')
+        self.clear()
+        for line in f:
+            if line.startswith('#'):
+                continue
+            line = line.strip()
+            if not line:
+                continue
+            a = line.split()
+            if len(a) != 2:
+                raise MAVFenceError("invalid fence point line: %s" % line)
+            p = mavlink.MAVLink_fence_point_message(self.target_system, self.target_component,
+                                                    self.count(), 0, float(a[0]), float(a[1]))
+            self.add(p)
+        f.close()
+        for i in range(self.count()):
+            self.points[i].count = self.count()
+        return len(self.points)
+
+
+    def save(self, filename):
+        '''save fence points to a file'''
+        f = open(filename, mode='w')
+        for p in self.points:
+            f.write("%f\t%f\n" % (p.lat, p.lng))
+        f.close()