public void setup(List<KeyValuePair<int, string>> source, string paramname, MAVLink.MAVLinkParamList paramlist)
//, string paramname2 = "", Control enabledisable = null)
{
base.SelectedIndexChanged -= MavlinkComboBox_SelectedIndexChanged;
_source2 = source;
this.DisplayMember = "Value";
this.ValueMember = "Key";
this.DataSource = source;
this.ParamName = paramname;
if (paramlist.ContainsKey(paramname))
{
this.Enabled = true;
this.Visible = true;
enableControl(true);
var item = paramlist[paramname];
this.SelectedValue = (int) paramlist[paramname].Value;
}
base.SelectedIndexChanged += new EventHandler(MavlinkComboBox_SelectedIndexChanged);
}
bool ReceviedPacket(MAVLink.MAVLinkMessage rawpacket)
{
if (rawpacket.msgid == (byte) MAVLink.MAVLINK_MSG_ID.TERRAIN_REQUEST)
{
MAVLink.mavlink_terrain_request_t packet =
rawpacket.ToStructure<MAVLink.mavlink_terrain_request_t>();
if (issending)
return false;
lastmessage = rawpacket;
lastrequest = packet;
log.Info("received TERRAIN_REQUEST " + packet.lat/1e7 + " " + packet.lon/1e7 + " space " +
packet.grid_spacing + " " + Convert.ToString((long) packet.mask, 2));
System.Threading.ThreadPool.QueueUserWorkItem(QueueSendGrid);
}
else if (rawpacket.msgid == (byte) MAVLink.MAVLINK_MSG_ID.TERRAIN_REPORT)
{
MAVLink.mavlink_terrain_report_t packet =
rawpacket.ToStructure<MAVLink.mavlink_terrain_report_t>();
log.Info("received TERRAIN_REPORT " + packet.lat/1e7 + " " + packet.lon/1e7 + " " + packet.loaded + " " +
packet.pending);
}
return false;
}
public GMapMarkerPhoto(MAVLink.mavlink_camera_feedback_t mark, bool shotBellowMinInterval = false)
: base(new PointLatLng(mark.lat/1e7, mark.lng/1e7))
{
local = mark;
this.shotBellowMinInterval = shotBellowMinInterval;
Offset = new Point(-localcache1.Width/2, -localcache1.Height/2);
Size = localcache1.Size;
Alt = mark.alt_msl;
ToolTipMode = MarkerTooltipMode.OnMouseOver;
Roll = local.roll - rolltrim;
Pitch = local.pitch - pitchtrim;
Yaw = local.yaw - yawtrim;
ToolTipText = "Photo" + "\nAlt: " + mark.alt_msl + "\nNo: "+ mark.img_idx + "\nRoll: "+Roll.ToString("0.00");
Tag = mark.time_usec;
var footprint = ImageProjection.calc(new PointLatLngAlt(Position.Lat, Position.Lng, Alt), Roll,
Pitch, Yaw, hfov, vfov);
footprintpoly = new GMapPolygon(footprint.ConvertAll(x => x.Point()), "FP"+mark.time_usec);
footprintpoly.Fill = Brushes.Transparent;
footprintpoly.Stroke = Pens.Crimson;
}
public HIL.Vector3 getOffsetFromLeader(MAVLink leader, MAVLink mav)
{
//convert Wgs84ConversionInfo to utm
CoordinateTransformationFactory ctfac = new CoordinateTransformationFactory();
GeographicCoordinateSystem wgs84 = GeographicCoordinateSystem.WGS84;
int utmzone = (int)((leader.MAV.cs.lng - -183.0) / 6.0);
IProjectedCoordinateSystem utm = ProjectedCoordinateSystem.WGS84_UTM(utmzone, leader.MAV.cs.lat < 0 ? false : true);
ICoordinateTransformation trans = ctfac.CreateFromCoordinateSystems(wgs84, utm);
double[] masterpll = { leader.MAV.cs.lng, leader.MAV.cs.lat };
// get leader utm coords
double[] masterutm = trans.MathTransform.Transform(masterpll);
double[] mavpll = { mav.MAV.cs.lng, mav.MAV.cs.lat };
//getLeader follower utm coords
double[] mavutm = trans.MathTransform.Transform(mavpll);
return new HIL.Vector3(masterutm[1] - mavutm[1], masterutm[0] - mavutm[0], 0);
}
public void UpdateIcon(MAVLink mav,float x, float y, float z, bool movable)
{
foreach (var icon in icons)
{
if (icon.interf == mav)
{
icon.Movable = movable;
if (!movable)
{
icon.x = 0;
icon.y = 0;
icon.z = 0;
icon.Color = Color.Blue;
}
else
{
icon.x = x;
icon.y = y;
icon.z = z;
icon.Color = Color.Red;
}
this.Invalidate();
return;
}
}
icons.Add(new icon() { interf = mav, y=y, z = z,x = x, Movable = movable, Name = mav.ToString() });
}
public MAVLinkSerialPort(MAVLinkInterface mavint, MAVLink.SERIAL_CONTROL_DEV port)
{
this.mavint = mavint;
this.port = port;
if (!mavint.BaseStream.IsOpen)
{
mavint.BaseStream.Open();
}
if (mavint.getHeartBeat().Length == 0)
{
throw new Exception("No valid heartbeats read from port");
}
if (subscription.Value != null)
mavint.UnSubscribeToPacketType(subscription);
subscription = mavint.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.SERIAL_CONTROL, ReceviedPacket, true);
bgdata = new Thread(mainloop);
bgdata.Name = "MAVLinkSerialPort";
bgdata.IsBackground = true;
bgdata.Start();
}
private bool receivedPacket(MAVLink.MAVLinkMessage arg)
{
if (arg.msgid == (uint)MAVLink.MAVLINK_MSG_ID.STATUSTEXT)
{
var message = ASCIIEncoding.ASCII.GetString(arg.ToStructure<MAVLink.mavlink_statustext_t>().text);
UpdateUserMessage(message);
if (message.ToLower().Contains("calibration successful") ||
message.ToLower().Contains("calibration failed"))
{
try
{
Invoke((MethodInvoker)delegate
{
BUT_calib_accell.Text = Strings.Done;
BUT_calib_accell.Enabled = false;
});
_incalibrate = false;
MainV2.comPort.UnSubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.STATUSTEXT, receivedPacket);
}
catch
{
}
}
}
return true;
}
private void BUT_connect_Click(object sender, EventArgs e)
{
Comms.CommsSerialScan.Scan(false);
DateTime deadline = DateTime.Now.AddSeconds(50);
while (Comms.CommsSerialScan.foundport == false)
{
System.Threading.Thread.Sleep(100);
if (DateTime.Now > deadline)
{
CustomMessageBox.Show("Timeout waiting for autoscan/no mavlink device connected");
return;
}
}
MAVLink com2 = new MAVLink();
com2.BaseStream.PortName = Comms.CommsSerialScan.portinterface.PortName;
com2.BaseStream.BaudRate = Comms.CommsSerialScan.portinterface.BaudRate;
com2.Open(true);
MainV2.Comports.Add(com2);
bindingSource1.ResetBindings(false);
}
public HIL.Vector3 getOffsets(MAVLink mav)
{
if (offsets.ContainsKey(mav))
{
return offsets[mav];
}
return new HIL.Vector3(offsets.Count, 0, 0);
}
public void setup(string paramname, MAVLink.MAVLinkParamList paramlist)
{
this.ParamName = paramname;
if (paramlist.ContainsKey(paramname))
{
this.Enabled = true;
list = ParameterMetaDataRepository.GetParameterBitMaskInt(ParamName,
MainV2.comPort.MAV.cs.firmware.ToString());
chkcount = list.Count;
int leftside = 9;
int top = 9;
uint value = (uint) paramlist[paramname].Value;
for (int a = 0; a < chkcount; a++)
{
CheckBox chk = new CheckBox();
chk.AutoSize = true;
chk.Text = list[a].Value.ToString();
chk.Location = new System.Drawing.Point(leftside, top);
chk.CheckedChanged -= MavlinkCheckBoxBitMask_CheckedChanged;
if ((value & (1 << list[a].Key)) > 0)
{
chk.Checked = true;
}
chklist.Add(new KeyValuePair<int, CheckBox>(list[a].Key, chk));
panel1.Controls.Add(chk);
chk.CheckedChanged += MavlinkCheckBoxBitMask_CheckedChanged;
//this.Controls.Add(new Label() { Location = chk.Location, Text = "test" });
leftside += chk.Width + 5;
if (leftside > 500)
{
top += chk.Height + 5;
leftside = 9;
}
}
this.panel1.Height = top + 25;
//this.Height = top + 25;
}
else
{
this.Enabled = false;
}
}
public GMapMarkerRallyPt(MAVLink.mavlink_rally_point_t mark)
: base(new PointLatLng(mark.lat / 1e7, mark.lng / 1e7))
{
Size = SizeSt;
Offset = new Point(-10, -40);
Alt = mark.alt;
Alt = (int)mark.alt;
ToolTipMode = MarkerTooltipMode.OnMouseOver;
ToolTipText = "Rally Point" + "\nAlt: " + mark.alt;
}
public void setup(double OnValue, double OffValue, string[] paramname, MAVLink.MAVLinkParamList paramlist,
Control enabledisable = null)
{
foreach (var s in paramname)
{
if (paramlist.ContainsKey(s))
{
setup(OnValue, OffValue, s, paramlist, enabledisable);
return;
}
}
}
private bool ReceviedPacket(MAVLink.MAVLinkMessage arg)
{
if (arg.msgid == (byte) MAVLink.MAVLINK_MSG_ID.DATA_TRANSMISSION_HANDSHAKE)
{
var packet = arg.ToStructure<MAVLink.mavlink_data_transmission_handshake_t>();
msgDataTransmissionHandshake = packet;
imageBuffer.Close();
imageBuffer = new MemoryStream((int)packet.size);
}
else if (arg.msgid == (byte)MAVLink.MAVLINK_MSG_ID.ENCAPSULATED_DATA)
{
var packet = arg.ToStructure<MAVLink.mavlink_encapsulated_data_t>();
msgEncapsulatedData = packet;
int start = msgDataTransmissionHandshake.payload * msgEncapsulatedData.seqnr;
int left = (int)msgDataTransmissionHandshake.size - start;
var writeamount = Math.Min(msgEncapsulatedData.data.Length, left);
imageBuffer.Seek(start, SeekOrigin.Begin);
imageBuffer.Write(msgEncapsulatedData.data, 0, writeamount);
// we have a complete image
if ((msgEncapsulatedData.seqnr+1) == msgDataTransmissionHandshake.packets)
{
using (
Bitmap bmp = new Bitmap(msgDataTransmissionHandshake.width, msgDataTransmissionHandshake.height,
PixelFormat.Format8bppIndexed))
{
SetGrayscalePalette(bmp);
var bitmapData = bmp.LockBits(new Rectangle(Point.Empty, bmp.Size), ImageLockMode.ReadWrite,
bmp.PixelFormat);
if (imageBuffer.Length > msgDataTransmissionHandshake.size)
return true;
var buffer = imageBuffer.ToArray();
Marshal.Copy(buffer, 0, bitmapData.Scan0, buffer.Length);
bmp.UnlockBits(bitmapData);
if (newImage != null)
newImage(this, new ImageEventHandle(bmp));
//bmp.Save("test.bmp", ImageFormat.Bmp);
}
}
}
return true;
}
public void setup(Type source, string paramname, MAVLink.MAVLinkParamList paramlist)//, string paramname2 = "", Control enabledisable = null)
{
base.SelectedIndexChanged -= MavlinkComboBox_SelectedIndexChanged;
_source = source;
this.DataSource = Enum.GetNames(source);
this.ParamName = paramname;
if (paramlist.ContainsKey(paramname))
{
this.Enabled = true;
this.Visible = true;
enableControl(true);
this.Text = Enum.GetName(source, (Int32)paramlist[paramname].Value);
}
base.SelectedIndexChanged += new EventHandler(MavlinkComboBox_SelectedIndexChanged);
}
public void setup(double OnValue, double OffValue, string paramname, MAVLink.MAVLinkParamList paramlist,
Control enabledisable = null)
{
base.CheckedChanged -= MavlinkCheckBox_CheckedChanged;
this.OnValue = OnValue;
this.OffValue = OffValue;
this.ParamName = paramname;
this._control = enabledisable;
if (paramlist.ContainsKey(paramname))
{
this.Enabled = true;
this.Visible = true;
if (paramlist[paramname].Value == OnValue)
{
this.Checked = true;
enableControl(true);
}
else if (paramlist[paramname].Value == OffValue)
{
this.Checked = false;
enableControl(false);
}
else
{
this.CheckState = System.Windows.Forms.CheckState.Indeterminate;
enableControl(false);
}
}
else
{
this.Enabled = false;
}
base.CheckedChanged += new EventHandler(MavlinkCheckBox_CheckedChanged);
}
public void AddCommand(MAVLink.MAV_CMD cmd, double p1, double p2, double p3, double p4, double x, double y, double z)
{
selectedrow = Commands.Rows.Add();
Commands.Rows[selectedrow].Cells[Command.Index].Value = cmd.ToString();
ChangeColumnHeader(cmd.ToString());
// switch wp to spline if spline checked
if (splinemode && cmd == MAVLink.MAV_CMD.WAYPOINT)
{
Commands.Rows[selectedrow].Cells[Command.Index].Value = MAVLink.MAV_CMD.SPLINE_WAYPOINT.ToString();
ChangeColumnHeader(MAVLink.MAV_CMD.SPLINE_WAYPOINT.ToString());
}
if (cmd == MAVLink.MAV_CMD.WAYPOINT)
{
setfromMap(y, x, (int)z, Math.Round(p1, 1));
}
else
{
Commands.Rows[selectedrow].Cells[Param1.Index].Value = p1;
Commands.Rows[selectedrow].Cells[Param2.Index].Value = p2;
Commands.Rows[selectedrow].Cells[Param3.Index].Value = p3;
Commands.Rows[selectedrow].Cells[Param4.Index].Value = p4;
Commands.Rows[selectedrow].Cells[Lat.Index].Value = y;
Commands.Rows[selectedrow].Cells[Lon.Index].Value = x;
Commands.Rows[selectedrow].Cells[Alt.Index].Value = z;
}
writeKML();
}
public void setOffsets(MAVLink mav, double x, double y, double z)
{
offsets[mav] = new HIL.Vector3(x,y,z);
//log.Info(mav.ToString() + " " + offsets[mav].ToString());
}
static void doread(object o)
{
run++;
running++;
MAVLink port = (MAVLink)o;
try
{
port.BaseStream.Open();
int baud = 0;
redo:
DateTime deadline = DateTime.Now.AddSeconds(5);
while (DateTime.Now < deadline)
{
Console.WriteLine("Scan port {0} at {1}", port.BaseStream.PortName, port.BaseStream.BaudRate);
byte[] packet = new byte[0];
try
{
packet = port.readPacket();
}
catch { }
if (packet.Length > 0)
{
port.BaseStream.Close();
Console.WriteLine("Found Mavlink on port {0} at {1}", port.BaseStream.PortName, port.BaseStream.BaudRate);
foundport = true;
portinterface = port.BaseStream;
if (CommsSerialScan.connect)
{
MainV2.comPort.BaseStream = port.BaseStream;
doconnect();
}
running--;
return;
}
if (foundport)
{
break;
}
}
if (!foundport && port.BaseStream.BaudRate > 0)
{
baud++;
if (baud < bauds.Length)
{
port.BaseStream.BaudRate = bauds[baud];
goto redo;
}
}
try
{
port.BaseStream.Close();
}
catch { }
}
catch (Exception ex) { Console.WriteLine(ex.ToString()); }
running--;
Console.WriteLine("Scan port {0} Finished!!", port.BaseStream.PortName);
}
internal void processToScreen()
{
toolTip1.RemoveAll();
disableNumericUpDownControls(this);
// process hashdefines and update display
foreach (string value in MainV2.comPort.MAV.param.Keys)
{
if (value == null || value == "")
{
continue;
}
//System.Diagnostics.Debug.WriteLine("Doing: " + value);
string name = value;
Control[] text = this.Controls.Find(name, true);
foreach (Control ctl in text)
{
try
{
if (ctl.GetType() == typeof(NumericUpDown))
{
float numbervalue = (float)MainV2.comPort.MAV.param[value];
MAVLink.modifyParamForDisplay(true, value, ref numbervalue);
NumericUpDown thisctl = ((NumericUpDown)ctl);
thisctl.Maximum = 9000;
thisctl.Minimum = -9000;
thisctl.Value = (decimal)numbervalue;
thisctl.Increment = (decimal)0.0001;
if (thisctl.Name.EndsWith("_P") || thisctl.Name.EndsWith("_I") || thisctl.Name.EndsWith("_D") ||
thisctl.Name.EndsWith("_LOW") || thisctl.Name.EndsWith("_HIGH") || thisctl.Value == 0 ||
thisctl.Value.ToString("0.####", new System.Globalization.CultureInfo("en-US")).Contains("."))
{
thisctl.DecimalPlaces = 4;
}
else
{
thisctl.Increment = (decimal)1;
thisctl.DecimalPlaces = 1;
}
if (thisctl.Name.ToUpper().EndsWith("THR_RATE_IMAX"))
{
thisctl.Maximum = 1000; // is a pwm
thisctl.Minimum = 0;
}
else if (thisctl.Name.EndsWith("_IMAX"))
{
thisctl.Maximum = 180;
thisctl.Minimum = -180;
}
thisctl.Enabled = true;
ThemeManager.ApplyThemeTo(thisctl);
thisctl.Validated += null;
if (tooltips[value] != null)
{
try
{
toolTip1.SetToolTip(ctl, ((paramsettings)tooltips[value]).desc);
}
catch { }
}
thisctl.Validated += new EventHandler(EEPROM_View_float_TextChanged);
}
else if (ctl.GetType() == typeof(ComboBox))
{
ComboBox thisctl = ((ComboBox)ctl);
thisctl.SelectedValue = (int)(float)MainV2.comPort.MAV.param[value];
thisctl.Validated += new EventHandler(ComboBox_Validated);
ThemeManager.ApplyThemeTo(thisctl);
}
}
catch { }
}
if (text.Length == 0)
{
//Console.WriteLine(name + " not found");
}
}
}
internal void EEPROM_View_float_TextChanged(object sender, EventArgs e)
{
if (startup == true)
{
return;
}
float value = 0;
string name = ((Control)sender).Name;
// do domainupdown state check
try
{
if (sender.GetType() == typeof(NumericUpDown))
{
value = float.Parse(((Control)sender).Text);
MAVLink.modifyParamForDisplay(false, ((Control)sender).Name, ref value);
changes[name] = value;
}
else if (sender.GetType() == typeof(ComboBox))
{
value = (int)((ComboBox)sender).SelectedValue;
changes[name] = value;
}
((Control)sender).BackColor = Color.Green;
}
catch (Exception)
{
((Control)sender).BackColor = Color.Red;
}
try
{
// enable roll and pitch pairing for ac2
if (CHK_lockrollpitch.Checked)
{
if (name.StartsWith("RATE_") || name.StartsWith("STB_") || name.StartsWith("ACRO_"))
{
if (name.Contains("_RLL_"))
{
string newname = name.Replace("_RLL_", "_PIT_");
Control[] arr = this.Controls.Find(newname, true);
changes[newname] = value;
if (arr.Length > 0)
{
arr[0].Text = ((Control)sender).Text;
arr[0].BackColor = Color.Green;
}
}
else if (name.Contains("_PIT_"))
{
string newname = name.Replace("_PIT_", "_RLL_");
Control[] arr = this.Controls.Find(newname, true);
changes[newname] = value;
if (arr.Length > 0)
{
arr[0].Text = ((Control)sender).Text;
arr[0].BackColor = Color.Green;
}
}
}
}
// keep nav_lat and nav_lon paired
if (name.Contains("NAV_LAT_"))
{
string newname = name.Replace("NAV_LAT_", "NAV_LON_");
Control[] arr = this.Controls.Find(newname, true);
changes[newname] = value;
if (arr.Length > 0)
{
arr[0].Text = ((Control)sender).Text;
arr[0].BackColor = Color.Green;
}
}
// keep loiter_lat and loiter_lon paired
if (name.Contains("LOITER_LAT_"))
{
string newname = name.Replace("LOITER_LAT_", "LOITER_LON_");
Control[] arr = this.Controls.Find(newname, true);
changes[newname] = value;
if (arr.Length > 0)
{
arr[0].Text = ((Control)sender).Text;
arr[0].BackColor = Color.Green;
}
}
// keep nav_lat and nav_lon paired
if (name.Contains("HLD_LAT_"))
{
string newname = name.Replace("HLD_LAT_", "HLD_LON_");
Control[] arr = this.Controls.Find(newname, true);
changes[newname] = value;
if (arr.Length > 0)
{
arr[0].Text = ((Control)sender).Text;
arr[0].BackColor = Color.Green;
}
}
}
catch { }
}
public void requestDatastream(MAVLink.MAV_DATA_STREAM id, byte hzrate, int sysid = -1, int compid = -1)
{
if (sysid == -1)
sysid = sysidcurrent;
if (compid == -1)
compid = compidcurrent;
double pps = 0;
switch (id)
{
case MAVLink.MAV_DATA_STREAM.ALL:
break;
case MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS:
if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.SYS_STATUS] <
DateTime.Now.AddSeconds(-2))
break;
pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.SYS_STATUS];
if (hzratecheck(pps, hzrate))
{
return;
}
break;
case MAVLink.MAV_DATA_STREAM.EXTRA1:
if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.ATTITUDE] <
DateTime.Now.AddSeconds(-2))
break;
pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.ATTITUDE];
if (hzratecheck(pps, hzrate))
{
return;
}
break;
case MAVLink.MAV_DATA_STREAM.EXTRA2:
if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.VFR_HUD] <
DateTime.Now.AddSeconds(-2))
break;
pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.VFR_HUD];
if (hzratecheck(pps, hzrate))
{
return;
}
break;
case MAVLink.MAV_DATA_STREAM.EXTRA3:
if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.AHRS] <
DateTime.Now.AddSeconds(-2))
break;
pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.AHRS];
if (hzratecheck(pps, hzrate))
{
return;
}
break;
case MAVLink.MAV_DATA_STREAM.POSITION:
if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.GLOBAL_POSITION_INT] <
DateTime.Now.AddSeconds(-2))
break;
pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.GLOBAL_POSITION_INT];
if (hzratecheck(pps, hzrate))
{
return;
}
break;
case MAVLink.MAV_DATA_STREAM.RAW_CONTROLLER:
if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.RC_CHANNELS_SCALED] <
DateTime.Now.AddSeconds(-2))
break;
pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.RC_CHANNELS_SCALED];
if (hzratecheck(pps, hzrate))
{
return;
}
break;
case MAVLink.MAV_DATA_STREAM.RAW_SENSORS:
if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.RAW_IMU] <
DateTime.Now.AddSeconds(-2))
break;
pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.RAW_IMU];
if (hzratecheck(pps, hzrate))
{
return;
}
break;
case MAVLink.MAV_DATA_STREAM.RC_CHANNELS:
if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.RC_CHANNELS_RAW] <
DateTime.Now.AddSeconds(-2))
break;
pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.RC_CHANNELS_RAW];
if (hzratecheck(pps, hzrate))
{
return;
}
break;
}
//packetspersecond[temp[5]];
if (pps == 0 && hzrate == 0)
{
return;
}
log.InfoFormat("Request stream {0} at {1} hz for {2}:{3}",
Enum.Parse(typeof (MAV_DATA_STREAM), id.ToString()), hzrate, sysid, compid);
getDatastream(id, hzrate);
}
/*
* public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow)
* {
* UpdateCurrentSettings(bs, false, MainV2.comPort);
* }
*/
public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow, MAVLink mavinterface)
{
lock (locker)
{
if (DateTime.Now > lastupdate.AddMilliseconds(50) || updatenow) // 20 hz
{
lastupdate = DateTime.Now;
//check if valid mavinterface
if (mavinterface != null && mavinterface.packetsnotlost != 0)
{
linkqualitygcs = (ushort)((mavinterface.packetsnotlost / (mavinterface.packetsnotlost + mavinterface.packetslost)) * 100.0);
}
if (DateTime.Now.Second != lastsecondcounter.Second)
{
lastsecondcounter = DateTime.Now;
if (lastpos.Lat != 0 && lastpos.Lng != 0)
{
if (!MainV2.comPort.BaseStream.IsOpen && !MainV2.comPort.logreadmode)
{
distTraveled = 0;
}
distTraveled += (float)lastpos.GetDistance(new PointLatLngAlt(lat, lng, 0, "")) * multiplierdist;
lastpos = new PointLatLngAlt(lat, lng, 0, "");
}
else
{
lastpos = new PointLatLngAlt(lat, lng, 0, "");
}
// throttle is up, or groundspeed is > 3 m/s
if (ch3percent > 12 || _groundspeed > 3.0)
{
timeInAir++;
}
if (!gotwind)
{
dowindcalc();
}
}
if (mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] != null) // status text
{
var msg = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT].ByteArrayToStructure <MAVLink.mavlink_statustext_t>(6);
/*
* enum gcs_severity {
* SEVERITY_LOW=1,
* SEVERITY_MEDIUM,
* SEVERITY_HIGH,
* SEVERITY_CRITICAL
* };
*/
byte sev = msg.severity;
string logdata = Encoding.ASCII.GetString(mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT], 6, mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT].Length - 6);
int ind = logdata.IndexOf('\0');
if (ind != -1)
{
logdata = logdata.Substring(0, ind);
}
if (sev >= 3)
{
messageHigh = logdata;
}
try
{
while (messages.Count > 50)
{
messages.RemoveAt(0);
}
messages.Add(logdata + "\n");
}
catch { }
mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] = null;
}
byte[] bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED];
if (bytearray != null) // hil mavlink 0.9
{
var hil = bytearray.ByteArrayToStructure <MAVLink.mavlink_rc_channels_scaled_t>(6);
hilch1 = hil.chan1_scaled;
hilch2 = hil.chan2_scaled;
hilch3 = hil.chan3_scaled;
hilch4 = hil.chan4_scaled;
hilch5 = hil.chan5_scaled;
hilch6 = hil.chan6_scaled;
hilch7 = hil.chan7_scaled;
hilch8 = hil.chan8_scaled;
// Console.WriteLine("MAVLINK_MSG_ID_RC_CHANNELS_SCALED Packet");
mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_FENCE_STATUS];
if (bytearray != null)
{
var fence = bytearray.ByteArrayToStructure <MAVLink.mavlink_fence_status_t>(6);
if (fence.breach_status != (byte)MAVLink.FENCE_BREACH.NONE)
{
// fence breached
}
mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_FENCE_STATUS] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_HIL_CONTROLS];
if (bytearray != null) // hil mavlink 0.9 and 1.0
{
var hil = bytearray.ByteArrayToStructure <MAVLink.mavlink_hil_controls_t>(6);
hilch1 = (int)(hil.roll_ailerons * 10000);
hilch2 = (int)(hil.pitch_elevator * 10000);
hilch3 = (int)(hil.throttle * 10000);
hilch4 = (int)(hil.yaw_rudder * 10000);
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_HIL_CONTROLS] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_HWSTATUS];
if (bytearray != null)
{
var hwstatus = bytearray.ByteArrayToStructure <MAVLink.mavlink_hwstatus_t>(6);
hwvoltage = hwstatus.Vcc / 1000.0f;
i2cerrors = hwstatus.I2Cerr;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_HWSTATUS] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_RANGEFINDER];
if (bytearray != null)
{
var sonar = bytearray.ByteArrayToStructure <MAVLink.mavlink_rangefinder_t>(6);
sonarrange = sonar.distance;
sonarvoltage = sonar.voltage;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_WIND];
if (bytearray != null)
{
var wind = bytearray.ByteArrayToStructure <MAVLink.mavlink_wind_t>(6);
gotwind = true;
wind_dir = (wind.direction + 360) % 360;
wind_vel = wind.speed * multiplierspeed;
//MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_HEARTBEAT];
if (bytearray != null)
{
var hb = bytearray.ByteArrayToStructure <MAVLink.mavlink_heartbeat_t>(6);
armed = (hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) == (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED;
failsafe = hb.system_status == (byte)MAVLink.MAV_STATE.CRITICAL;
string oldmode = mode;
mode = "Unknown";
if ((hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED) != 0)
{
List <KeyValuePair <int, string> > modelist = Common.getModesList();
foreach (KeyValuePair <int, string> pair in modelist)
{
if (pair.Key == hb.custom_mode)
{
mode = pair.Value.ToString();
}
}
}
if (oldmode != mode && MainV2.speechEnable && MainV2.getConfig("speechmodeenabled") == "True")
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode")));
}
}
bytearray = mavinterface.MAV.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS];
if (bytearray != null)
{
var sysstatus = bytearray.ByteArrayToStructure <MAVLink.mavlink_sys_status_t>(6);
battery_voltage = sysstatus.voltage_battery;
battery_remaining = sysstatus.battery_remaining;
current = sysstatus.current_battery;
packetdropremote = sysstatus.drop_rate_comm;
//MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_SCALED_PRESSURE];
if (bytearray != null)
{
var pres = bytearray.ByteArrayToStructure <MAVLink.mavlink_scaled_pressure_t>(6);
press_abs = pres.press_abs;
press_temp = pres.temperature;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_SENSOR_OFFSETS];
if (bytearray != null)
{
var sensofs = bytearray.ByteArrayToStructure <MAVLink.mavlink_sensor_offsets_t>(6);
mag_ofs_x = sensofs.mag_ofs_x;
mag_ofs_y = sensofs.mag_ofs_y;
mag_ofs_z = sensofs.mag_ofs_z;
mag_declination = sensofs.mag_declination;
raw_press = sensofs.raw_press;
raw_temp = sensofs.raw_temp;
gyro_cal_x = sensofs.gyro_cal_x;
gyro_cal_y = sensofs.gyro_cal_y;
gyro_cal_z = sensofs.gyro_cal_z;
accel_cal_x = sensofs.accel_cal_x;
accel_cal_y = sensofs.accel_cal_y;
accel_cal_z = sensofs.accel_cal_z;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE];
if (bytearray != null)
{
var att = bytearray.ByteArrayToStructure <MAVLink.mavlink_attitude_t>(6);
roll = att.roll * rad2deg;
pitch = att.pitch * rad2deg;
yaw = att.yaw * rad2deg;
// Console.WriteLine(roll + " " + pitch + " " + yaw);
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT];
if (bytearray != null)
{
var gps = bytearray.ByteArrayToStructure <MAVLink.mavlink_gps_raw_int_t>(6);
if (!useLocation)
{
lat = gps.lat * 1.0e-7f;
lng = gps.lon * 1.0e-7f;
// alt = gps.alt; // using vfr as includes baro calc
}
gpsstatus = gps.fix_type;
// Console.WriteLine("gpsfix {0}",gpsstatus);
gpshdop = (float)Math.Round((double)gps.eph / 100.0, 2);
satcount = gps.satellites_visible;
groundspeed = gps.vel * 1.0e-2f;
groundcourse = gps.cog * 1.0e-2f;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_GPS_STATUS];
if (bytearray != null)
{
var gps = bytearray.ByteArrayToStructure <MAVLink.mavlink_gps_status_t>(6);
satcount = gps.satellites_visible;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_RADIO];
if (bytearray != null)
{
var radio = bytearray.ByteArrayToStructure <MAVLink.mavlink_radio_t>(6);
rssi = radio.rssi;
remrssi = radio.remrssi;
txbuffer = radio.txbuf;
rxerrors = radio.rxerrors;
noise = radio.noise;
remnoise = radio.remnoise;
fixedp = radio.fixedp;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT];
if (bytearray != null)
{
var loc = bytearray.ByteArrayToStructure <MAVLink.mavlink_global_position_int_t>(6);
// the new arhs deadreckoning may send 0 alt and 0 long. check for and undo
useLocation = true;
if (loc.lat == 0 && loc.lon == 0)
{
useLocation = false;
}
else
{
//alt = loc.alt / 1000.0f;
lat = loc.lat / 10000000.0f;
lng = loc.lon / 10000000.0f;
}
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT];
if (bytearray != null)
{
var wpcur = bytearray.ByteArrayToStructure <MAVLink.mavlink_mission_current_t>(6);
int oldwp = (int)wpno;
wpno = wpcur.seq;
if (oldwp != wpno && MainV2.speechEnable && MainV2.getConfig("speechwaypointenabled") == "True")
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint")));
}
//MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT];
if (bytearray != null)
{
var nav = bytearray.ByteArrayToStructure <MAVLink.mavlink_nav_controller_output_t>(6);
nav_roll = nav.nav_roll;
nav_pitch = nav.nav_pitch;
nav_bearing = nav.nav_bearing;
target_bearing = nav.target_bearing;
wp_dist = nav.wp_dist;
alt_error = nav.alt_error;
aspd_error = nav.aspd_error / 100.0f;
xtrack_error = nav.xtrack_error;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW];
if (bytearray != null)
{
var rcin = bytearray.ByteArrayToStructure <MAVLink.mavlink_rc_channels_raw_t>(6);
ch1in = rcin.chan1_raw;
ch2in = rcin.chan2_raw;
ch3in = rcin.chan3_raw;
ch4in = rcin.chan4_raw;
ch5in = rcin.chan5_raw;
ch6in = rcin.chan6_raw;
ch7in = rcin.chan7_raw;
ch8in = rcin.chan8_raw;
//percent
rxrssi = (float)((rcin.rssi / 255.0) * 100.0);
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW];
if (bytearray != null)
{
var servoout = bytearray.ByteArrayToStructure <MAVLink.mavlink_servo_output_raw_t>(6);
ch1out = servoout.servo1_raw;
ch2out = servoout.servo2_raw;
ch3out = servoout.servo3_raw;
ch4out = servoout.servo4_raw;
ch5out = servoout.servo5_raw;
ch6out = servoout.servo6_raw;
ch7out = servoout.servo7_raw;
ch8out = servoout.servo8_raw;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU];
if (bytearray != null)
{
var imu = bytearray.ByteArrayToStructure <MAVLink.mavlink_raw_imu_t>(6);
gx = imu.xgyro;
gy = imu.ygyro;
gz = imu.zgyro;
ax = imu.xacc;
ay = imu.yacc;
az = imu.zacc;
mx = imu.xmag;
my = imu.ymag;
mz = imu.zmag;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_SCALED_IMU];
if (bytearray != null)
{
var imu = bytearray.ByteArrayToStructure <MAVLink.mavlink_scaled_imu_t>(6);
gx = imu.xgyro;
gy = imu.ygyro;
gz = imu.zgyro;
ax = imu.xacc;
ay = imu.yacc;
az = imu.zacc;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD];
if (bytearray != null)
{
var vfr = bytearray.ByteArrayToStructure <MAVLink.mavlink_vfr_hud_t>(6);
//groundspeed = vfr.groundspeed;
airspeed = vfr.airspeed;
alt = vfr.alt; // this might include baro
ch3percent = vfr.throttle;
//Console.WriteLine(alt);
//climbrate = vfr.climb;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_MEMINFO];
if (bytearray != null)
{
var mem = bytearray.ByteArrayToStructure <MAVLink.mavlink_meminfo_t>(6);
freemem = mem.freemem;
brklevel = mem.brkval;
}
}
//Console.Write(DateTime.Now.Millisecond + " start ");
// update form
try
{
if (bs != null)
{
//System.Diagnostics.Debug.WriteLine(DateTime.Now.Millisecond);
// Console.Write(" "+DateTime.Now.Millisecond);
//bs.DataSource = this;
// Console.Write(" " + DateTime.Now.Millisecond + " 1 " + updatenow + " " + System.Threading.Thread.CurrentThread.Name);
//bs.ResetBindings(false);
//bs.ResetCurrentItem();
// mono workaround - this is alot faster
bs.Add(this);
// Console.WriteLine(" " + DateTime.Now.Millisecond + " done ");
}
}
catch { log.InfoFormat("CurrentState Binding error"); }
}
}
/// <summary>
/// add wp to command queue - dont upload to mav
/// </summary>
/// <param name="cmd"></param>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <param name="p3"></param>
/// <param name="p4"></param>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="z"></param>
public void AddWPtoList(MAVLink.MAV_CMD cmd, double p1, double p2, double p3, double p4, double x, double y,
double z)
{
MainV2.instance.FlightPlanner.AddCommand(cmd, p1, p2, p3, p4, x, y, z);
}
public bool translateMode(string modein, ref MAVLink.mavlink_set_mode_t mode)
{
mode.target_system = MAV.sysid;
if (modein == null || modein == "")
return false;
try
{
List<KeyValuePair<int, string>> modelist = Common.getModesList(MAV.cs);
foreach (KeyValuePair<int, string> pair in modelist)
{
if (pair.Value.ToLower() == modein.ToLower())
{
mode.base_mode = (byte) MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED;
mode.custom_mode = (uint) pair.Key;
}
}
if (mode.base_mode == 0)
{
MessageBox.Show("No Mode Changed " + modein);
return false;
}
}
catch
{
System.Windows.Forms.MessageBox.Show("Failed to find Mode");
return false;
}
return true;
}
public void setLeader(MAVLink lead)
{
Leader = lead;
}
string GetItemCaption(MAVLink.mavlink_log_entry_t item)
{
return new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc).AddSeconds(item.time_utc).ToLocalTime().ToString();
}
public void setup(float Min, float Max, float Scale, float Increment, string[] paramname,
MAVLink.MAVLinkParamList paramlist, Control enabledisable = null)
{
this.ValueChanged -= MavlinkNumericUpDown_ValueChanged;
// default to first item
this.ParamName = paramname[0];
// set a new item is first item doesnt exist
foreach (var paramn in paramname)
{
if (paramlist.ContainsKey(paramn))
{
this.ParamName = paramn;
break;
}
}
// update local name
Name = ParamName;
// set min and max of both are equal
if (Min == Max)
{
double mint = Min, maxt = Max;
ParameterMetaDataRepository.GetParameterRange(ParamName, ref mint, ref maxt,
MainV2.comPort.MAV.cs.firmware.ToString());
Min = (float) mint;
Max = (float) maxt;
}
_scale = Scale;
this.Minimum = (decimal) (Min);
this.Maximum = (decimal) (Max);
this.Increment = (decimal) (Increment);
this.DecimalPlaces = BitConverter.GetBytes(decimal.GetBits((decimal) Increment)[3])[2];
this._control = enabledisable;
if (paramlist.ContainsKey(ParamName))
{
this.Enabled = true;
this.Visible = true;
enableControl(true);
decimal value = (decimal) ((float) paramlist[ParamName]/_scale);
int dec = BitConverter.GetBytes(decimal.GetBits((decimal) value)[3])[2];
if (dec > this.DecimalPlaces)
this.DecimalPlaces = dec;
if (value < this.Minimum)
this.Minimum = value;
if (value > this.Maximum)
this.Maximum = value;
base.Value = value;
}
else
{
this.Enabled = false;
enableControl(false);
}
this.ValueChanged += new EventHandler(MavlinkNumericUpDown_ValueChanged);
}
public void setup(float Min, float Max, float Scale, float Increment, string paramname,
MAVLink.MAVLinkParamList paramlist, Control enabledisable = null)
{
setup(Min, Max, Scale, Increment, new string[] {paramname}, paramlist, enabledisable);
}
public void InsertWP(int idx, MAVLink.MAV_CMD cmd, double p1, double p2, double p3, double p4, double x, double y,
double z)
{
MainV2.instance.FlightPlanner.InsertCommand(idx, cmd, p1, p2, p3, p4, x, y, z);
}
private static bool sitldetection(MAVLink.MAVLinkMessage arg)
{
issitl = true;
return true;
}
public bool doMotorTest(int motor, MAVLink.MOTOR_TEST_THROTTLE_TYPE thr_type, int throttle, int timeout)
{
return MainV2.comPort.doCommand(MAVLink.MAV_CMD.DO_MOTOR_TEST, (float) motor, (float) (byte) thr_type,
(float) throttle, (float) timeout, 0, 0, 0);
}
public static Motor[] build_motors(MAVLink.MAV_TYPE frame, int frame_orientation)
{
motors = new Motor[0];
if (frame == MAVLink.MAV_TYPE.HEXAROTOR) // y6
{
// hard coded config for supported frames
if (frame_orientation == AP_MOTORS_PLUS_FRAME)
{
// plus frame set-up
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor(AP_MOTORS_MOT_3, -120, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
add_motor(AP_MOTORS_MOT_4, 60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor(AP_MOTORS_MOT_5, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
add_motor(AP_MOTORS_MOT_6, 120, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
}
else
{
// X frame set-up
add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
add_motor(AP_MOTORS_MOT_3, -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6);
add_motor(AP_MOTORS_MOT_4, 150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
add_motor(AP_MOTORS_MOT_5, 30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor(AP_MOTORS_MOT_6, -150, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
}
}
else if (frame == MAVLink.MAV_TYPE.HEXAROTOR && false) // y6
{
if (frame_orientation >= AP_MOTORS_NEW_PLUS_FRAME)
{
// Y6 motor definition with all top motors spinning clockwise, all bottom motors counter clockwise
add_motor_raw(AP_MOTORS_MOT_1, -1.0, 0.500, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor_raw(AP_MOTORS_MOT_2, -1.0, 0.500, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor_raw(AP_MOTORS_MOT_3, 0.0, -1.000, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
add_motor_raw(AP_MOTORS_MOT_4, 0.0, -1.000, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor_raw(AP_MOTORS_MOT_5, 1.0, 0.500, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
add_motor_raw(AP_MOTORS_MOT_6, 1.0, 0.500, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
}
else
{
// original Y6 motor definition
add_motor_raw(AP_MOTORS_MOT_1, -1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor_raw(AP_MOTORS_MOT_2, 1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
add_motor_raw(AP_MOTORS_MOT_3, 1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
add_motor_raw(AP_MOTORS_MOT_4, 0.0, -1.333, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
add_motor_raw(AP_MOTORS_MOT_5, -1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor_raw(AP_MOTORS_MOT_6, 0.0, -1.333, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
}
}
else if (frame == MAVLink.MAV_TYPE.OCTOROTOR)
{
// hard coded config for supported frames
if (frame_orientation == AP_MOTORS_PLUS_FRAME)
{
// plus frame set-up
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
add_motor(AP_MOTORS_MOT_3, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
add_motor(AP_MOTORS_MOT_6, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
add_motor(AP_MOTORS_MOT_7, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
add_motor(AP_MOTORS_MOT_8, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
}
else if (frame_orientation == AP_MOTORS_V_FRAME)
{
// V frame set-up
add_motor_raw(AP_MOTORS_MOT_1, 1.0, 0.34, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
add_motor_raw(AP_MOTORS_MOT_2, -1.0, -0.32, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
add_motor_raw(AP_MOTORS_MOT_3, 1.0, -0.32, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
add_motor_raw(AP_MOTORS_MOT_4, -0.5, -1.0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor_raw(AP_MOTORS_MOT_5, 1.0, 1.0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
add_motor_raw(AP_MOTORS_MOT_6, -1.0, 0.34, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor_raw(AP_MOTORS_MOT_7, -1.0, 1.0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor_raw(AP_MOTORS_MOT_8, 0.5, -1.0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
}
else
{
// X frame set-up
add_motor(AP_MOTORS_MOT_1, 22.5, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor(AP_MOTORS_MOT_2, -157.5, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
add_motor(AP_MOTORS_MOT_3, 67.5, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor(AP_MOTORS_MOT_4, 157.5, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor(AP_MOTORS_MOT_5, -22.5, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
add_motor(AP_MOTORS_MOT_6, -112.5, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
add_motor(AP_MOTORS_MOT_7, -67.5, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
add_motor(AP_MOTORS_MOT_8, 112.5, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
}
}
else if (frame == MAVLink.MAV_TYPE.OCTOROTOR && false) // octaquad
{
// hard coded config for supported frames
if (frame_orientation == AP_MOTORS_PLUS_FRAME)
{
// plus frame set-up
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
add_motor(AP_MOTORS_MOT_3, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
add_motor(AP_MOTORS_MOT_4, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
add_motor(AP_MOTORS_MOT_5, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
add_motor(AP_MOTORS_MOT_6, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
add_motor(AP_MOTORS_MOT_7, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor(AP_MOTORS_MOT_8, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6);
}
else if (frame_orientation == AP_MOTORS_V_FRAME)
{
// V frame set-up
add_motor(AP_MOTORS_MOT_1, 45, 0.7981, 1);
add_motor(AP_MOTORS_MOT_2, -45, -0.7981, 7);
add_motor(AP_MOTORS_MOT_3, -135, 1.0000, 5);
add_motor(AP_MOTORS_MOT_4, 135, -1.0000, 3);
add_motor(AP_MOTORS_MOT_5, -45, 0.7981, 8);
add_motor(AP_MOTORS_MOT_6, 45, -0.7981, 2);
add_motor(AP_MOTORS_MOT_7, 135, 1.0000, 4);
add_motor(AP_MOTORS_MOT_8, -135, -1.0000, 6);
}
else if (frame_orientation == AP_MOTORS_H_FRAME)
{
// H frame set-up - same as X but motors spin in opposite directions
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor(AP_MOTORS_MOT_2, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7);
add_motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8);
add_motor(AP_MOTORS_MOT_6, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor(AP_MOTORS_MOT_7, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
add_motor(AP_MOTORS_MOT_8, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
}
else
{
// X frame set-up
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor(AP_MOTORS_MOT_2, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
add_motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
add_motor(AP_MOTORS_MOT_6, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
add_motor(AP_MOTORS_MOT_7, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor(AP_MOTORS_MOT_8, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6);
}
}
else if (frame == MAVLink.MAV_TYPE.QUADROTOR)
{
// hard coded config for supported frames
if (frame_orientation == AP_MOTORS_PLUS_FRAME)
{
// plus frame set-up
add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor(AP_MOTORS_MOT_3, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
}
else if (frame_orientation == AP_MOTORS_V_FRAME)
{
// V frame set-up
add_motor(AP_MOTORS_MOT_1, 45, 0.7981, 1);
add_motor(AP_MOTORS_MOT_2, -135, 1.0000, 3);
add_motor(AP_MOTORS_MOT_3, -45, -0.7981, 4);
add_motor(AP_MOTORS_MOT_4, 135, -1.0000, 2);
}
else if (frame_orientation == AP_MOTORS_H_FRAME)
{
// H frame set-up - same as X but motors spin in opposite directiSons
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
add_motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
}
else if (frame_orientation == AP_MOTORS_VTAIL_FRAME)
{
/* Lynxmotion Hunter Vtail 400/500
Roll control comes only from the front motors, Yaw control only from the rear motors
roll factor is measured by the angle perpendicular to that of the prop arm to the roll axis (x)
pitch factor is measured by the angle perpendicular to the prop arm to the pitch axis (y)
assumptions:
20 20
\ / 3_____________1
\ / |
\ / |
40 \/ 40 20 | 20
Tail / \
2 4
All angles measured from their closest axis
Note: if we want the front motors to help with yaw,
motors 1's yaw factor should be changed to sin(radians(40)). Where "40" is the vtail angle
motors 3's yaw factor should be changed to -sin(radians(40))
*/
// front right: 70 degrees right of roll axis, 20 degrees up of pitch axis, no yaw
add_motor_raw(AP_MOTORS_MOT_1, cosf(radians(160)), cosf(radians(-70)), 0, 1);
// back left: no roll, 70 degrees down of pitch axis, full yaw
add_motor_raw(AP_MOTORS_MOT_2, 0, cosf(radians(160)), AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
// front left: 70 degrees left of roll axis, 20 degrees up of pitch axis, no yaw
add_motor_raw(AP_MOTORS_MOT_3, cosf(radians(20)), cosf(radians(70)), 0, 4);
// back right: no roll, 70 degrees down of pitch axis, full yaw
add_motor_raw(AP_MOTORS_MOT_4, 0, cosf(radians(-160)), AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
}
else
{
// X frame set-up
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
add_motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
}
}
else if (frame == MAVLink.MAV_TYPE.TRICOPTER)
{
if (frame_orientation >= AP_MOTORS_NEW_PLUS_FRAME)
{
// Y6 motor definition with all top motors spinning clockwise, all bottom motors counter clockwise
add_motor_raw(AP_MOTORS_MOT_1, -1.0, 0.500, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor_raw(AP_MOTORS_MOT_2, -1.0, 0.500, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor_raw(AP_MOTORS_MOT_3, 0.0, -1.000, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
add_motor_raw(AP_MOTORS_MOT_4, 0.0, -1.000, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor_raw(AP_MOTORS_MOT_5, 1.0, 0.500, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
add_motor_raw(AP_MOTORS_MOT_6, 1.0, 0.500, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
}
else
{
// original Y6 motor definition
add_motor_raw(AP_MOTORS_MOT_1, -1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor_raw(AP_MOTORS_MOT_2, 1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
add_motor_raw(AP_MOTORS_MOT_3, 1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
add_motor_raw(AP_MOTORS_MOT_4, 0.0, -1.333, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
add_motor_raw(AP_MOTORS_MOT_5, -1.0, 0.666, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor_raw(AP_MOTORS_MOT_6, 0.0, -1.333, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
}
}
return motors;
}
void getDatastream(MAVLink.MAV_DATA_STREAM id, byte hzrate)
{
mavlink_request_data_stream_t req = new mavlink_request_data_stream_t();
req.target_system = MAV.sysid;
req.target_component = MAV.compid;
req.req_message_rate = hzrate;
req.start_stop = 1; // start
req.req_stream_id = (byte) id; // id
// send each one twice.
generatePacket((byte) MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req);
generatePacket((byte) MAVLINK_MSG_ID.REQUEST_DATA_STREAM, req);
}
public void setOffsets(MAVLink mav, double x, double y, double z)
{
offsets[mav] = new HIL.Vector3(x, y, z);
//log.Info(mav.ToString() + " " + offsets[mav].ToString());
}
private void grid1_UpdateOffsets(MAVLink mav, float x, float y, float z, Grid.icon ico)
{
if (mav == SwarmInterface.Leader)
{
CustomMessageBox.Show("Can not move Leader");
ico.z = 0;
}
else
{
((Formation)SwarmInterface).setOffsets(mav, x, y, z);
}
}
List <string[]> readLog(string fn)
{
List <string[]> list = new List <string[]>();
if (fn.ToLower().EndsWith("tlog"))
{
MAVLink mine = new MAVLink();
mine.logplaybackfile = new BinaryReader(File.Open(fn, FileMode.Open, FileAccess.Read, FileShare.Read));
mine.logreadmode = true;
mine.packets.Initialize(); // clear
CurrentState cs = new CurrentState();
string[] oldvalues = { "" };
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
byte[] packet = mine.readPacket();
cs.datetime = mine.lastlogread;
cs.UpdateCurrentSettings(null, true, mine);
// line "GPS: 82686250, 1, 8, -34.1406480, 118.5441900, 0.0000, 309.1900, 315.9500, 0.0000, 279.1200" string
string[] vals = new string[] { "GPS", (cs.datetime - new DateTime(cs.datetime.Year, cs.datetime.Month, cs.datetime.Day, 0, 0, 0, DateTimeKind.Local)).TotalMilliseconds.ToString(), "1",
cs.satcount.ToString(), cs.lat.ToString(), cs.lng.ToString(), "0.0", cs.alt.ToString(), cs.alt.ToString(), "0.0", cs.groundcourse.ToString() };
if (oldvalues.Length > 2 && oldvalues[latpos] == vals[latpos] &&
oldvalues[lngpos] == vals[lngpos] &&
oldvalues[altpos] == vals[altpos])
{
continue;
}
oldvalues = vals;
list.Add(vals);
// 4 5 7
Console.Write((mine.logplaybackfile.BaseStream.Position * 100 / mine.logplaybackfile.BaseStream.Length) + " \r");
}
mine.logplaybackfile.Close();
return(list);
}
StreamReader sr = new StreamReader(fn);
string lasttime = "0";
while (!sr.EndOfStream)
{
string line = sr.ReadLine();
if (line.ToLower().StartsWith("gps"))
{
string[] vals = line.Split(new char[] { ',', ':' });
if (lasttime == vals[1])
{
continue;
}
lasttime = vals[1];
list.Add(vals);
}
}
sr.Close();
sr.Dispose();
return(list);
}
