public HIL.Vector3 getOffsetFromLeader(MAVLinkInterface leader, MAVLinkInterface mav)
{
//convert Wgs84ConversionInfo to utm
CoordinateTransformationFactory ctfac = new CoordinateTransformationFactory();
GeographicCoordinateSystem wgs84 = GeographicCoordinateSystem.WGS84;
int utmzone = (int)((leader.MAV.cs.lng - -186.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(MAVLinkInterface 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;
}
}
Console.WriteLine("ADD MAV {0} {1} {2}",x,y,z);
icons.Add(new icon() { interf = mav, y=y, z = z,x = x, Movable = movable, Name = mav.ToString() });
}
public MAVState(MAVLinkInterface mavLinkInterface)
{
this.parent = mavLinkInterface;
this.packetspersecond = new double[0x100];
this.packetspersecondbuild = new DateTime[0x100];
this.lastvalidpacket = DateTime.MinValue;
this.sysid = 0;
this.compid = 0;
sendlinkid = (byte)(new Random().Next(256));
signing = false;
this.param = new MAVLinkParamList();
this.packets = new Dictionary<uint, MAVLinkMessage>();
this.aptype = 0;
this.apname = 0;
this.recvpacketcount = 0;
this.VersionString = "";
this.SoftwareVersions = "";
this.SerialString = "";
this.FrameString = "";
camerapoints.Clear();
GMapMarkerOverlapCount.Clear();
this.packetslost = 0f;
this.packetsnotlost = 0f;
this.packetlosttimer = DateTime.MinValue;
cs.parent = this;
}
public ConfigPanel(string XMLFile, MAVLinkInterface mavinterface)
{
_mavinterface = mavinterface;
InitializeComponent();
LoadXML(XMLFile);
}
public HIL.Vector3 getOffsets(MAVLinkInterface mav)
{
if (offsets.ContainsKey(mav))
{
return offsets[mav];
}
return new HIL.Vector3(offsets.Count, 0, 0);
}
public static string tlogToCSV(string filepath)
{
CurrentState.SpeedUnit = "m/s";
CurrentState.DistanceUnit = "m";
MAVLinkInterface proto = new MAVLinkInterface();
OpenFileDialog openFileDialog1 = new OpenFileDialog();
string LogFilePath;
openFileDialog1.FileName = filepath;
foreach (string logfile in openFileDialog1.FileNames)
{
using (MAVLinkInterface mine = new MAVLinkInterface())
{
try
{
mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read));
}
catch (Exception ex) { log.Debug(ex.ToString()); }
mine.logreadmode = true;
mine.MAV.packets.Initialize(); // clear
StreamWriter sw = new StreamWriter(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + Path.GetFileNameWithoutExtension(logfile) + ".csv");
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
byte[] packet = mine.readPacket();
string text = "";
mine.DebugPacket(packet, ref text, true, ",");
sw.Write(mine.lastlogread.ToString("yyyy-MM-ddTHH:mm:ss.fff") + "," + text);
}
sw.Close();
mine.logreadmode = false;
mine.logplaybackfile.Close();
mine.logplaybackfile = null;
LogFilePath = (Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar + (Path.GetFileNameWithoutExtension(logfile) + ".csv"));
return LogFilePath;
}
}
return null;
}
private void BUT_connect_Click(object sender, EventArgs e)
{
var mav = new MAVLinkInterface();
try
{
MainV2.instance.doConnect(mav, CMB_serialport.Text, CMB_baudrate.Text);
MainV2.Comports.Add(mav);
}
catch (Exception ex)
{
}
}
public static string magfit(string logfile)
{
//'''find best magnetometer rotation fit to a log file'''
// print("Processing log %s" % filename);
// mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps);
MAVLinkInterface mavint = new MAVLinkInterface();
try
{
mavint.BaseStream = new Comms.CommsFile();
mavint.BaseStream.PortName = logfile;
mavint.BaseStream.Open();
}
catch (Exception ex) { return ""; }
mavint.logreadmode = true;
return process(mavint);
}
public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow,
MAVLinkInterface mavinterface, MAVState MAV)
{
lock (this)
{
if (DateTime.Now > lastupdate.AddMilliseconds(50) || updatenow) // 20 hz
{
lastupdate = DateTime.Now;
//check if valid mavinterface
if (parent != null && parent.packetsnotlost != 0)
{
if ((DateTime.Now - MAV.lastvalidpacket).TotalSeconds > 10)
{
linkqualitygcs = 0;
}
else
{
linkqualitygcs =
(ushort) ((parent.packetsnotlost/(parent.packetsnotlost + parent.packetslost))*100.0);
}
if (linkqualitygcs > 100)
linkqualitygcs = 100;
}
if (datetime.Second != lastsecondcounter.Second)
{
lastsecondcounter = datetime;
if (lastpos.Lat != 0 && lastpos.Lng != 0 && armed)
{
if (!mavinterface.BaseStream.IsOpen && !mavinterface.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();
}
// re-request streams
if (!(lastdata.AddSeconds(8) > DateTime.Now) && mavinterface.BaseStream.IsOpen)
{
try
{
mavinterface.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS, MAV.cs.ratestatus,
MAV.sysid); // mode
mavinterface.requestDatastream(MAVLink.MAV_DATA_STREAM.POSITION, MAV.cs.rateposition,
MAV.sysid); // request gps
mavinterface.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTRA1, MAV.cs.rateattitude,
MAV.sysid); // request attitude
mavinterface.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTRA2, MAV.cs.rateattitude,
MAV.sysid); // request vfr
mavinterface.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTRA3, MAV.cs.ratesensors, MAV.sysid);
// request extra stuff - tridge
mavinterface.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MAV.cs.ratesensors,
MAV.sysid); // request raw sensor
mavinterface.requestDatastream(MAVLink.MAV_DATA_STREAM.RC_CHANNELS, MAV.cs.raterc, MAV.sysid);
// request rc info
}
catch
{
log.Error("Failed to request rates");
}
lastdata = DateTime.Now.AddSeconds(30); // prevent flooding
}
byte[] bytearray = MAV.packets[(byte) 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("RC_CHANNELS_SCALED Packet");
MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.RC_CHANNELS_SCALED] = null;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.AUTOPILOT_VERSION];
if (bytearray != null)
{
var version = bytearray.ByteArrayToStructure<MAVLink.mavlink_autopilot_version_t>(6);
//#define FIRMWARE_VERSION 3,4,0,FIRMWARE_VERSION_TYPE_DEV
// flight_sw_version 0x03040000 uint
byte main = (byte) (version.flight_sw_version >> 24);
byte sub = (byte) ((version.flight_sw_version >> 16) & 0xff);
byte rev = (byte) ((version.flight_sw_version >> 8) & 0xff);
MAVLink.FIRMWARE_VERSION_TYPE type =
(MAVLink.FIRMWARE_VERSION_TYPE) (version.flight_sw_version & 0xff);
this.version = new Version(main, sub, (int) type, rev);
MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.AUTOPILOT_VERSION] = null;
}
bytearray = MAV.packets[(byte) 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
messageHigh = "Fence Breach";
messageHighTime = DateTime.Now;
}
MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.FENCE_STATUS] = null;
}
bytearray = MAV.packets[(byte) 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[(byte)MAVLink.MSG_NAMES.HIL_CONTROLS] = null;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.OPTICAL_FLOW];
if (bytearray != null)
{
var optflow = bytearray.ByteArrayToStructure<MAVLink.mavlink_optical_flow_t>(6);
opt_m_x = optflow.flow_comp_m_x;
opt_m_y = optflow.flow_comp_m_y;
opt_x = optflow.flow_x;
opt_y = optflow.flow_y;
opt_qua = optflow.quality;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.MOUNT_STATUS];
if (bytearray != null)
{
var status = bytearray.ByteArrayToStructure<MAVLink.mavlink_mount_status_t>(6);
campointa = status.pointing_a/100.0f;
campointb = status.pointing_b/100.0f;
campointc = status.pointing_c/100.0f;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.VIBRATION];
if (bytearray != null)
{
var vibe = bytearray.ByteArrayToStructure<MAVLink.mavlink_vibration_t>(6);
vibeclip0 = vibe.clipping_0;
vibeclip1 = vibe.clipping_1;
vibeclip2 = vibe.clipping_2;
vibex = vibe.vibration_x;
vibey = vibe.vibration_y;
vibez = vibe.vibration_z;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.AIRSPEED_AUTOCAL];
if (bytearray != null)
{
var asac = bytearray.ByteArrayToStructure<MAVLink.mavlink_airspeed_autocal_t>(6);
asratio = asac.ratio;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.SYSTEM_TIME];
if (bytearray != null)
{
var systime = bytearray.ByteArrayToStructure<MAVLink.mavlink_system_time_t>(6);
DateTime date1 = new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc);
try
{
date1 = date1.AddMilliseconds(systime.time_unix_usec/1000);
gpstime = date1;
}
catch
{
}
}
bytearray = MAV.packets[(byte) 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[(byte)MAVLink.MSG_NAMES.HWSTATUS] = null;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.EKF_STATUS_REPORT];
if (bytearray != null)
{
var ekfstatusm = bytearray.ByteArrayToStructure<MAVLink.mavlink_ekf_status_report_t>(6);
// > 1, between 0-1 typical > 1 = reject measurement - red
// 0.5 > amber
ekfvelv = ekfstatusm.velocity_variance;
ekfcompv = ekfstatusm.compass_variance;
ekfposhor = ekfstatusm.pos_horiz_variance;
ekfposvert = ekfstatusm.pos_vert_variance;
ekfteralt = ekfstatusm.terrain_alt_variance;
ekfflags = ekfstatusm.flags;
ekfstatus =
(float)
Math.Max(ekfvelv,
Math.Max(ekfcompv, Math.Max(ekfposhor, Math.Max(ekfposvert, ekfteralt))));
if (ekfvelv >= 1)
{
messageHigh = Strings.ERROR + " " + Strings.velocity_variance;
messageHighTime = DateTime.Now;
}
if (ekfcompv >= 1)
{
messageHigh = Strings.ERROR + " " + Strings.compass_variance;
messageHighTime = DateTime.Now;
}
if (ekfposhor >= 1)
{
messageHigh = Strings.ERROR + " " + Strings.pos_horiz_variance;
messageHighTime = DateTime.Now;
}
if (ekfposvert >= 1)
{
messageHigh = Strings.ERROR + " " + Strings.pos_vert_variance;
messageHighTime = DateTime.Now;
}
if (ekfteralt >= 1)
{
messageHigh = Strings.ERROR + " " + Strings.terrain_alt_variance;
messageHighTime = DateTime.Now;
}
for (int a = 1; a < (int) MAVLink.EKF_STATUS_FLAGS.ENUM_END; a = a << 1)
{
int currentbit = (ekfstatusm.flags & a);
if (currentbit == 0)
{
var currentflag =
(MAVLink.EKF_STATUS_FLAGS)
Enum.Parse(typeof (MAVLink.EKF_STATUS_FLAGS), a.ToString());
switch (currentflag)
{
case MAVLink.EKF_STATUS_FLAGS.EKF_ATTITUDE: // step 1
case MAVLink.EKF_STATUS_FLAGS.EKF_VELOCITY_HORIZ: // with pos
case MAVLink.EKF_STATUS_FLAGS.EKF_VELOCITY_VERT: // with pos
//case MAVLink.EKF_STATUS_FLAGS.EKF_POS_HORIZ_REL: // optical flow
case MAVLink.EKF_STATUS_FLAGS.EKF_POS_HORIZ_ABS: // step 1
case MAVLink.EKF_STATUS_FLAGS.EKF_POS_VERT_ABS: // step 1
//case MAVLink.EKF_STATUS_FLAGS.EKF_POS_VERT_AGL: // range finder
//case MAVLink.EKF_STATUS_FLAGS.EKF_CONST_POS_MODE: // never true when absolute - non gps
//case MAVLink.EKF_STATUS_FLAGS.EKF_PRED_POS_HORIZ_REL: // optical flow
case MAVLink.EKF_STATUS_FLAGS.EKF_PRED_POS_HORIZ_ABS: // ekf has origin - post arm
//messageHigh = Strings.ERROR + " " + currentflag.ToString().Replace("_", " ");
//messageHighTime = DateTime.Now;
break;
default:
break;
}
}
}
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.RANGEFINDER];
if (bytearray != null)
{
var sonar = bytearray.ByteArrayToStructure<MAVLink.mavlink_rangefinder_t>(6);
sonarrange = sonar.distance;
sonarvoltage = sonar.voltage;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.POWER_STATUS];
if (bytearray != null)
{
var power = bytearray.ByteArrayToStructure<MAVLink.mavlink_power_status_t>(6);
boardvoltage = power.Vcc;
servovoltage = power.Vservo;
voltageflag = (MAVLink.MAV_POWER_STATUS) power.flags;
}
bytearray = MAV.packets[(byte) 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;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.HEARTBEAT];
if (bytearray != null)
{
var hb = bytearray.ByteArrayToStructure<MAVLink.mavlink_heartbeat_t>(6);
if (hb.type == (byte) MAVLink.MAV_TYPE.GCS)
{
// skip gcs hb's
// only happens on log playback - and shouldnt get them here
}
else
{
armed = (hb.base_mode & (byte) MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) ==
(byte) MAVLink.MAV_MODE_FLAG.SAFETY_ARMED;
// for future use
landed = hb.system_status == (byte) MAVLink.MAV_STATE.STANDBY;
failsafe = hb.system_status == (byte) MAVLink.MAV_STATE.CRITICAL;
string oldmode = mode;
if ((hb.base_mode & (byte) MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED) != 0)
{
// prevent running thsi unless we have to
if (_mode != hb.custom_mode)
{
List<KeyValuePair<int, string>> modelist = Common.getModesList(this);
bool found = false;
foreach (KeyValuePair<int, string> pair in modelist)
{
if (pair.Key == hb.custom_mode)
{
mode = pair.Value.ToString();
_mode = hb.custom_mode;
found = true;
break;
}
}
if (!found)
{
log.Warn("Mode not found bm:" + hb.base_mode + " cm:" + hb.custom_mode);
_mode = hb.custom_mode;
}
}
}
if (oldmode != mode && MainV2.speechEnable && MainV2.comPort.MAV.cs == this &&
MainV2.getConfig("speechmodeenabled") == "True")
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode")));
}
}
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.SYS_STATUS];
if (bytearray != null)
{
var sysstatus = bytearray.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6);
load = (float) sysstatus.load/10.0f;
battery_voltage = (float) sysstatus.voltage_battery/1000.0f;
battery_remaining = sysstatus.battery_remaining;
current = (float) sysstatus.current_battery/100.0f;
packetdropremote = sysstatus.drop_rate_comm;
Mavlink_Sensors sensors_enabled = new Mavlink_Sensors(sysstatus.onboard_control_sensors_enabled);
Mavlink_Sensors sensors_health = new Mavlink_Sensors(sysstatus.onboard_control_sensors_health);
Mavlink_Sensors sensors_present = new Mavlink_Sensors(sysstatus.onboard_control_sensors_present);
terrainactive = sensors_health.terrain && sensors_enabled.terrain && sensors_present.terrain;
if (sensors_health.gps != sensors_enabled.gps && sensors_present.gps)
{
messageHigh = Strings.BadGPSHealth;
messageHighTime = DateTime.Now;
}
else if (sensors_health.gyro != sensors_enabled.gyro && sensors_present.gyro)
{
messageHigh = Strings.BadGyroHealth;
messageHighTime = DateTime.Now;
}
else if (sensors_health.accelerometer != sensors_enabled.accelerometer &&
sensors_present.accelerometer)
{
messageHigh = Strings.BadAccelHealth;
messageHighTime = DateTime.Now;
}
else if (sensors_health.compass != sensors_enabled.compass && sensors_present.compass)
{
messageHigh = Strings.BadCompassHealth;
messageHighTime = DateTime.Now;
}
else if (sensors_health.barometer != sensors_enabled.barometer && sensors_present.barometer)
{
messageHigh = Strings.BadBaroHealth;
messageHighTime = DateTime.Now;
}
else if (sensors_health.LASER_POSITION != sensors_enabled.LASER_POSITION &&
sensors_present.LASER_POSITION)
{
messageHigh = Strings.BadLiDARHealth;
messageHighTime = DateTime.Now;
}
else if (sensors_health.optical_flow != sensors_enabled.optical_flow &&
sensors_present.optical_flow)
{
messageHigh = Strings.BadOptFlowHealth;
messageHighTime = DateTime.Now;
}
else if (sensors_health.terrain != sensors_enabled.terrain && sensors_present.terrain)
{
messageHigh = Strings.BadorNoTerrainData;
messageHighTime = DateTime.Now;
}
else if (sensors_health.geofence != sensors_enabled.geofence &&
sensors_present.geofence)
{
messageHigh = Strings.GeofenceBreach;
messageHighTime = DateTime.Now;
}
else if (sensors_health.ahrs != sensors_enabled.ahrs && sensors_present.ahrs)
{
messageHigh = Strings.BadAHRS;
messageHighTime = DateTime.Now;
}
else if (sensors_health.rc_receiver != sensors_enabled.rc_receiver &&
sensors_present.rc_receiver)
{
messageHigh = Strings.NORCReceiver;
messageHighTime = DateTime.Now;
}
MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.SYS_STATUS] = null;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.BATTERY2];
if (bytearray != null)
{
var bat = bytearray.ByteArrayToStructure<MAVLink.mavlink_battery2_t>(6);
_battery_voltage2 = bat.voltage;
current2 = bat.current_battery;
}
bytearray = MAV.packets[(byte) 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 = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.TERRAIN_REPORT];
if (bytearray != null)
{
var terrainrep = bytearray.ByteArrayToStructure<MAVLink.mavlink_terrain_report_t>(6);
ter_curalt = terrainrep.current_height;
ter_alt = terrainrep.terrain_height;
ter_load = terrainrep.loaded;
ter_pend = terrainrep.pending;
ter_space = terrainrep.spacing;
}
bytearray = MAV.packets[(byte) 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 = MAV.packets[(byte) 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(MAV.sysid + " " +roll + " " + pitch + " " + yaw);
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.ATTITUDE] = null;
}
bytearray = MAV.packets[(byte) 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
alt = loc.relative_alt/1000.0f;
useLocation = true;
if (loc.lat == 0 && loc.lon == 0)
{
useLocation = false;
}
else
{
lat = loc.lat/10000000.0;
lng = loc.lon/10000000.0;
altasl = loc.alt/1000.0f;
}
}
bytearray = MAV.packets[(byte) 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-7;
lng = gps.lon*1.0e-7;
altasl = gps.alt/1000.0f;
// 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[(byte)MAVLink.MSG_NAMES.GPS_RAW] = null;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.GPS2_RAW];
if (bytearray != null)
{
var gps = bytearray.ByteArrayToStructure<MAVLink.mavlink_gps2_raw_t>(6);
lat2 = gps.lat*1.0e-7;
lng2 = gps.lon*1.0e-7;
altasl2 = gps.alt/1000.0f;
gpsstatus2 = gps.fix_type;
gpshdop2 = (float) Math.Round((double) gps.eph/100.0, 2);
satcount2 = gps.satellites_visible;
groundspeed2 = gps.vel*1.0e-2f;
groundcourse2 = gps.cog*1.0e-2f;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.GPS_STATUS];
if (bytearray != null)
{
var gps = bytearray.ByteArrayToStructure<MAVLink.mavlink_gps_status_t>(6);
satcount = gps.satellites_visible;
}
bytearray = MAV.packets[(byte) 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.@fixed;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.RADIO_STATUS];
if (bytearray != null)
{
var radio = bytearray.ByteArrayToStructure<MAVLink.mavlink_radio_status_t>(6);
rssi = radio.rssi;
remrssi = radio.remrssi;
txbuffer = radio.txbuf;
rxerrors = radio.rxerrors;
noise = radio.noise;
remnoise = radio.remnoise;
fixedp = radio.@fixed;
}
bytearray = MAV.packets[(byte) 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 (mode.ToLower() == "auto" && wpno != 0)
{
lastautowp = (int) wpno;
}
if (oldwp != wpno && MainV2.speechEnable && MainV2.comPort.MAV.cs == this &&
MainV2.getConfig("speechwaypointenabled") == "True")
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint")));
}
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.WAYPOINT_CURRENT] = null;
}
bytearray = MAV.packets[(byte) 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[(byte)MAVLink.MSG_NAMES.NAV_CONTROLLER_OUTPUT] = null;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.RPM];
if (bytearray != null)
{
var rpm = bytearray.ByteArrayToStructure<MAVLink.mavlink_rpm_t>(6);
rpm1 = rpm.rpm1;
rpm2 = rpm.rpm2;
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.NAV_CONTROLLER_OUTPUT] = null;
}
bytearray = MAV.packets[(byte) 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 = (int) ((rcin.rssi/255.0)*100.0);
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.RC_CHANNELS_RAW] = null;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.RC_CHANNELS];
if (bytearray != null)
{
var rcin = bytearray.ByteArrayToStructure<MAVLink.mavlink_rc_channels_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;
ch9in = rcin.chan9_raw;
ch10in = rcin.chan10_raw;
ch11in = rcin.chan11_raw;
ch12in = rcin.chan12_raw;
ch13in = rcin.chan13_raw;
ch14in = rcin.chan14_raw;
ch15in = rcin.chan15_raw;
ch16in = rcin.chan16_raw;
//percent
rxrssi = (int) ((rcin.rssi/255.0)*100.0);
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.RC_CHANNELS_RAW] = null;
}
bytearray = MAV.packets[(byte) 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;
MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.SERVO_OUTPUT_RAW] = null;
}
bytearray = MAV.packets[(byte) 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;
var timesec = imu.time_usec*1.0e-6;
var deltawall = (DateTime.Now - lastimutime).TotalSeconds;
var deltaimu = timesec - imutime;
//Console.WriteLine( + " " + deltawall + " " + deltaimu + " " + System.Threading.Thread.CurrentThread.Name);
if (speedup > 0)
speedup = (float) (speedup*0.95 + (deltaimu/deltawall)*0.05);
imutime = timesec;
lastimutime = DateTime.Now;
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.RAW_IMU] = null;
}
bytearray = MAV.packets[(byte) 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;
mx = imu.xmag;
my = imu.ymag;
mz = imu.zmag;
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.RAW_IMU] = null;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.SCALED_IMU2];
if (bytearray != null)
{
var imu2 = bytearray.ByteArrayToStructure<MAVLink.mavlink_scaled_imu2_t>(6);
gx2 = imu2.xgyro;
gy2 = imu2.ygyro;
gz2 = imu2.zgyro;
ax2 = imu2.xacc;
ay2 = imu2.yacc;
az2 = imu2.zacc;
mx2 = imu2.xmag;
my2 = imu2.ymag;
mz2 = imu2.zmag;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.SCALED_IMU3];
if (bytearray != null)
{
var imu3 = bytearray.ByteArrayToStructure<MAVLink.mavlink_scaled_imu3_t>(6);
gx3 = imu3.xgyro;
gy3 = imu3.ygyro;
gz3 = imu3.zgyro;
ax3 = imu3.xacc;
ay3 = imu3.yacc;
az3 = imu3.zacc;
mx3 = imu3.xmag;
my3 = imu3.ymag;
mz3 = imu3.zmag;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.PID_TUNING];
if (bytearray != null)
{
var pid = bytearray.ByteArrayToStructure<MAVLink.mavlink_pid_tuning_t>(6);
//todo: currently only deals with single axis at once
pidff = pid.FF;
pidP = pid.P;
pidI = pid.I;
pidD = pid.D;
pidaxis = pid.axis;
piddesired = pid.desired;
pidachieved = pid.achieved;
}
bytearray = MAV.packets[(byte) 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;
// heading = vfr.heading;
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.VFR_HUD] = null;
}
bytearray = MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.MEMINFO];
if (bytearray != null)
{
var mem = bytearray.ByteArrayToStructure<MAVLink.mavlink_meminfo_t>(6);
freemem = mem.freemem;
brklevel = mem.brkval;
}
}
try
{
if (csCallBack != null)
csCallBack(this, null);
}
catch
{
}
//Console.Write(DateTime.Now.Millisecond + " start ");
// update form
try
{
if (bs != null)
{
bs.DataSource = this;
bs.ResetBindings(false);
return;
/*
if (bs.Count > 200)
{
while (bs.Count > 3)
bs.RemoveAt(1);
//bs.Clear();
}
bs.Add(this);
/*
return;
bs.DataSource = this;
bs.ResetBindings(false);
return;
hires.Stopwatch sw = new hires.Stopwatch();
sw.Start();
bs.DataSource = this;
bs.ResetBindings(false);
sw.Stop();
var elaps = sw.Elapsed;
Console.WriteLine("1 " + elaps.ToString("0.#####") + " done ");
sw.Start();
bs.SuspendBinding();
bs.Clear();
bs.ResumeBinding();
bs.Add(this);
sw.Stop();
elaps = sw.Elapsed;
Console.WriteLine("2 " + elaps.ToString("0.#####") + " done ");
sw.Start();
if (bs.Count > 100)
bs.Clear();
bs.Add(this);
sw.Stop();
elaps = sw.Elapsed;
Console.WriteLine("3 " + elaps.ToString("0.#####") + " done ");
*/
}
}
catch
{
log.InfoFormat("CurrentState Binding error");
}
}
}
/// <summary>
/// Use the default sysid
/// </summary>
/// <param name="bs"></param>
/// <param name="updatenow"></param>
/// <param name="mavinterface"></param>
public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow,
MAVLinkInterface mavinterface)
{
UpdateCurrentSettings(bs, updatenow, mavinterface, mavinterface.MAV);
}
/// <summary>
/// Processes a tlog to get the offsets - creates dxf of data
/// </summary>
/// <param name="fn">Filename</param>
/// <returns>Offsets</returns>
public static double[] getOffsets(string fn, int throttleThreshold = 0)
{
// based off tridge's work
string logfile = fn;
// old method
float minx = 0;
float maxx = 0;
float miny = 0;
float maxy = 0;
float minz = 0;
float maxz = 0;
// this is for a dxf
Polyline3dVertex vertex;
List<Polyline3dVertex> vertexes = new List<Polyline3dVertex>();
// data storage
Tuple<float, float, float> offset = new Tuple<float, float, float>(0, 0, 0);
List<Tuple<float, float, float>> data = new List<Tuple<float, float, float>>();
Hashtable filter = new Hashtable();
// track data to use
bool useData = false;
if (throttleThreshold <= 0)
useData = true;
log.Info("Start log: " + DateTime.Now);
using (MAVLinkInterface mine = new MAVLinkInterface())
{
try
{
mine.logplaybackfile =
new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read));
}
catch (Exception ex)
{
log.Debug(ex.ToString());
CustomMessageBox.Show("Log Can not be opened. Are you still connected?");
return new double[] {0};
}
mine.logreadmode = true;
mine.MAV.packets.Initialize(); // clear
// gather data
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
byte[] packetraw = mine.readPacket();
var packet = mine.DebugPacket(packetraw, false);
// this is for packets we dont know about
if (packet == null)
continue;
if (packet.GetType() == typeof (MAVLink.mavlink_vfr_hud_t))
{
if (((MAVLink.mavlink_vfr_hud_t) packet).throttle >= throttleThreshold)
{
useData = true;
}
else
{
useData = false;
}
}
if (packet.GetType() == typeof (MAVLink.mavlink_sensor_offsets_t))
{
offset = new Tuple<float, float, float>(
((MAVLink.mavlink_sensor_offsets_t) packet).mag_ofs_x,
((MAVLink.mavlink_sensor_offsets_t) packet).mag_ofs_y,
((MAVLink.mavlink_sensor_offsets_t) packet).mag_ofs_z);
}
else if (packet.GetType() == typeof (MAVLink.mavlink_raw_imu_t) && useData)
{
int div = 20;
// fox dxf
vertex = new Polyline3dVertex(new Vector3f(
((MAVLink.mavlink_raw_imu_t) packet).xmag - offset.Item1,
((MAVLink.mavlink_raw_imu_t) packet).ymag - offset.Item2,
((MAVLink.mavlink_raw_imu_t) packet).zmag - offset.Item3)
);
vertexes.Add(vertex);
// for old method
setMinorMax(((MAVLink.mavlink_raw_imu_t) packet).xmag - offset.Item1, ref minx, ref maxx);
setMinorMax(((MAVLink.mavlink_raw_imu_t) packet).ymag - offset.Item2, ref miny, ref maxy);
setMinorMax(((MAVLink.mavlink_raw_imu_t) packet).zmag - offset.Item3, ref minz, ref maxz);
// for new lease sq
string item = (int) (((MAVLink.mavlink_raw_imu_t) packet).xmag/div) + "," +
(int) (((MAVLink.mavlink_raw_imu_t) packet).ymag/div) + "," +
(int) (((MAVLink.mavlink_raw_imu_t) packet).zmag/div);
if (filter.ContainsKey(item))
{
filter[item] = (int) filter[item] + 1;
if ((int) filter[item] > 3)
continue;
}
else
{
filter[item] = 1;
}
data.Add(new Tuple<float, float, float>(
((MAVLink.mavlink_raw_imu_t) packet).xmag - offset.Item1,
((MAVLink.mavlink_raw_imu_t) packet).ymag - offset.Item2,
((MAVLink.mavlink_raw_imu_t) packet).zmag - offset.Item3));
}
}
log.Info("Log Processed " + DateTime.Now);
Console.WriteLine("Extracted " + data.Count + " data points");
Console.WriteLine("Current offset: " + offset);
mine.logreadmode = false;
mine.logplaybackfile.Close();
mine.logplaybackfile = null;
}
if (data.Count < 10)
{
CustomMessageBox.Show("Log does not contain enough data");
throw new Exception("Not Enough Data");
}
data.Sort(
delegate(Tuple<float, float, float> d1, Tuple<float, float, float> d2)
{
// get distance from 0,0,0
double ans1 = Math.Sqrt(d1.Item1*d1.Item1 + d1.Item2*d1.Item2 + d1.Item3*d1.Item3);
double ans2 = Math.Sqrt(d2.Item1*d2.Item1 + d2.Item2*d2.Item2 + d2.Item3*d2.Item3);
if (ans1 > ans2)
return 1;
if (ans1 < ans2)
return -1;
return 0;
}
);
data.RemoveRange(data.Count - (data.Count/16), data.Count/16);
System.Console.WriteLine("Old Method {0} {1} {2}", -(maxx + minx)/2, -(maxy + miny)/2, -(maxz + minz)/2);
double[] x = LeastSq(data);
log.Info("Least Sq Done " + DateTime.Now);
doDXF(vertexes, x);
Array.Resize<double>(ref x, 3);
return x;
}
public void setLeader(MAVLinkInterface lead)
{
Leader = lead;
}
public void setOffsets(MAVLinkInterface mav, double x, double y, double z)
{
offsets[mav] = new HIL.Vector3(x, y, z);
//log.Info(mav.ToString() + " " + offsets[mav].ToString());
}
public void doConnect(MAVLinkInterface comPort, string port, string baud)
{
bool skipconnectcheck = true;
comPort.BaseStream = new MissionPlanner.Comms.SerialPort();
comPort.BaseStream.PortName = port;
try
{
comPort.BaseStream.BaudRate = int.Parse(baud);
}
catch { };
// 记录tlog文件
try
{
Directory.CreateDirectory("D://LogFiles");
//Directory.CreateDirectory(MainV2.LogDir);
comPort.logfile =
new BufferedStream(
File.Open(
"D://LogFiles" + Path.DirectorySeparatorChar +
DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + ".tlog", FileMode.CreateNew,
FileAccess.ReadWrite, FileShare.None));
}
catch (Exception exp2)
{
log.Error(exp2);
CustomMessageBox.Show(Strings.Failclog);
} // soft fail
comPort.Open(false, skipconnectcheck);
// create a copy
int[] list = comPort.sysidseen.ToArray();
comPort.GetParam("MIS_TOTAL");
}
public void doConnect(MAVLinkInterface comPort, string portname, string baud)
{
bool skipconnectcheck = false;
log.Info("We are connecting");
switch (portname)
{
case "preset":
skipconnectcheck = true;
break;
case "TCP":
comPort.BaseStream = new TcpSerial();
_connectionControl.CMB_serialport.Text = "TCP";
break;
case "UDP":
comPort.BaseStream = new UdpSerial();
_connectionControl.CMB_serialport.Text = "UDP";
break;
case "UDPCl":
comPort.BaseStream = new UdpSerialConnect();
_connectionControl.CMB_serialport.Text = "UDPCl";
break;
case "AUTO":
default:
comPort.BaseStream = new SerialPort();
break;
}
// Tell the connection UI that we are now connected.
_connectionControl.IsConnected(true);
// Here we want to reset the connection stats counter etc.
this.ResetConnectionStats();
comPort.MAV.cs.ResetInternals();
//cleanup any log being played
comPort.logreadmode = false;
if (comPort.logplaybackfile != null)
comPort.logplaybackfile.Close();
comPort.logplaybackfile = null;
try
{
// do autoscan
if (portname == "AUTO")
{
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(Strings.Timeout);
_connectionControl.IsConnected(false);
return;
}
}
_connectionControl.CMB_serialport.Text = portname = Comms.CommsSerialScan.portinterface.PortName;
_connectionControl.CMB_baudrate.Text = baud = Comms.CommsSerialScan.portinterface.BaudRate.ToString();
}
log.Info("Set Portname");
// set port, then options
comPort.BaseStream.PortName = portname;
log.Info("Set Baudrate");
try
{
comPort.BaseStream.BaudRate = int.Parse(baud);
}
catch (Exception exp)
{
log.Error(exp);
}
// prevent serialreader from doing anything
comPort.giveComport = true;
log.Info("About to do dtr if needed");
// reset on connect logic.
if (config["CHK_resetapmonconnect"] == null || bool.Parse(config["CHK_resetapmonconnect"].ToString()) == true)
{
log.Info("set dtr rts to false");
comPort.BaseStream.DtrEnable = false;
comPort.BaseStream.RtsEnable = false;
comPort.BaseStream.toggleDTR();
}
comPort.giveComport = false;
// setup to record new logs
try
{
Directory.CreateDirectory(MainV2.LogDir);
comPort.logfile = new BufferedStream(File.Open(MainV2.LogDir + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + ".tlog", FileMode.CreateNew, FileAccess.ReadWrite, FileShare.None));
comPort.rawlogfile = new BufferedStream(File.Open(MainV2.LogDir + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + ".rlog", FileMode.CreateNew, FileAccess.ReadWrite, FileShare.None));
log.Info("creating logfile " + DateTime.Now.ToString("yyyy-MM-dd HH-mm-ss") + ".tlog");
}
catch (Exception exp2) { log.Error(exp2); CustomMessageBox.Show(Strings.Failclog); } // soft fail
// reset connect time - for timeout functions
connecttime = DateTime.Now;
// do the connect
comPort.Open(false, skipconnectcheck);
if (!comPort.BaseStream.IsOpen)
{
log.Info("comport is closed. existing connect");
try
{
_connectionControl.IsConnected(false);
UpdateConnectIcon();
comPort.Close();
}
catch { }
return;
}
// 3dr radio is hidden as no hb packet is ever emitted
if (comPort.sysidseen.Count > 1)
{
// we have more than one mav
// user selection of sysid
MissionPlanner.Controls.SysidSelector id = new SysidSelector();
id.Show();
}
// create a copy
int[] list = comPort.sysidseen.ToArray();
// get all the params
foreach (var sysid in list)
{
comPort.sysidcurrent = sysid;
comPort.getParamList();
}
// set to first seen
comPort.sysidcurrent = list[0];
// detect firmware we are conected to.
if (comPort.MAV.cs.firmware == Firmwares.ArduCopter2)
{
_connectionControl.TOOL_APMFirmware.SelectedIndex = _connectionControl.TOOL_APMFirmware.Items.IndexOf(Firmwares.ArduCopter2);
}
else if (comPort.MAV.cs.firmware == Firmwares.Ateryx)
{
_connectionControl.TOOL_APMFirmware.SelectedIndex = _connectionControl.TOOL_APMFirmware.Items.IndexOf(Firmwares.Ateryx);
}
else if (comPort.MAV.cs.firmware == Firmwares.ArduRover)
{
_connectionControl.TOOL_APMFirmware.SelectedIndex = _connectionControl.TOOL_APMFirmware.Items.IndexOf(Firmwares.ArduRover);
}
else if (comPort.MAV.cs.firmware == Firmwares.ArduPlane)
{
_connectionControl.TOOL_APMFirmware.SelectedIndex = _connectionControl.TOOL_APMFirmware.Items.IndexOf(Firmwares.ArduPlane);
}
// check for newer firmware
var softwares = Firmware.LoadSoftwares();
if (softwares.Count > 0)
{
try
{
string[] fields1 = comPort.MAV.VersionString.Split(' ');
foreach (Firmware.software item in softwares)
{
string[] fields2 = item.name.Split(' ');
// check primare firmware type. ie arudplane, arducopter
if (fields1[0] == fields2[0])
{
Version ver1 = VersionDetection.GetVersion(comPort.MAV.VersionString);
Version ver2 = VersionDetection.GetVersion(item.name);
if (ver2 > ver1)
{
Common.MessageShowAgain(Strings.NewFirmware, Strings.NewFirmwareA + item.name + Strings.Pleaseup);
break;
}
// check the first hit only
break;
}
}
}
catch (Exception ex) { log.Error(ex); }
}
FlightData.CheckBatteryShow();
MissionPlanner.Utilities.Tracking.AddEvent("Connect", "Connect", comPort.MAV.cs.firmware.ToString(), comPort.MAV.param.Count.ToString());
MissionPlanner.Utilities.Tracking.AddTiming("Connect", "Connect Time", (DateTime.Now - connecttime).TotalMilliseconds, "");
MissionPlanner.Utilities.Tracking.AddEvent("Connect", "Baud", comPort.BaseStream.BaudRate.ToString(), "");
// save the baudrate for this port
config[_connectionControl.CMB_serialport.Text + "_BAUD"] = _connectionControl.CMB_baudrate.Text;
this.Text = titlebar + " " + comPort.MAV.VersionString;
// refresh config window if needed
if (MyView.current != null)
{
if (MyView.current.Name == "HWConfig")
MyView.ShowScreen("HWConfig");
if (MyView.current.Name == "SWConfig")
MyView.ShowScreen("SWConfig");
}
// load wps on connect option.
if (config["loadwpsonconnect"] != null && bool.Parse(config["loadwpsonconnect"].ToString()) == true)
{
// only do it if we are connected.
if (comPort.BaseStream.IsOpen)
{
MenuFlightPlanner_Click(null, null);
FlightPlanner.BUT_read_Click(null, null);
}
}
// get any rallypoints
if (MainV2.comPort.MAV.param.ContainsKey("RALLY_TOTAL") && int.Parse(MainV2.comPort.MAV.param["RALLY_TOTAL"].ToString()) > 0)
{
FlightPlanner.getRallyPointsToolStripMenuItem_Click(null, null);
double maxdist = 0;
foreach (var rally in comPort.MAV.rallypoints)
{
foreach (var rally1 in comPort.MAV.rallypoints)
{
var pnt1 = new PointLatLngAlt(rally.Value.lat / 10000000.0f, rally.Value.lng / 10000000.0f);
var pnt2 = new PointLatLngAlt(rally1.Value.lat / 10000000.0f, rally1.Value.lng / 10000000.0f);
var dist = pnt1.GetDistance(pnt2);
maxdist = Math.Max(maxdist, dist);
}
}
if (comPort.MAV.param.ContainsKey("RALLY_LIMIT_KM") && (maxdist / 1000.0) > (float)comPort.MAV.param["RALLY_LIMIT_KM"])
{
CustomMessageBox.Show(Strings.Warningrallypointdistance + " " + (maxdist / 1000.0).ToString("0.00") + " > " + (float)comPort.MAV.param["RALLY_LIMIT_KM"]);
}
}
// set connected icon
this.MenuConnect.Image = displayicons.disconnect;
}
catch (Exception ex)
{
log.Warn(ex);
try
{
_connectionControl.IsConnected(false);
UpdateConnectIcon();
comPort.Close();
}
catch (Exception ex2)
{
log.Warn(ex2);
}
CustomMessageBox.Show("Can not establish a connection\n\n" + ex.Message);
return;
}
}
/// <summary>
/// main serial reader thread
/// controls
/// serial reading
/// link quality stats
/// speech voltage - custom - alt warning - data lost
/// heartbeat packet sending
///
/// and can't fall out
/// </summary>
private void SerialReader()
{
if (serialThread == true)
return;
serialThread = true;
SerialThreadrunner.Reset();
int minbytes = 0;
int altwarningmax = 0;
bool armedstatus = false;
string lastmessagehigh = "";
DateTime speechcustomtime = DateTime.Now;
DateTime speechlowspeedtime = DateTime.Now;
DateTime linkqualitytime = DateTime.Now;
while (serialThread)
{
try
{
Thread.Sleep(1); // was 5
try
{
if (GCSViews.Terminal.comPort is MAVLinkSerialPort)
{
}
else
{
if (GCSViews.Terminal.comPort != null && GCSViews.Terminal.comPort.IsOpen)
continue;
}
}
catch { }
// update connect/disconnect button and info stats
UpdateConnectIcon();
// 30 seconds interval speech options
if (speechEnable && speechEngine != null && (DateTime.Now - speechcustomtime).TotalSeconds > 30 &&
(MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen))
{
if (MainV2.speechEngine.State == SynthesizerState.Ready)
{
if (MainV2.getConfig("speechcustomenabled") == "True")
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechcustom")));
}
speechcustomtime = DateTime.Now;
}
// speech for battery alerts
//speechbatteryvolt
float warnvolt = 0;
float.TryParse(MainV2.getConfig("speechbatteryvolt"), out warnvolt);
float warnpercent = 0;
float.TryParse(MainV2.getConfig("speechbatterypercent"), out warnpercent);
if (MainV2.getConfig("speechbatteryenabled") == "True" &&
MainV2.comPort.MAV.cs.battery_voltage <= warnvolt &&
MainV2.comPort.MAV.cs.battery_voltage >= 5.0)
{
if (MainV2.speechEngine.State == SynthesizerState.Ready)
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechbattery")));
}
}
else if (MainV2.getConfig("speechbatteryenabled") == "True" &&
(MainV2.comPort.MAV.cs.battery_remaining) < warnpercent &&
MainV2.comPort.MAV.cs.battery_voltage >= 5.0 &&
MainV2.comPort.MAV.cs.battery_remaining != 0.0)
{
if (MainV2.speechEngine.State == SynthesizerState.Ready)
{
MainV2.speechEngine.SpeakAsync(
Common.speechConversion(MainV2.getConfig("speechbattery")));
}
}
}
// speech for airspeed alerts
if (speechEnable && speechEngine != null && (DateTime.Now - speechlowspeedtime).TotalSeconds > 10 && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen))
{
if (MainV2.getConfig("speechlowspeedenabled") == "True" && MainV2.comPort.MAV.cs.armed)
{
float warngroundspeed = 0;
float.TryParse(MainV2.getConfig("speechlowgroundspeedtrigger"), out warngroundspeed);
float warnairspeed = 0;
float.TryParse(MainV2.getConfig("speechlowairspeedtrigger"), out warnairspeed);
if (MainV2.comPort.MAV.cs.airspeed < warnairspeed)
{
if (MainV2.speechEngine.State == SynthesizerState.Ready)
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechlowairspeed")));
speechlowspeedtime = DateTime.Now;
}
}
else if (MainV2.comPort.MAV.cs.groundspeed < warngroundspeed)
{
if (MainV2.speechEngine.State == SynthesizerState.Ready)
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechlowgroundspeed")));
speechlowspeedtime = DateTime.Now;
}
}
else
{
speechlowspeedtime = DateTime.Now;
}
}
}
// speech altitude warning - message high warning
if (speechEnable && speechEngine != null && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen))
{
float warnalt = float.MaxValue;
float.TryParse(MainV2.getConfig("speechaltheight"), out warnalt);
try
{
int todo; // need a reset method
altwarningmax = (int)Math.Max(MainV2.comPort.MAV.cs.alt, altwarningmax);
if (MainV2.getConfig("speechaltenabled") == "True" && MainV2.comPort.MAV.cs.alt != 0.00 && (MainV2.comPort.MAV.cs.alt <= warnalt) && MainV2.comPort.MAV.cs.armed)
{
if (altwarningmax > warnalt)
{
if (MainV2.speechEngine.State == SynthesizerState.Ready)
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechalt")));
}
}
}
catch { } // silent fail
try
{
// say the latest high priority message
if (MainV2.speechEngine.State == SynthesizerState.Ready && lastmessagehigh != MainV2.comPort.MAV.cs.messageHigh)
{
MainV2.speechEngine.SpeakAsync(MainV2.comPort.MAV.cs.messageHigh);
lastmessagehigh = MainV2.comPort.MAV.cs.messageHigh;
}
}
catch { }
}
// attenuate the link qualty over time
if ((DateTime.Now - MainV2.comPort.MAV.lastvalidpacket).TotalSeconds >= 1)
{
if (linkqualitytime.Second != DateTime.Now.Second)
{
MainV2.comPort.MAV.cs.linkqualitygcs = (ushort)(MainV2.comPort.MAV.cs.linkqualitygcs * 0.8f);
linkqualitytime = DateTime.Now;
// force redraw is no other packets are being read
GCSViews.FlightData.myhud.Invalidate();
}
}
// data loss warning - wait min of 10 seconds, ignore first 30 seconds of connect, repeat at 5 seconds interval
if ((DateTime.Now - MainV2.comPort.MAV.lastvalidpacket).TotalSeconds > 10
&& (DateTime.Now - connecttime).TotalSeconds > 30
&& (DateTime.Now - nodatawarning).TotalSeconds > 5
&& (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen)
&& MainV2.comPort.MAV.cs.armed)
{
if (speechEnable && speechEngine != null)
{
if (MainV2.speechEngine.State == SynthesizerState.Ready)
{
MainV2.speechEngine.SpeakAsync("WARNING No Data for " + (int)(DateTime.Now - MainV2.comPort.MAV.lastvalidpacket).TotalSeconds + " Seconds");
nodatawarning = DateTime.Now;
}
}
}
// get home point on armed status change.
if (armedstatus != MainV2.comPort.MAV.cs.armed && comPort.BaseStream.IsOpen)
{
armedstatus = MainV2.comPort.MAV.cs.armed;
// status just changed to armed
if (MainV2.comPort.MAV.cs.armed == true)
{
try
{
MainV2.comPort.MAV.cs.HomeLocation = new PointLatLngAlt(MainV2.comPort.getWP(0));
if (MyView.current != null && MyView.current.Name == "FlightPlanner")
{
// update home if we are on flight data tab
FlightPlanner.updateHome();
}
}
catch
{
// dont hang this loop
this.BeginInvoke((MethodInvoker)delegate { CustomMessageBox.Show("Failed to update home location"); });
}
}
if (speechEnable && speechEngine != null)
{
if (MainV2.getConfig("speecharmenabled") == "True")
{
if (armedstatus)
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speecharm")));
else
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechdisarm")));
}
}
}
// send a hb every seconds from gcs to ap
if (heatbeatSend.Second != DateTime.Now.Second)
{
MAVLink.mavlink_heartbeat_t htb = new MAVLink.mavlink_heartbeat_t()
{
type = (byte)MAVLink.MAV_TYPE.GCS,
autopilot = (byte)MAVLink.MAV_AUTOPILOT.INVALID,
mavlink_version = 3// MAVLink.MAVLINK_VERSION
};
foreach (var port in Comports)
{
try
{
port.sendPacket(htb);
}
catch (Exception ex)
{
log.Error(ex);
// close the bad port
port.Close();
// refresh the screen if needed
if (port == MainV2.comPort)
{
// refresh config window if needed
if (MyView.current != null)
{
if (MyView.current.Name == "HWConfig")
MyView.ShowScreen("HWConfig");
if (MyView.current.Name == "SWConfig")
MyView.ShowScreen("SWConfig");
}
}
}
}
heatbeatSend = DateTime.Now;
}
// if not connected or busy, sleep and loop
if (!comPort.BaseStream.IsOpen || comPort.giveComport == true)
{
if (!comPort.BaseStream.IsOpen)
{
// check if other ports are still open
foreach (var port in Comports)
{
if (port.BaseStream.IsOpen)
{
Console.WriteLine("Main comport shut, swapping to other mav");
comPort = port;
break;
}
}
}
System.Threading.Thread.Sleep(100);
}
// read the interfaces
foreach (var port in Comports)
{
if (!port.BaseStream.IsOpen)
{
// skip primary interface
if (port == comPort)
continue;
// modify array and drop out
Comports.Remove(port);
break;
}
while (port.BaseStream.IsOpen && port.BaseStream.BytesToRead > minbytes && port.giveComport == false)
{
try
{
port.readPacket();
}
catch (Exception ex) { log.Error(ex); }
}
// update currentstate of sysids on the port
foreach (var sysid in port.sysidseen)
{
try
{
port.MAVlist[sysid].cs.UpdateCurrentSettings(null, false, port, port.MAVlist[sysid]);
}
catch (Exception ex) { log.Error(ex); }
}
}
}
catch (Exception e)
{
log.Error("Serial Reader fail :" + e.ToString());
try
{
comPort.Close();
}
catch (Exception ex) { log.Error(ex); }
}
}
Console.WriteLine("SerialReader Done");
SerialThreadrunner.Set();
}
private void but_multimav_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;
}
}
MAVLinkInterface com2 = new MAVLinkInterface();
com2.BaseStream.PortName = Comms.CommsSerialScan.portinterface.PortName;
com2.BaseStream.BaudRate = Comms.CommsSerialScan.portinterface.BaudRate;
com2.Open(true);
MainV2.Comports.Add(com2);
CMB_mavs.DataSource = MainV2.Comports;
}
List<string[]> readLog(string fn)
{
if (logcache.Count > 0)
return logcache;
List<string[]> list = new List<string[]>();
if (fn.ToLower().EndsWith("tlog"))
{
MAVLinkInterface mine = new MAVLinkInterface();
mine.logplaybackfile = new BinaryReader(File.Open(fn, FileMode.Open, FileAccess.Read, FileShare.Read));
mine.logreadmode = true;
mine.MAV.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);
// old
// line "GPS: 82686250, 1, 8, -34.1406480, 118.5441900, 0.0000, 309.1900, 315.9500, 0.0000, 279.1200" string
//Status,Time,NSats,HDop,Lat,Lng,RelAlt,Alt,Spd,GCrs
//GPS, 3, 122732, 10, 0.00, -35.3628880, 149.1621961, 808.90, 810.30, 23.30, 94.04
string[] vals = new string[] { "GPS", "3", (cs.datetime.ToUniversalTime() - new DateTime(cs.datetime.Year,cs.datetime.Month,cs.datetime.Day,0,0,0,DateTimeKind.Utc)).TotalMilliseconds.ToString(),
cs.satcount.ToString(),cs.gpshdop.ToString(),cs.lat.ToString(),cs.lng.ToString(),cs.altasl.ToString(),cs.altasl.ToString(),cs.groundspeed.ToString(),cs.yaw.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();
logcache = list;
return list;
}
StreamReader sr = new StreamReader(fn);
string lasttime = "0";
while (!sr.EndOfStream)
{
string line = sr.ReadLine();
if (line.ToLower().StartsWith("gps"))
{
if (!sr.EndOfStream)
{
string line2 = sr.ReadLine();
if (line2.ToLower().StartsWith("att"))
{
line = string.Concat(line, ",", line2);
}
}
string[] vals = line.Split(new char[] {',',':'});
if (lasttime == vals[timepos])
continue;
lasttime = vals[timepos];
list.Add(vals);
}
}
sr.Close();
logcache = list;
return list;
}
static string process(MAVLinkInterface mavint)
{
DateTime Deadline = DateTime.Now.AddSeconds(60);
Vector3 last_mag = null;
double last_usec = 0;
double count = 0;
double[] total_error = new double[rotations.Length];
float AHRS_ORIENTATION = 0;
float COMPASS_ORIENT = 0;
float COMPASS_EXTERNAL = 0;
//# now gather all the data
while (DateTime.Now < Deadline || mavint.BaseStream.BytesToRead > 0)
{
MAVLink.MAVLinkMessage packetbytes = mavint.readPacket();
if (packetbytes.Length < 5)
continue;
object packet = packetbytes.data;
if (packet == null)
continue;
if (packet is MAVLink.mavlink_param_value_t)
{
MAVLink.mavlink_param_value_t m = (MAVLink.mavlink_param_value_t) packet;
if (str(m.param_id) == "AHRS_ORIENTATION")
AHRS_ORIENTATION = (int) (m.param_value);
if (str(m.param_id) == "COMPASS_ORIENT")
COMPASS_ORIENT = (int) (m.param_value);
if (str(m.param_id) == "COMPASS_EXTERNAL")
COMPASS_EXTERNAL = (int) (m.param_value);
}
if (packet is MAVLink.mavlink_raw_imu_t)
{
MAVLink.mavlink_raw_imu_t m = (MAVLink.mavlink_raw_imu_t) packet;
Vector3 mag = new Vector3(m.xmag, m.ymag, m.zmag);
mag = mag_fixup(mag, AHRS_ORIENTATION, COMPASS_ORIENT, COMPASS_EXTERNAL);
Vector3 gyr = new Vector3(m.xgyro, m.ygyro, m.zgyro)*0.001;
double usec = m.time_usec;
if (last_mag != null && gyr.length() > radians(5.0))
{
add_errors(mag, gyr, last_mag, (usec - last_usec)*1.0e-6, total_error, rotations);
count += 1;
}
last_mag = mag;
last_usec = usec;
}
}
int best_i = 0;
double best_err = total_error[0];
foreach (var i in range(len(rotations)))
{
Rotation r = rotations[i];
// if (!r.is_90_degrees())
// continue;
//if ( opts.verbose) {
// print("%s err=%.2f" % (r, total_error[i]/count));}
if (total_error[i] < best_err)
{
best_i = i;
best_err = total_error[i];
}
}
Rotation rans = rotations[best_i];
Console.WriteLine("Best rotation is {0} err={1} from {2} points", rans, best_err/count, count);
//print("Best rotation is %s err=%.2f from %u points" % (r, best_err/count, count));
return rans.name;
}
private void but_droneapi_Click(object sender, EventArgs e)
{
string droneshareusername = MainV2.getConfig("droneshareusername");
InputBox.Show("Username", "Username", ref droneshareusername);
MainV2.config["droneshareusername"] = droneshareusername;
string dronesharepassword = MainV2.getConfig("dronesharepassword");
if (dronesharepassword != "")
{
try
{
// fail on bad entry
var crypto = new Crypto();
dronesharepassword = crypto.DecryptString(dronesharepassword);
}
catch
{
}
}
InputBox.Show("Password", "Password", ref dronesharepassword, true);
var crypto2 = new Crypto();
string encryptedpw = crypto2.EncryptString(dronesharepassword);
MainV2.config["dronesharepassword"] = encryptedpw;
DroneProto dp = new DroneProto();
if (dp.connect())
{
if (dp.loginUser(droneshareusername, dronesharepassword))
{
MAVLinkInterface mine = new MAVLinkInterface();
var comfile = new CommsFile();
mine.BaseStream = comfile;
mine.BaseStream.PortName = @"C:\Users\hog\Documents\apm logs\iris 6-4-14\2014-04-06 09-07-32.tlog";
mine.BaseStream.Open();
comfile.bps = 4000;
mine.getHeartBeat();
dp.setVechileId(mine.MAV.Guid, 0, mine.MAV.sysid);
dp.startMission();
while (true)
{
byte[] packet = mine.readPacket();
dp.SendMavlink(packet, 0);
}
// dp.close();
// mine.Close();
}
}
}
public void doDisconnect(MAVLinkInterface comPort)
{
try
{
comPort.BaseStream.DtrEnable = false;
comPort.Close();
}
catch
{
}
}
private void start_Terminal(bool px4)
{
setcomport();
try
{
if (MainV2.comPort != null && MainV2.comPort.BaseStream != null && MainV2.comPort.BaseStream.IsOpen)
MainV2.comPort.BaseStream.Close();
if (comPort.IsOpen)
{
Console.WriteLine("Terminal Start - Close Port");
threadrun = false;
// if (DialogResult.Cancel == CustomMessageBox.Show("The port is open\n Continue?", "Continue", MessageBoxButtons.YesNo))
{
// return;
}
comPort.Close();
// allow things to cleanup
System.Threading.Thread.Sleep(400);
}
comPort.ReadBufferSize = 1024 * 1024 * 4;
comPort.PortName = MainV2.comPortName;
// test moving baud rate line
comPort.BaudRate = int.Parse(MainV2._connectionControl.CMB_baudrate.Text);
if (px4)
{
TXT_terminal.AppendText("Rebooting\n");
// keep it local
MAVLinkInterface mine = new MAVLinkInterface();
mine.BaseStream.PortName = MainV2.comPortName;
mine.BaseStream.BaudRate = comPort.BaudRate;
mine.giveComport = true;
mine.BaseStream.Open();
// check if we are a mavlink stream
byte[] buffer = mine.readPacket();
if (buffer.Length > 0)
{
log.Info("got packet - sending reboot via mavlink");
TXT_terminal.AppendText("Via Mavlink\n");
mine.doReboot(false);
try
{
mine.BaseStream.Close();
}
catch { }
}
else
{
log.Info("no packet - sending reboot via console");
TXT_terminal.AppendText("Via Console\n");
MainV2.comPort.BaseStream.Write("exit\rreboot\r");
try
{
MainV2.comPort.BaseStream.Close();
}
catch { }
}
TXT_terminal.AppendText("Waiting for reboot\n");
// wait 7 seconds for px4 reboot
log.Info("waiting for reboot");
DateTime deadline = DateTime.Now.AddSeconds(8);
while (DateTime.Now < deadline)
{
System.Threading.Thread.Sleep(500);
Application.DoEvents();
}
int a = 0;
while (a < 5)
{
try
{
if (!comPort.IsOpen)
comPort.Open();
}
catch { }
System.Threading.Thread.Sleep(200);
a++;
}
}
else
{
log.Info("About to open " + comPort.PortName);
comPort.Open();
comPort.toggleDTR();
}
try
{
comPort.DiscardInBuffer();
}
catch { }
Console.WriteLine("Terminal_Load run " + threadrun + " " + comPort.IsOpen);
BUT_disconnect.Enabled = true;
System.Threading.Thread t11 = new System.Threading.Thread(delegate()
{
threadrun = true;
Console.WriteLine("Terminal thread start run run " + threadrun + " " + comPort.IsOpen);
// 10 sec
waitandsleep(10000);
Console.WriteLine("Terminal thread 1 run " + threadrun + " " + comPort.IsOpen);
// 100 ms
readandsleep(100);
Console.WriteLine("Terminal thread 2 run " + threadrun + " " + comPort.IsOpen);
try
{
if (!inlogview && comPort.IsOpen)
comPort.Write("\n\n\n");
// 1 secs
if (!inlogview && comPort.IsOpen)
readandsleep(1000);
if (!inlogview && comPort.IsOpen)
comPort.Write("\r\r\r?\r");
}
catch (Exception ex) { Console.WriteLine("Terminal thread 3 " + ex.ToString()); ChangeConnectStatus(false); threadrun = false; return; }
Console.WriteLine("Terminal thread 3 run " + threadrun + " " + comPort.IsOpen);
while (threadrun)
{
try
{
System.Threading.Thread.Sleep(10);
if (!threadrun)
break;
if (this.Disposing)
break;
if (inlogview)
continue;
if (!comPort.IsOpen)
{
Console.WriteLine("Comport Closed");
ChangeConnectStatus(false);
break;
}
if (comPort.BytesToRead > 0)
{
comPort_DataReceived((object)null, (SerialDataReceivedEventArgs)null);
}
}
catch (Exception ex) { Console.WriteLine("Terminal thread 4 " + ex.ToString()); }
}
threadrun = false;
try
{
comPort.DtrEnable = false;
}
catch { }
try
{
Console.WriteLine("term thread close run " + threadrun + " " + comPort.IsOpen);
ChangeConnectStatus(false);
comPort.Close();
}
catch { }
Console.WriteLine("Comport thread close run " + threadrun);
});
t11.IsBackground = true;
t11.Name = "Terminal serial thread";
t11.Start();
// doesnt seem to work on mac
//comPort.DataReceived += new SerialDataReceivedEventHandler(comPort_DataReceived);
if (this.IsDisposed || this.Disposing)
return;
TXT_terminal.AppendText("Opened com port\r\n");
inputStartPos = TXT_terminal.SelectionStart;
}
catch (Exception ex) { log.Error(ex); TXT_terminal.AppendText("Cant open serial port\r\n"); return; }
TXT_terminal.Focus();
}
void dolog()
{
flightdata.Clear();
using (MAVLinkInterface mine = new MAVLinkInterface())
{
try
{
mine.logplaybackfile =
new BinaryReader(File.Open(txt_tlog.Text, FileMode.Open, FileAccess.Read, FileShare.Read));
}
catch
{
CustomMessageBox.Show("Log Can not be opened. Are you still connected?");
return;
}
mine.logreadmode = true;
mine.MAV.packets.Initialize(); // clear
mine.readPacket();
startlogtime = mine.lastlogread;
double oldlatlngsum = 0;
int appui = 0;
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
byte[] packet = mine.readPacket();
cs.datetime = mine.lastlogread;
cs.UpdateCurrentSettings(null, true, mine);
if (appui != DateTime.Now.Second)
{
// cant do entire app as mixes with flightdata timer
this.Refresh();
appui = DateTime.Now.Second;
}
try
{
if (MainV2.speechEngine != null)
MainV2.speechEngine.SpeakAsyncCancelAll();
}
catch
{
}
// ignore because of this Exception System.PlatformNotSupportedException: No voice installed on the system or none available with the current security setting.
// if ((float)(cs.lat + cs.lng + cs.alt) != oldlatlngsum
// && cs.lat != 0 && cs.lng != 0)
DateTime nexttime = mine.lastlogread.AddMilliseconds(-(mine.lastlogread.Millisecond%100));
if (!flightdata.ContainsKey(nexttime))
{
Console.WriteLine(cs.lat + " " + cs.lng + " " + cs.alt + " lah " +
(float) (cs.lat + cs.lng + cs.alt) + "!=" + oldlatlngsum);
CurrentState cs2 = (CurrentState) cs.Clone();
try
{
flightdata.Add(nexttime, cs2);
}
catch
{
}
oldlatlngsum = (cs.lat + cs.lng + cs.alt);
}
}
mine.logreadmode = false;
mine.logplaybackfile.Close();
mine.logplaybackfile = null;
}
}
// Return List with all GPS Messages splitted in string arrays
Dictionary<long, VehicleLocation> readGPSMsgInLog(string fn)
{
Dictionary<long, VehicleLocation> vehiclePositionList = new Dictionary<long,VehicleLocation>();
// Telemetry Log
if (fn.ToLower().EndsWith("tlog"))
{
using (MAVLinkInterface mine = new MAVLinkInterface())
{
mine.logplaybackfile = new BinaryReader(File.Open(fn, FileMode.Open, FileAccess.Read, FileShare.Read));
mine.logreadmode = true;
mine.MAV.packets.Initialize(); // clear
CurrentState cs = new CurrentState();
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
byte[] packet = mine.readPacket();
cs.datetime = mine.lastlogread;
cs.UpdateCurrentSettings(null, true, mine);
VehicleLocation location = new VehicleLocation();
location.Time = cs.datetime;
location.Lat = cs.lat;
location.Lon = cs.lng;
location.RelAlt = cs.alt;
location.AltAMSL = cs.altasl;
location.Roll = cs.roll;
location.Pitch = cs.pitch;
location.Yaw = cs.yaw;
vehiclePositionList[ToMilliseconds(location.Time)] = location;
// 4 5 7
Console.Write((mine.logplaybackfile.BaseStream.Position * 100 / mine.logplaybackfile.BaseStream.Length) + " \r");
}
mine.logplaybackfile.Close();
}
}
// DataFlash Log
else
{
// convert bin to log
if (fn.ToLower().EndsWith("bin"))
{
string tempfile = Path.GetTempFileName();
Log.BinaryLog.ConvertBin(fn, tempfile);
fn = tempfile;
}
StreamReader sr = new StreamReader(fn);
// Will hold the last seen Attitude information in order to incorporate them into the GPS Info
float currentYaw = 0f;
float currentRoll = 0f;
float currentPitch = 0f;
int a = 0;
while (!sr.EndOfStream)
{
a++;
string line = sr.ReadLine();
var item = DFLog.GetDFItemFromLine(line, a);
if (item.msgtype == "GPS")
{
if (!DFLog.logformat.ContainsKey("GPS"))
continue;
int latindex = DFLog.FindMessageOffset("GPS", "Lat");
int lngindex = DFLog.FindMessageOffset("GPS", "Lng");
int altindex = DFLog.FindMessageOffset("GPS", "Alt");
int raltindex = DFLog.FindMessageOffset("GPS", "RAlt");
if (raltindex == -1)
raltindex = DFLog.FindMessageOffset("GPS", "RelAlt");
VehicleLocation location = new VehicleLocation();
try
{
location.Time = DFLog.GetTimeGPS(line);
location.Lat = double.Parse(item.items[latindex], CultureInfo.InvariantCulture);
location.Lon = double.Parse(item.items[lngindex], CultureInfo.InvariantCulture);
location.RelAlt = double.Parse(item.items[raltindex], CultureInfo.InvariantCulture);
location.AltAMSL = double.Parse(item.items[altindex], CultureInfo.InvariantCulture);
location.Roll = currentRoll;
location.Pitch = currentPitch;
location.Yaw = currentYaw;
long millis = ToMilliseconds(location.Time);
//System.Diagnostics.Debug.WriteLine("GPS MSG - UTCMillis = " + millis + " GPS Week = " + getValueFromStringArray(gpsLineValues, gpsweekpos) + " TimeMS = " + getValueFromStringArray(gpsLineValues, timepos));
if (!vehiclePositionList.ContainsKey(millis) && (location.Time != DateTime.MinValue))
vehiclePositionList[millis] = location;
}
catch
{
Console.WriteLine("Bad GPS Line");
}
}
else if (item.msgtype == "ATT")
{
int Rindex = DFLog.FindMessageOffset("ATT", "Roll");
int Pindex = DFLog.FindMessageOffset("ATT", "Pitch");
int Yindex = DFLog.FindMessageOffset("ATT", "Yaw");
currentRoll = float.Parse(item.items[Rindex], CultureInfo.InvariantCulture);
currentPitch = float.Parse(item.items[Pindex], CultureInfo.InvariantCulture);
currentYaw = float.Parse(item.items[Yindex], CultureInfo.InvariantCulture);
}
// Look for GPS Messages. However GPS Messages do not have Roll, Pitch and Yaw
// So we have to look for one ATT message after having read a GPS one
//if (line.ToLower().StartsWith("gps"))
//{
// VehicleLocation location = new VehicleLocation();
// string[] gpsLineValues = line.Split(new char[] { ',', ':' });
// try
// {
// location.Time = GetTimeFromGps(int.Parse(getValueFromStringArray(gpsLineValues, gpsweekpos), CultureInfo.InvariantCulture), int.Parse(getValueFromStringArray(gpsLineValues, timepos), CultureInfo.InvariantCulture));
// location.Lat = double.Parse(getValueFromStringArray(gpsLineValues, latpos), CultureInfo.InvariantCulture);
// location.Lon = double.Parse(getValueFromStringArray(gpsLineValues, lngpos), CultureInfo.InvariantCulture);
// location.RelAlt = double.Parse(getValueFromStringArray(gpsLineValues, altpos), CultureInfo.InvariantCulture);
// location.AltAMSL = double.Parse(getValueFromStringArray(gpsLineValues, altAMSLpos), CultureInfo.InvariantCulture);
// location.Roll = currentRoll;
// location.Pitch = currentPitch;
// location.Yaw = currentYaw;
// long millis = ToMilliseconds(location.Time);
// //System.Diagnostics.Debug.WriteLine("GPS MSG - UTCMillis = " + millis + " GPS Week = " + getValueFromStringArray(gpsLineValues, gpsweekpos) + " TimeMS = " + getValueFromStringArray(gpsLineValues, timepos));
// if (!vehiclePositionList.ContainsKey(millis))
// vehiclePositionList[millis] = location;
// }
// catch { Console.WriteLine("Bad GPS Line"); }
//}
//else if (line.ToLower().StartsWith("att"))
//{
// string[] attLineValues = line.Split(new char[] { ',', ':' });
// currentRoll = float.Parse(getValueFromStringArray(attLineValues, rollATT), CultureInfo.InvariantCulture);
// currentPitch = float.Parse(getValueFromStringArray(attLineValues, pitchATT), CultureInfo.InvariantCulture);
// currentYaw = float.Parse(getValueFromStringArray(attLineValues, yawATT), CultureInfo.InvariantCulture);
//}
}
sr.Close();
}
return vehiclePositionList;
}
// Return List with all GPS Messages splitted in string arrays
Dictionary<long, VehicleLocation> readGPSMsgInLog(string fn)
{
Dictionary<long, VehicleLocation> vehiclePositionList = new Dictionary<long,VehicleLocation>();
// Telemetry Log
if (fn.ToLower().EndsWith("tlog"))
{
MAVLinkInterface mine = new MAVLinkInterface();
mine.logplaybackfile = new BinaryReader(File.Open(fn, FileMode.Open, FileAccess.Read, FileShare.Read));
mine.logreadmode = true;
mine.MAV.packets.Initialize(); // clear
CurrentState cs = new CurrentState();
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
byte[] packet = mine.readPacket();
cs.datetime = mine.lastlogread;
cs.UpdateCurrentSettings(null, true, mine);
VehicleLocation location = new VehicleLocation();
location.Time = cs.datetime;
location.Lat = cs.lat;
location.Lon = cs.lng;
location.RelAlt = cs.alt;
location.AltAMSL = cs.altasl;
location.Roll = cs.roll;
location.Pitch = cs.pitch;
location.Yaw = cs.yaw;
vehiclePositionList[ToMilliseconds(location.Time)] = location;
// 4 5 7
Console.Write((mine.logplaybackfile.BaseStream.Position * 100 / mine.logplaybackfile.BaseStream.Length) + " \r");
}
mine.logplaybackfile.Close();
}
// DataFlash Log
else
{
StreamReader sr = new StreamReader(fn);
// Will hold the last seen Attitude information in order to incorporate them into the GPS Info
float currentYaw = 0f;
float currentRoll = 0f;
float currentPitch = 0f;
while (!sr.EndOfStream)
{
string line = sr.ReadLine();
// Look for GPS Messages. However GPS Messages do not have Roll, Pitch and Yaw
// So we have to look for one ATT message after having read a GPS one
if (line.ToLower().StartsWith("gps"))
{
VehicleLocation location = new VehicleLocation();
string[] gpsLineValues = line.Split(new char[] { ',', ':' });
try
{
location.Time = GetTimeFromGps(int.Parse(getValueFromStringArray(gpsLineValues, gpsweekpos), CultureInfo.InvariantCulture), int.Parse(getValueFromStringArray(gpsLineValues, timepos), CultureInfo.InvariantCulture));
location.Lat = double.Parse(getValueFromStringArray(gpsLineValues, latpos), CultureInfo.InvariantCulture);
location.Lon = double.Parse(getValueFromStringArray(gpsLineValues, lngpos), CultureInfo.InvariantCulture);
location.RelAlt = double.Parse(getValueFromStringArray(gpsLineValues, altpos), CultureInfo.InvariantCulture);
location.AltAMSL = double.Parse(getValueFromStringArray(gpsLineValues, altAMSLpos), CultureInfo.InvariantCulture);
location.Roll = currentRoll;
location.Pitch = currentPitch;
location.Yaw = currentYaw;
long millis = ToMilliseconds(location.Time);
//System.Diagnostics.Debug.WriteLine("GPS MSG - UTCMillis = " + millis + " GPS Week = " + getValueFromStringArray(gpsLineValues, gpsweekpos) + " TimeMS = " + getValueFromStringArray(gpsLineValues, timepos));
if (!vehiclePositionList.ContainsKey(millis))
vehiclePositionList[millis] = location;
}
catch { Console.WriteLine("Bad GPS Line"); }
}
else if (line.ToLower().StartsWith("att"))
{
string[] attLineValues = line.Split(new char[] { ',', ':' });
currentRoll = float.Parse(getValueFromStringArray(attLineValues, rollATT), CultureInfo.InvariantCulture);
currentPitch = float.Parse(getValueFromStringArray(attLineValues, pitchATT), CultureInfo.InvariantCulture);
currentYaw = float.Parse(getValueFromStringArray(attLineValues, yawATT), CultureInfo.InvariantCulture);
}
}
sr.Close();
}
return vehiclePositionList;
}
private void grid1_UpdateOffsets(MAVLinkInterface 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);
}
}
public void doDisconnect(MAVLinkInterface comPort)
{
log.Info("We are disconnecting");
try
{
if (speechEngine != null) // cancel all pending speech
speechEngine.SpeakAsyncCancelAll();
comPort.BaseStream.DtrEnable = false;
comPort.Close();
}
catch (Exception ex)
{
log.Error(ex);
}
// now that we have closed the connection, cancel the connection stats
// so that the 'time connected' etc does not grow, but the user can still
// look at the now frozen stats on the still open form
try
{
// if terminal is used, then closed using this button.... exception
if (this.connectionStatsForm != null)
((ConnectionStats)this.connectionStatsForm.Controls[0]).StopUpdates();
}
catch { }
// refresh config window if needed
if (MyView.current != null)
{
if (MyView.current.Name == "HWConfig")
MyView.ShowScreen("HWConfig");
if (MyView.current.Name == "SWConfig")
MyView.ShowScreen("SWConfig");
}
try
{
System.Threading.ThreadPool.QueueUserWorkItem((WaitCallback)delegate
{
try
{
MissionPlanner.Log.LogSort.SortLogs(Directory.GetFiles(MainV2.LogDir, "*.tlog"));
}
catch { }
}
);
}
catch { }
this.MenuConnect.Image = global::MissionPlanner.Properties.Resources.light_connect_icon;
}
public static void tlog(string logfile)
{
List<MLArray> mlList = new List<MLArray>();
Hashtable datappl = new Hashtable();
using (MAVLinkInterface mine = new MAVLinkInterface())
{
try
{
mine.logplaybackfile =
new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read));
}
catch
{
CustomMessageBox.Show("Log Can not be opened. Are you still connected?");
return;
}
mine.logreadmode = true;
mine.speechenabled = false;
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
MAVLink.MAVLinkMessage packet = mine.readPacket();
object data = packet.data;
if (data == null)
continue;
if (data is MAVLink.mavlink_heartbeat_t)
{
if (((MAVLink.mavlink_heartbeat_t) data).type == (byte) MAVLink.MAV_TYPE.GCS)
continue;
}
Type test = data.GetType();
DateTime time = mine.lastlogread;
double matlabtime = GetMatLabSerialDate(time);
try
{
foreach (var field in test.GetFields())
{
// field.Name has the field's name.
object fieldValue = field.GetValue(data); // Get value
if (field.FieldType.IsArray)
{
}
else
{
if (!datappl.ContainsKey(field.Name + "_" + field.DeclaringType.Name))
{
datappl[field.Name + "_" + field.DeclaringType.Name] = new List<double[]>();
}
List<double[]> list =
((List<double[]>) datappl[field.Name + "_" + field.DeclaringType.Name]);
object value = fieldValue;
if (value.GetType() == typeof (Single))
{
list.Add(new double[] {matlabtime, (double) (Single) field.GetValue(data)});
}
else if (value.GetType() == typeof (short))
{
list.Add(new double[] {matlabtime, (double) (short) field.GetValue(data)});
}
else if (value.GetType() == typeof (ushort))
{
list.Add(new double[] {matlabtime, (double) (ushort) field.GetValue(data)});
}
else if (value.GetType() == typeof (byte))
{
list.Add(new double[] {matlabtime, (double) (byte) field.GetValue(data)});
}
else if (value.GetType() == typeof (sbyte))
{
list.Add(new double[] {matlabtime, (double) (sbyte) field.GetValue(data)});
}
else if (value.GetType() == typeof (Int32))
{
list.Add(new double[] {matlabtime, (double) (Int32) field.GetValue(data)});
}
else if (value.GetType() == typeof (UInt32))
{
list.Add(new double[] {matlabtime, (double) (UInt32) field.GetValue(data)});
}
else if (value.GetType() == typeof (ulong))
{
list.Add(new double[] {matlabtime, (double) (ulong) field.GetValue(data)});
}
else if (value.GetType() == typeof (long))
{
list.Add(new double[] {matlabtime, (double) (long) field.GetValue(data)});
}
else if (value.GetType() == typeof (double))
{
list.Add(new double[] {matlabtime, (double) (double) field.GetValue(data)});
}
else
{
Console.WriteLine("Unknown data type");
}
}
}
}
catch
{
}
}
mine.logreadmode = false;
mine.logplaybackfile.Close();
mine.logplaybackfile = null;
}
foreach (string item in datappl.Keys)
{
double[][] temp = ((List<double[]>) datappl[item]).ToArray();
MLArray dbarray = new MLDouble(item.Replace(" ", "_"), temp);
mlList.Add(dbarray);
}
try
{
MatFileWriter mfw = new MatFileWriter(logfile + ".mat", mlList, true);
}
catch (Exception err)
{
MessageBox.Show("There was an error when creating the MAT-file: \n" + err.ToString(),
"MAT-File Creation Error!", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
return;
}
}
/// <summary>
/// Recevied UDP packet, process and send required data to serial port.
/// </summary>
/// <param name="data">Packet</param>
/// <param name="receviedbytes">Length</param>
/// <param name="comPort">Com Port</param>
private void RECVprocess(byte[] data, int receviedbytes, MAVLinkInterface comPort)
{
sitl_fdm sitldata = new sitl_fdm();
if (data[0] == 'D' && data[1] == 'A')
{
// Xplanes sends
// 5 byte header
// 1 int for the index - numbers on left of output
// 8 floats - might be useful. or 0 if not
int count = 5;
while (count < receviedbytes)
{
int index = BitConverter.ToInt32(data, count);
DATA[index] = new float[8];
DATA[index][0] = BitConverter.ToSingle(data, count + 1 * 4); ;
DATA[index][1] = BitConverter.ToSingle(data, count + 2 * 4); ;
DATA[index][2] = BitConverter.ToSingle(data, count + 3 * 4); ;
DATA[index][3] = BitConverter.ToSingle(data, count + 4 * 4); ;
DATA[index][4] = BitConverter.ToSingle(data, count + 5 * 4); ;
DATA[index][5] = BitConverter.ToSingle(data, count + 6 * 4); ;
DATA[index][6] = BitConverter.ToSingle(data, count + 7 * 4); ;
DATA[index][7] = BitConverter.ToSingle(data, count + 8 * 4); ;
count += 36; // 8 * float
}
bool xplane9 = !CHK_xplane10.Checked;
if (xplane9)
{
sitldata.pitchDeg = (DATA[18][0]);
sitldata.rollDeg = (DATA[18][1]);
sitldata.yawDeg = (DATA[18][2]);
sitldata.pitchRate = (DATA[17][0] * rad2deg);
sitldata.rollRate = (DATA[17][1] * rad2deg);
sitldata.yawRate = (DATA[17][2] * rad2deg);
sitldata.heading = ((float)DATA[19][2]);
sitldata.speedN =-DATA[21][5];// (DATA[3][7] * 0.44704 * Math.Sin(sitldata.heading * deg2rad));
sitldata.speedE = DATA[21][3];// (DATA[3][7] * 0.44704 * Math.Cos(sitldata.heading * deg2rad));
sitldata.speedD = -DATA[21][4];
}
else
{
sitldata.pitchDeg = (DATA[17][0]);
sitldata.rollDeg = (DATA[17][1]);
sitldata.yawDeg = (DATA[17][2]);
sitldata.pitchRate = (DATA[16][0] * rad2deg);
sitldata.rollRate = (DATA[16][1] * rad2deg);
sitldata.yawRate = (DATA[16][2] * rad2deg);
sitldata.heading = (DATA[18][2]); // 18-2
sitldata.speedN = -DATA[21][5];// (DATA[3][7] * 0.44704 * Math.Sin(sitldata.heading * deg2rad));
sitldata.speedE = DATA[21][3];// (DATA[3][7] * 0.44704 * Math.Cos(sitldata.heading * deg2rad));
sitldata.speedD = -DATA[21][4];
}
sitldata.airspeed = ((DATA[3][5] * .44704));
sitldata.latitude = (DATA[20][0]);
sitldata.longitude = (DATA[20][1]);
sitldata.altitude = (DATA[20][2] * ft2m);
Matrix3 dcm = new Matrix3();
dcm.from_euler(sitldata.rollDeg * deg2rad, sitldata.pitchDeg * deg2rad, sitldata.heading * deg2rad);
// rad = tas^2 / (tan(angle) * G)
float turnrad = (float)(((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704)) / (float)(9.808f * Math.Tan(sitldata.rollDeg * deg2rad)));
float gload = (float)(1 / Math.Cos(sitldata.rollDeg * deg2rad)); // calculated Gs
// a = v^2/r
float centripaccel = (float)((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704)) / turnrad;
Vector3 accel_body = dcm.transposed() * (new Vector3(0, 0, -9.808));
Vector3 centrip_accel = new Vector3(0, centripaccel * Math.Cos(sitldata.rollDeg * deg2rad), centripaccel * Math.Sin(sitldata.rollDeg * deg2rad));
// accel_body += centrip_accel;
Vector3 velocitydelta = dcm.transposed() * (new Vector3((sitldata_old.speedN - sitldata.speedN), (sitldata_old.speedE - sitldata.speedE), (sitldata_old.speedD - sitldata.speedD)));
Vector3 velocity = dcm.transposed() * (new Vector3((sitldata.speedN), (sitldata.speedE), (sitldata.speedD)));
//Console.WriteLine("vel " + velocity.ToString());
//Console.WriteLine("ved " + velocitydelta.ToString());
// a = dv / dt
// 50 hz = 0.02sec
Vector3 accel_mine_body = dcm.transposed() * (new Vector3((sitldata_old.speedN - sitldata.speedN) / 0.02, (sitldata_old.speedE - sitldata.speedE) / 0.02, (sitldata_old.speedD - sitldata.speedD) / 0.02));
// Console.WriteLine("G"+accel_body.ToString());
//Console.WriteLine("M"+accel_mine_body.ToString());
// sitldata.pitchRate = 0;
// sitldata.rollRate = 0;
// sitldata.yawRate = 0;
//accel_mine_body.x =0;// *= -1;
//accel_mine_body.y =0;//*= -1;
// accel_body -= accel_mine_body;
sitldata.xAccel = accel_body.x;// DATA[4][5] * 1;
sitldata.yAccel = accel_body.y;// DATA[4][6] * 1;
sitldata.zAccel = accel_body.z;// (0 - DATA[4][4]) * 9.808;
sitldata.xAccel = DATA[4][5] *9.808;
sitldata.yAccel = DATA[4][6] *9.808;
sitldata.zAccel = -DATA[4][4] *9.808;
// Console.WriteLine(accel_body.ToString());
// Console.WriteLine(" {0} {1} {2}",sitldata.xAccel, sitldata.yAccel, sitldata.zAccel);
}
else if (receviedbytes == 0x64) // FG binary udp
{
//FlightGear
/*
fgIMUData imudata2 = data.ByteArrayToStructureBigEndian<fgIMUData>(0);
if (imudata2.magic != 0x4c56414d)
return;
if (imudata2.latitude == 0)
return;
chkSensor.Checked = true;
imu.time_usec = ((ulong)DateTime.Now.ToBinary());
imu.xacc = ((Int16)(imudata2.accelX * 9808 / 32.2));
imu.xgyro = ((Int16)(imudata2.rateRoll * 17.453293));
imu.xmag = 0;
imu.yacc = ((Int16)(imudata2.accelY * 9808 / 32.2));
imu.ygyro = ((Int16)(imudata2.ratePitch * 17.453293));
imu.ymag = 0;
imu.zacc = ((Int16)(imudata2.accelZ * 9808 / 32.2)); // + 1000
imu.zgyro = ((Int16)(imudata2.rateYaw * 17.453293));
imu.zmag = 0;
gps.alt = ((int)(imudata2.altitude * ft2m * 1000));
gps.fix_type = 3;
gps.cog = (ushort)(Math.Atan2(imudata2.velocityE, imudata2.velocityN) * rad2deg * 100);
gps.lat = (int)(imudata2.latitude * 1.0e7);
gps.lon = (int)(imudata2.longitude * 1.0e7);
gps.time_usec = ((ulong)DateTime.Now.Ticks);
gps.vel = (ushort)(Math.Sqrt((imudata2.velocityN * imudata2.velocityN) + (imudata2.velocityE * imudata2.velocityE)) * ft2m * 100);
//FileStream stream = File.OpenWrite("fgdata.txt");
//stream.Write(data, 0, receviedbytes);
//stream.Close();
*/
}
else if (receviedbytes == 662 || receviedbytes == 658) // 658 = 3.83 662 = 3.91
{
/*
// Model data in body frame coordinates (X=Right, Y=Front, Z=Up)
public float Model_fVel_Body_X; public float Model_fVel_Body_Y; public float Model_fVel_Body_Z; // m/s Model velocity in body coordinates
public float Model_fAngVel_Body_X; public float Model_fAngVel_Body_Y; public float Model_fAngVel_Body_Z; // rad/s Model angular velocity in body coordinates
public float Model_fAccel_Body_X; public float Model_fAccel_Body_Y; public float Model_fAccel_Body_Z; // m/s/s Model acceleration in body coordinates
*/
TDataFromAeroSimRC aeroin_last = aeroin;
aeroin = data.ByteArrayToStructure<TDataFromAeroSimRC>(0);
sitldata.pitchDeg = (aeroin.Model_fPitch * rad2deg);
sitldata.rollDeg = (aeroin.Model_fRoll * -1 * rad2deg);
sitldata.yawDeg = ((aeroin.Model_fHeading * rad2deg));
sitldata.pitchRate =aeroin.Model_fAngVel_Body_X * rad2deg;
sitldata.rollRate = aeroin.Model_fAngVel_Body_Y * rad2deg;
sitldata.yawRate = -aeroin.Model_fAngVel_Body_Z * rad2deg;
// calc gravity
Matrix3 dcm = new Matrix3();
dcm.from_euler(sitldata.rollDeg * deg2rad, sitldata.pitchDeg * deg2rad, sitldata.yawDeg * deg2rad);
Vector3 accel_body = dcm.transposed() * (new Vector3(0, 0, -9.8)); // -9.8
sitldata.xAccel = aeroin.Model_fAccel_Body_Y / 9.808 + accel_body.x;// pitch - back forward-
sitldata.yAccel = aeroin.Model_fAccel_Body_X / 9.808 + accel_body.y; // roll - left right-
sitldata.zAccel = -aeroin.Model_fAccel_Body_Z /9.808 + accel_body.z;
// Console.WriteLine("2 {0,20} {1,20} {2,20}", aeroin.Model_fAccel_Body_X.ToString("0.000"), aeroin.Model_fAccel_Body_Y.ToString("0.000"), aeroin.Model_fAccel_Body_Z.ToString("0.000"));
sitldata.altitude = aeroin.Model_fPosZ;
sitldata.latitude = aeroin.Model_fLatitude;
sitldata.longitude = aeroin.Model_fLongitude;
sitldata.speedN = aeroin.Model_fVelY;
sitldata.speedE = aeroin.Model_fVelX;
sitldata.speedD = aeroin.Model_fVelZ;
float xvec = aeroin.Model_fVelY - aeroin.Model_fWindVelY;
float yvec = aeroin.Model_fVelX - aeroin.Model_fWindVelX;
sitldata.airspeed = ((float)Math.Sqrt((yvec * yvec) + (xvec * xvec)));
}
else if (receviedbytes == 408)
{
FGNetFDM fdm = data.ByteArrayToStructureBigEndian<FGNetFDM>(0);
lastfdmdata = fdm;
sitldata.altitude = (fdm.altitude);
sitldata.latitude = (fdm.latitude * rad2deg);
sitldata.longitude = (fdm.longitude * rad2deg);
sitldata.rollDeg = fdm.phi * rad2deg;
sitldata.pitchDeg = fdm.theta * rad2deg;
sitldata.yawDeg = fdm.psi * rad2deg;
sitldata.rollRate = fdm.phidot * rad2deg;
sitldata.pitchRate = fdm.thetadot * rad2deg;
sitldata.yawRate = fdm.psidot * rad2deg;
sitldata.speedN = fdm.v_north * ft2m;
sitldata.speedE = fdm.v_east * ft2m;
sitldata.speedD = fdm.v_down * ft2m;
sitldata.xAccel = (fdm.A_X_pilot * 9.808 / 32.2); // pitch
sitldata.yAccel = (fdm.A_Y_pilot * 9.808 / 32.2); // roll
sitldata.zAccel = (fdm.A_Z_pilot / 32.2 * 9.808);
sitldata.airspeed = fdm.vcas * 0.5144444f;// knots to m/s
// Console.WriteLine("1 {0} {1} {2} {3}",(float)sitldata.rollDeg,MainV2.comPort.MAV.cs.roll,sitldata.pitchDeg,MainV2.comPort.MAV.cs.pitch);
if (RAD_JSBSim.Checked)
sitldata.airspeed = fdm.vcas * ft2m;// fps to m/s
}
else
{
log.Info("Bad Udp Packet " + receviedbytes);
return;
}
if (sitldata.altitude < 0)
sitldata.altitude = 0.00001;
sitldata_old = sitldata;
// write arduimu to ardupilot
if (CHK_quad.Checked && !RAD_aerosimrc.Checked) // quad does its own
{
return;
}
if (RAD_JSBSim.Checked && chkSITL.Checked)
{
byte[] buffer = new byte[1500];
while (JSBSimSEND.Client.Available > 5)
{
int read = JSBSimSEND.Client.Receive(buffer);
// Console.WriteLine(ASCIIEncoding.ASCII.GetString(buffer,0,read));
}
sitldata.magic = (int)0x4c56414f;
byte[] sendme = StructureToByteArray(sitldata);
SITLSEND.Send(sendme, sendme.Length);
return;
}
if (chkSITL.Checked)
{
sitldata.magic = (int)0x4c56414f;
byte[] sendme = StructureToByteArray(sitldata);
SITLSEND.Send(sendme, sendme.Length);
return;
}
TimeSpan gpsspan = DateTime.Now - lastgpsupdate;
// add gps delay
if (gpsspan.TotalMilliseconds >= GPS_rate)
{
lastgpsupdate = DateTime.Now;
// save current fix = 3
sitl_fdmbuffer[gpsbufferindex % sitl_fdmbuffer.Length] = sitldata;
// Console.WriteLine((gpsbufferindex % gpsbuffer.Length) + " " + ((gpsbufferindex + (gpsbuffer.Length - 1)) % gpsbuffer.Length));
// return buffer index + 5 = (3 + 5) = 8 % 6 = 2
oldgps = sitl_fdmbuffer[(gpsbufferindex + (sitl_fdmbuffer.Length - 1)) % sitl_fdmbuffer.Length];
//comPort.sendPacket(oldgps);
gpsbufferindex++;
}
MAVLink.mavlink_hil_state_t hilstate = new MAVLink.mavlink_hil_state_t();
DateTime epochBegin = new DateTime(1980, 1, 6, 0, 0, 0, DateTimeKind.Utc);
hilstate.time_usec = (UInt64)((DateTime.Now.Ticks - epochBegin.Ticks) / 10); // microsec
hilstate.lat = (int)(oldgps.latitude * 1e7); // * 1E7
hilstate.lon = (int)(oldgps.longitude * 1e7); // * 1E7
hilstate.alt = (int)(oldgps.altitude * 1000); // mm
// Console.WriteLine(hilstate.alt);
// Console.WriteLine("{0} {1} {2}", sitldata.rollDeg.ToString("0.0"), sitldata.pitchDeg.ToString("0.0"), sitldata.yawDeg.ToString("0.0"));
hilstate.pitch = (float)sitldata.pitchDeg * deg2rad; // (rad)
hilstate.pitchspeed = (float)sitldata.pitchRate * deg2rad; // (rad/s)
hilstate.roll = (float)sitldata.rollDeg * deg2rad; // (rad)
hilstate.rollspeed = (float)sitldata.rollRate * deg2rad; // (rad/s)
hilstate.yaw = (float)sitldata.yawDeg * deg2rad; // (rad)
hilstate.yawspeed = (float)sitldata.yawRate * deg2rad; // (rad/s)
hilstate.vx = (short)(sitldata.speedN * 100); // m/s * 100 - lat
hilstate.vy = (short)(sitldata.speedE * 100); // m/s * 100 - long
hilstate.vz = (short)(sitldata.speedD * 100); // m/s * 100 - + speed down
hilstate.xacc = (short)(sitldata.xAccel * 101.957); // (mg)
hilstate.yacc = (short)(sitldata.yAccel * 101.957); // (mg)
hilstate.zacc = (short)(sitldata.zAccel * 101.957); // (mg)
packetcount++;
if (!comPort.BaseStream.IsOpen)
return;
if (comPort.BaseStream.BytesToWrite > 100)
return;
// if (packetcount % 2 == 0)
// return;
comPort.sendPacket(hilstate);
// comPort.sendPacket(oldgps);
//comPort.sendPacket(new MAVLink.mavlink_vfr_hud_t() { airspeed = (float)sitldata.airspeed } );
MAVLink.mavlink_raw_pressure_t pres = new MAVLink.mavlink_raw_pressure_t();
double calc = (101325 * Math.Pow(1 - 2.25577 * Math.Pow(10, -5) * sitldata.altitude, 5.25588)); // updated from valid gps
pres.press_diff1 = (short)(int)(calc - 101325); // 0 alt is 0 pa
// comPort.sendPacket(pres);
}
/*
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, MAVLinkInterface mavinterface)
{
lock (this)
{
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 && armed)
{
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[(byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT] != null) // status text
{
var msg = mavinterface.MAV.packets[(byte)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[(byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT], 6, mavinterface.MAV.packets[(byte)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;
messageHighTime = DateTime.Now;
}
try
{
while (messages.Count > 50)
{
messages.RemoveAt(0);
}
messages.Add(logdata + "\n");
}
catch { }
mavinterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT] = null;
}
byte[] bytearray = mavinterface.MAV.packets[(byte)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("RC_CHANNELS_SCALED Packet");
mavinterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.RC_CHANNELS_SCALED] = null;
}
bytearray = mavinterface.MAV.packets[(byte)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
messageHigh = "Fence Breach";
messageHighTime = DateTime.Now;
}
mavinterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.FENCE_STATUS] = null;
}
bytearray = mavinterface.MAV.packets[(byte)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[(byte)MAVLink.MSG_NAMES.HIL_CONTROLS] = null;
}
bytearray = mavinterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SYSTEM_TIME];
if (bytearray != null)
{
var systime = bytearray.ByteArrayToStructure<MAVLink.mavlink_system_time_t>(6);
DateTime newtime = DateTime.Now;
//UInt64 ms_per_week = 7000ULL*86400ULL;
//UInt64 unix_offset = 17000ULL*86400ULL + 52*10*7000ULL*86400ULL - 15000ULL;
//UInt64 fix_time_ms = unix_offset + time_week*ms_per_week + time_week_ms;
}
bytearray = mavinterface.MAV.packets[(byte)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[(byte)MAVLink.MSG_NAMES.HWSTATUS] = null;
}
bytearray = mavinterface.MAV.packets[(byte)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[(byte)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;
}
bytearray = mavinterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.HEARTBEAT];
if (bytearray != null)
{
var hb = bytearray.ByteArrayToStructure<MAVLink.mavlink_heartbeat_t>(6);
if (hb.type == (byte)MAVLink.MAV_TYPE.GCS)
{
// skip gcs hb's
// only happens on log playback - and shouldnt get them here
}
else
{
armed = (hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) == (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED;
// for future use
bool landed = hb.system_status == (byte)MAVLink.MAV_STATE.STANDBY;
failsafe = hb.system_status == (byte)MAVLink.MAV_STATE.CRITICAL;
string oldmode = mode;
if ((hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED) != 0)
{
// prevent running thsi unless we have to
if (_mode != hb.custom_mode)
{
List<KeyValuePair<int, string>> modelist = Common.getModesList(this);
bool found = false;
foreach (KeyValuePair<int, string> pair in modelist)
{
if (pair.Key == hb.custom_mode)
{
mode = pair.Value.ToString();
_mode = hb.custom_mode;
found = true;
break;
}
}
if (!found)
{
log.Warn("Mode not found bm:" + hb.base_mode + " cm:" + hb.custom_mode);
_mode = hb.custom_mode;
}
}
}
if (oldmode != mode && MainV2.speechEnable && MainV2.getConfig("speechmodeenabled") == "True")
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode")));
}
}
}
bytearray = mavinterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SYS_STATUS];
if (bytearray != null)
{
var sysstatus = bytearray.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6);
load = (float)sysstatus.load / 10.0f;
battery_voltage = (float)sysstatus.voltage_battery / 1000.0f;
battery_remaining = (float)sysstatus.battery_remaining;
current = (float)sysstatus.current_battery / 100.0f;
packetdropremote = sysstatus.drop_rate_comm;
Mavlink_Sensors sensors_enabled = new Mavlink_Sensors(sysstatus.onboard_control_sensors_enabled);
Mavlink_Sensors sensors_health = new Mavlink_Sensors(sysstatus.onboard_control_sensors_health);
Mavlink_Sensors sensors_present = new Mavlink_Sensors(sysstatus.onboard_control_sensors_present);
if (sensors_health.gps != sensors_enabled.gps)
{
messageHigh = "Bad GPS Health";
messageHighTime = DateTime.Now;
}
else if (sensors_health.gyro != sensors_enabled.gyro)
{
messageHigh = "Bad Gyro Health";
messageHighTime = DateTime.Now;
}
else if (sensors_health.accelerometer != sensors_enabled.accelerometer)
{
messageHigh = "Bad Accel Health";
messageHighTime = DateTime.Now;
}
else if (sensors_health.compass != sensors_enabled.compass)
{
messageHigh = "Bad Compass Health";
messageHighTime = DateTime.Now;
}
else if (sensors_health.barometer != sensors_enabled.barometer)
{
messageHigh = "Bad Baro Health";
messageHighTime = DateTime.Now;
}
else if (sensors_health.optical_flow != sensors_enabled.optical_flow)
{
messageHigh = "Bad OptFlow Health";
messageHighTime = DateTime.Now;
}
else if (sensors_present.rc_receiver != sensors_enabled.rc_receiver)
{
messageHigh = "NO RC Receiver";
messageHighTime = DateTime.Now;
}
mavinterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SYS_STATUS] = null;
}
bytearray = mavinterface.MAV.packets[(byte)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[(byte)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[(byte)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[(byte)MAVLink.MSG_NAMES.ATTITUDE] = null;
}
bytearray = mavinterface.MAV.packets[(byte)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
}
altasl = gps.alt / 1000.0f;
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[(byte)MAVLink.MSG_NAMES.GPS_RAW] = null;
}
bytearray = mavinterface.MAV.packets[(byte)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[(byte)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.@fixed;
}
bytearray = mavinterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.RADIO_STATUS];
if (bytearray != null)
{
var radio = bytearray.ByteArrayToStructure<MAVLink.mavlink_radio_status_t>(6);
rssi = radio.rssi;
remrssi = radio.remrssi;
txbuffer = radio.txbuf;
rxerrors = radio.rxerrors;
noise = radio.noise;
remnoise = radio.remnoise;
fixedp = radio.@fixed;
}
bytearray = mavinterface.MAV.packets[(byte)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
alt = loc.relative_alt / 1000.0f;
useLocation = true;
if (loc.lat == 0 && loc.lon == 0)
{
useLocation = false;
}
else
{
lat = loc.lat / 10000000.0f;
lng = loc.lon / 10000000.0f;
}
}
bytearray = mavinterface.MAV.packets[(byte)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[(byte)MAVLink.MSG_NAMES.WAYPOINT_CURRENT] = null;
}
bytearray = mavinterface.MAV.packets[(byte)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[(byte)MAVLink.MSG_NAMES.NAV_CONTROLLER_OUTPUT] = null;
}
bytearray = mavinterface.MAV.packets[(byte)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[(byte)MAVLink.MSG_NAMES.RC_CHANNELS_RAW] = null;
}
bytearray = mavinterface.MAV.packets[(byte)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;
mavinterface.MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SERVO_OUTPUT_RAW] = null;
}
bytearray = mavinterface.MAV.packets[(byte)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[(byte)MAVLink.MSG_NAMES.RAW_IMU] = null;
}
bytearray = mavinterface.MAV.packets[(byte)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[(byte)MAVLink.MSG_NAMES.RAW_IMU] = null;
}
bytearray = mavinterface.MAV.packets[(byte)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[(byte)MAVLink.MSG_NAMES.VFR_HUD] = null;
}
bytearray = mavinterface.MAV.packets[(byte)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)
{
if (bs.Count > 200)
{
while (bs.Count > 3)
bs.RemoveAt(1);
//bs.Clear();
}
bs.Add(this);
return;
bs.DataSource = this;
bs.ResetBindings(false);
return;
hires.Stopwatch sw = new hires.Stopwatch();
sw.Start();
bs.DataSource = this;
bs.ResetBindings(false);
sw.Stop();
var elaps = sw.Elapsed;
Console.WriteLine("1 " + elaps.ToString("0.#####") + " done ");
sw.Start();
bs.SuspendBinding();
bs.Clear();
bs.ResumeBinding();
bs.Add(this);
sw.Stop();
elaps = sw.Elapsed;
Console.WriteLine("2 " + elaps.ToString("0.#####") + " done ");
sw.Start();
if (bs.Count > 100)
bs.Clear();
bs.Add(this);
sw.Stop();
elaps = sw.Elapsed;
Console.WriteLine("3 " + elaps.ToString("0.#####") + " done ");
}
}
catch { log.InfoFormat("CurrentState Binding error"); }
}
}