mirror of https://github.com/ArduPilot/ardupilot
Merge branch 'master' of https://code.google.com/p/ardupilot-mega
This commit is contained in:
commit
2f4edacf05
|
@ -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;
|
||||
|
|
|
@ -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; }
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -35,6 +35,10 @@ namespace System.IO.Ports
|
|||
Port = "5760";
|
||||
}
|
||||
|
||||
public void toggleDTR()
|
||||
{
|
||||
}
|
||||
|
||||
public string Port { get; set; }
|
||||
|
||||
public int ReadTimeout
|
||||
|
|
|
@ -35,6 +35,10 @@ namespace System.IO.Ports
|
|||
Port = "14550";
|
||||
}
|
||||
|
||||
public void toggleDTR()
|
||||
{
|
||||
}
|
||||
|
||||
public string Port { get; set; }
|
||||
|
||||
public int ReadTimeout
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -69,9 +69,7 @@ namespace ArdupilotMega
|
|||
//comPort.ReadBufferSize = 1024 * 1024;
|
||||
try
|
||||
{
|
||||
comPort.DtrEnable = false;
|
||||
System.Threading.Thread.Sleep(100);
|
||||
comPort.DtrEnable = true;
|
||||
comPort.toggleDTR();
|
||||
//comPort.Open();
|
||||
}
|
||||
catch (Exception)
|
||||
|
|
|
@ -98,10 +98,7 @@ namespace ArdupilotMega
|
|||
|
||||
BaseStream.DiscardInBuffer();
|
||||
|
||||
System.Threading.Thread.Sleep(200); // allow reset to work
|
||||
|
||||
if (BaseStream.DtrEnable)
|
||||
BaseStream.DtrEnable = false;
|
||||
BaseStream.toggleDTR();
|
||||
|
||||
// allow 2560 connect timeout on usb
|
||||
System.Threading.Thread.Sleep(1000);
|
||||
|
|
|
@ -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;
|
||||
});
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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("")]
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -16,7 +16,7 @@ def norm_heading(RAW_IMU, ATTITUDE, declination):
|
|||
zmag = RAW_IMU.zmag
|
||||
pitch = ATTITUDE.pitch
|
||||
roll = ATTITUDE.roll
|
||||
|
||||
|
||||
headX = xmag*cos(pitch) + ymag*sin(roll)*sin(pitch) + zmag*cos(roll)*sin(pitch)
|
||||
headY = ymag*cos(roll) - zmag*sin(roll)
|
||||
heading = atan2(-headY, headX)
|
||||
|
@ -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
|
||||
|
|
File diff suppressed because it is too large
Load Diff
|
@ -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:
|
||||
|
|
|
@ -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'''
|
||||
|
@ -69,7 +74,7 @@ class MAVWPLoader(object):
|
|||
)
|
||||
if not w.command in cmdmap:
|
||||
raise MAVWPError("Unknown v100 waypoint action %u" % w.command)
|
||||
|
||||
|
||||
w.command = cmdmap[w.command]
|
||||
return w
|
||||
|
||||
|
@ -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()
|
||||
|
|
Loading…
Reference in New Issue