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;
}
}
MAVLink com2 = new MAVLink();
com2.BaseStream.PortName = Comms.CommsSerialScan.portinterface.PortName;
com2.BaseStream.BaudRate = Comms.CommsSerialScan.portinterface.BaudRate;
com2.Open(true);
MainV2.Comports.Add(com2);
CMB_mavs.DataSource = MainV2.Comports;
}
/*
public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow)
{
UpdateCurrentSettings(bs, false, MainV2.comPort);
}
*/
public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow, MAVLink mavinterface)
{
lock (locker)
{
if (DateTime.Now > lastupdate.AddMilliseconds(50) || updatenow) // 20 hz
{
lastupdate = DateTime.Now;
//check if valid mavinterface
if (mavinterface != null && mavinterface.packetsnotlost != 0)
linkqualitygcs = (ushort)((mavinterface.packetsnotlost / (mavinterface.packetsnotlost + mavinterface.packetslost)) * 100.0);
if (DateTime.Now.Second != lastsecondcounter.Second)
{
lastsecondcounter = DateTime.Now;
if (lastpos.Lat != 0 && lastpos.Lng != 0)
{
if (!MainV2.comPort.BaseStream.IsOpen && !MainV2.comPort.logreadmode)
distTraveled = 0;
distTraveled += (float)lastpos.GetDistance(new PointLatLngAlt(lat, lng, 0, "")) * multiplierdist;
lastpos = new PointLatLngAlt(lat, lng, 0, "");
}
else
{
lastpos = new PointLatLngAlt(lat, lng, 0, "");
}
// throttle is up, or groundspeed is > 3 m/s
if (ch3percent > 12 || _groundspeed > 3.0)
timeInAir++;
if (!gotwind)
dowindcalc();
}
if (mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] != null) // status text
{
var msg = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT].ByteArrayToStructure<MAVLink.mavlink_statustext_t>(6);
/*
enum gcs_severity {
SEVERITY_LOW=1,
SEVERITY_MEDIUM,
SEVERITY_HIGH,
SEVERITY_CRITICAL
};
*/
byte sev = msg.severity;
string logdata = Encoding.ASCII.GetString(mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT], 6, mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT].Length - 6);
int ind = logdata.IndexOf('\0');
if (ind != -1)
logdata = logdata.Substring(0, ind);
if (sev >= 3)
{
messageHigh = logdata;
}
try
{
while (messages.Count > 50)
{
messages.RemoveAt(0);
}
messages.Add(logdata + "\n");
}
catch { }
mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] = null;
}
byte[] bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED];
if (bytearray != null) // hil mavlink 0.9
{
var hil = bytearray.ByteArrayToStructure<MAVLink.mavlink_rc_channels_scaled_t>(6);
hilch1 = hil.chan1_scaled;
hilch2 = hil.chan2_scaled;
hilch3 = hil.chan3_scaled;
hilch4 = hil.chan4_scaled;
hilch5 = hil.chan5_scaled;
hilch6 = hil.chan6_scaled;
hilch7 = hil.chan7_scaled;
hilch8 = hil.chan8_scaled;
// Console.WriteLine("MAVLINK_MSG_ID_RC_CHANNELS_SCALED Packet");
mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_FENCE_STATUS];
if (bytearray != null)
{
var fence = bytearray.ByteArrayToStructure<MAVLink.mavlink_fence_status_t>(6);
if (fence.breach_status != (byte)MAVLink.FENCE_BREACH.NONE)
{
// fence breached
}
mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_FENCE_STATUS] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_HIL_CONTROLS];
if (bytearray != null) // hil mavlink 0.9 and 1.0
{
var hil = bytearray.ByteArrayToStructure<MAVLink.mavlink_hil_controls_t>(6);
hilch1 = (int)(hil.roll_ailerons * 10000);
hilch2 = (int)(hil.pitch_elevator * 10000);
hilch3 = (int)(hil.throttle * 10000);
hilch4 = (int)(hil.yaw_rudder * 10000);
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_HIL_CONTROLS] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_HWSTATUS];
if (bytearray != null)
{
var hwstatus = bytearray.ByteArrayToStructure<MAVLink.mavlink_hwstatus_t>(6);
hwvoltage = hwstatus.Vcc / 1000.0f;
i2cerrors = hwstatus.I2Cerr;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_HWSTATUS] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_RANGEFINDER];
if (bytearray != null)
{
var sonar = bytearray.ByteArrayToStructure<MAVLink.mavlink_rangefinder_t>(6);
sonarrange = sonar.distance;
sonarvoltage = sonar.voltage;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_WIND];
if (bytearray != null)
{
var wind = bytearray.ByteArrayToStructure<MAVLink.mavlink_wind_t>(6);
gotwind = true;
wind_dir = (wind.direction + 360) % 360;
wind_vel = wind.speed * multiplierspeed;
//MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_HEARTBEAT];
if (bytearray != null)
{
var hb = bytearray.ByteArrayToStructure<MAVLink.mavlink_heartbeat_t>(6);
armed = (hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) == (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED;
failsafe = hb.system_status == (byte)MAVLink.MAV_STATE.CRITICAL;
string oldmode = mode;
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();
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);
}
}
}
if (oldmode != mode && MainV2.speechEnable && MainV2.getConfig("speechmodeenabled") == "True")
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode")));
}
}
bytearray = mavinterface.MAV.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS];
if (bytearray != null)
{
var sysstatus = bytearray.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6);
battery_voltage = (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.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_SCALED_PRESSURE];
if (bytearray != null)
{
var pres = bytearray.ByteArrayToStructure<MAVLink.mavlink_scaled_pressure_t>(6);
press_abs = pres.press_abs;
press_temp = pres.temperature;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_SENSOR_OFFSETS];
if (bytearray != null)
{
var sensofs = bytearray.ByteArrayToStructure<MAVLink.mavlink_sensor_offsets_t>(6);
mag_ofs_x = sensofs.mag_ofs_x;
mag_ofs_y = sensofs.mag_ofs_y;
mag_ofs_z = sensofs.mag_ofs_z;
mag_declination = sensofs.mag_declination;
raw_press = sensofs.raw_press;
raw_temp = sensofs.raw_temp;
gyro_cal_x = sensofs.gyro_cal_x;
gyro_cal_y = sensofs.gyro_cal_y;
gyro_cal_z = sensofs.gyro_cal_z;
accel_cal_x = sensofs.accel_cal_x;
accel_cal_y = sensofs.accel_cal_y;
accel_cal_z = sensofs.accel_cal_z;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE];
if (bytearray != null)
{
var att = bytearray.ByteArrayToStructure<MAVLink.mavlink_attitude_t>(6);
roll = att.roll * rad2deg;
pitch = att.pitch * rad2deg;
yaw = att.yaw * rad2deg;
// Console.WriteLine(roll + " " + pitch + " " + yaw);
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT];
if (bytearray != null)
{
var gps = bytearray.ByteArrayToStructure<MAVLink.mavlink_gps_raw_int_t>(6);
if (!useLocation)
{
lat = gps.lat * 1.0e-7f;
lng = gps.lon * 1.0e-7f;
// alt = gps.alt; // using vfr as includes baro calc
}
gpsstatus = gps.fix_type;
// Console.WriteLine("gpsfix {0}",gpsstatus);
gpshdop = (float)Math.Round((double)gps.eph / 100.0,2);
satcount = gps.satellites_visible;
groundspeed = gps.vel * 1.0e-2f;
groundcourse = gps.cog * 1.0e-2f;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_GPS_STATUS];
if (bytearray != null)
{
var gps = bytearray.ByteArrayToStructure<MAVLink.mavlink_gps_status_t>(6);
satcount = gps.satellites_visible;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_RADIO];
if (bytearray != null)
{
var radio = bytearray.ByteArrayToStructure<MAVLink.mavlink_radio_t>(6);
rssi = radio.rssi;
remrssi = radio.remrssi;
txbuffer = radio.txbuf;
rxerrors = radio.rxerrors;
noise = radio.noise;
remnoise = radio.remnoise;
fixedp = radio.fixedp;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT];
if (bytearray != null)
{
var loc = bytearray.ByteArrayToStructure<MAVLink.mavlink_global_position_int_t>(6);
// the new arhs deadreckoning may send 0 alt and 0 long. check for and undo
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[MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT];
if (bytearray != null)
{
var wpcur = bytearray.ByteArrayToStructure<MAVLink.mavlink_mission_current_t>(6);
int oldwp = (int)wpno;
wpno = wpcur.seq;
if (oldwp != wpno && MainV2.speechEnable && MainV2.getConfig("speechwaypointenabled") == "True")
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint")));
}
//MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT];
if (bytearray != null)
{
var nav = bytearray.ByteArrayToStructure<MAVLink.mavlink_nav_controller_output_t>(6);
nav_roll = nav.nav_roll;
nav_pitch = nav.nav_pitch;
nav_bearing = nav.nav_bearing;
target_bearing = nav.target_bearing;
wp_dist = nav.wp_dist;
alt_error = nav.alt_error;
aspd_error = nav.aspd_error / 100.0f;
xtrack_error = nav.xtrack_error;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW];
if (bytearray != null)
{
var rcin = bytearray.ByteArrayToStructure<MAVLink.mavlink_rc_channels_raw_t>(6);
ch1in = rcin.chan1_raw;
ch2in = rcin.chan2_raw;
ch3in = rcin.chan3_raw;
ch4in = rcin.chan4_raw;
ch5in = rcin.chan5_raw;
ch6in = rcin.chan6_raw;
ch7in = rcin.chan7_raw;
ch8in = rcin.chan8_raw;
//percent
rxrssi = (float)((rcin.rssi / 255.0) * 100.0);
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW];
if (bytearray != null)
{
var servoout = bytearray.ByteArrayToStructure<MAVLink.mavlink_servo_output_raw_t>(6);
ch1out = servoout.servo1_raw;
ch2out = servoout.servo2_raw;
ch3out = servoout.servo3_raw;
ch4out = servoout.servo4_raw;
ch5out = servoout.servo5_raw;
ch6out = servoout.servo6_raw;
ch7out = servoout.servo7_raw;
ch8out = servoout.servo8_raw;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU];
if (bytearray != null)
{
var imu = bytearray.ByteArrayToStructure<MAVLink.mavlink_raw_imu_t>(6);
gx = imu.xgyro;
gy = imu.ygyro;
gz = imu.zgyro;
ax = imu.xacc;
ay = imu.yacc;
az = imu.zacc;
mx = imu.xmag;
my = imu.ymag;
mz = imu.zmag;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_SCALED_IMU];
if (bytearray != null)
{
var imu = bytearray.ByteArrayToStructure<MAVLink.mavlink_scaled_imu_t>(6);
gx = imu.xgyro;
gy = imu.ygyro;
gz = imu.zgyro;
ax = imu.xacc;
ay = imu.yacc;
az = imu.zacc;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD];
if (bytearray != null)
{
var vfr = bytearray.ByteArrayToStructure<MAVLink.mavlink_vfr_hud_t>(6);
//groundspeed = vfr.groundspeed;
airspeed = vfr.airspeed;
// alt = vfr.alt; // this might include baro
ch3percent = vfr.throttle;
//Console.WriteLine(alt);
//climbrate = vfr.climb;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_MEMINFO];
if (bytearray != null)
{
var mem = bytearray.ByteArrayToStructure<MAVLink.mavlink_meminfo_t>(6);
freemem = mem.freemem;
brklevel = mem.brkval;
}
}
//Console.Write(DateTime.Now.Millisecond + " start ");
// update form
try
{
if (bs != null)
{
//System.Diagnostics.Debug.WriteLine(DateTime.Now.Millisecond);
// Console.Write(" "+DateTime.Now.Millisecond);
//bs.DataSource = this;
// Console.Write(" " + DateTime.Now.Millisecond + " 1 " + updatenow + " " + System.Threading.Thread.CurrentThread.Name);
//bs.ResetBindings(false);
//bs.ResetCurrentItem();
// mono workaround - this is alot faster
bs.Add(this);
// Console.WriteLine(" " + DateTime.Now.Millisecond + " done ");
}
}
catch { log.InfoFormat("CurrentState Binding error"); }
}
}
/// <summary>
/// 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;
bool armedstatus = false;
DateTime speechcustomtime = DateTime.Now;
DateTime speechbatterytime = DateTime.Now;
DateTime linkqualitytime = DateTime.Now;
while (serialThread)
{
try
{
Thread.Sleep(1); // was 5
// 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.getConfig("speechcustomenabled") == "True")
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechcustom")));
}
speechcustomtime = DateTime.Now;
}
// speech for battery alerts
if (speechEnable && speechEngine != null && (DateTime.Now - speechbatterytime).TotalSeconds > 10 && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen))
{
//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 != 0.0)
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechbattery")));
speechbatterytime = DateTime.Now;
}
else if (MainV2.getConfig("speechbatteryenabled") == "True" && (MainV2.comPort.MAV.cs.battery_remaining) < warnpercent && MainV2.comPort.MAV.cs.battery_voltage != 0.0 && MainV2.comPort.MAV.cs.battery_remaining != 0.0)
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechbattery")));
speechbatterytime = DateTime.Now;
}
}
// speech altitude warning.
if (speechEnable && speechEngine != null && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen))
{
float warnalt = float.MaxValue;
float.TryParse(MainV2.getConfig("speechaltheight"), out warnalt);
try
{
if (MainV2.getConfig("speechaltenabled") == "True" && MainV2.comPort.MAV.cs.alt != 0.00 && (MainV2.comPort.MAV.cs.alt - (int)double.Parse(MainV2.getConfig("TXT_homealt"))) <= warnalt)
{
if (MainV2.speechEngine.State == SynthesizerState.Ready)
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechalt")));
}
}
catch { } // silent fail
}
// 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);
continue;
}
// make sure we attenuate the link quality if we dont see any valid packets
if ((DateTime.Now - comPort.lastvalidpacket).TotalSeconds > 10)
{
MainV2.comPort.MAV.cs.linkqualitygcs = 0;
}
// attenuate the link qualty over time
if ((DateTime.Now - comPort.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();
}
}
// send a hb every seconds from gcs to ap
if (heatbeatSend.Second != DateTime.Now.Second && comPort.giveComport == false)
{
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,
};
comPort.sendPacket(htb);
foreach (var port in MainV2.Comports)
{
if (port == MainV2.comPort)
continue;
try
{
port.sendPacket(htb);
}
catch { }
}
heatbeatSend = DateTime.Now;
}
// data loss warning - wait min of 10 seconds, ignore first 30 seconds of connect, repeat at 5 seconds interval
if ((DateTime.Now - comPort.lastvalidpacket).TotalSeconds > 10 && (DateTime.Now - connecttime).TotalSeconds > 30 && (DateTime.Now - nodatawarning).TotalSeconds > 5)
{
if (speechEnable && speechEngine != null)
{
if (MainV2.speechEngine.State == SynthesizerState.Ready)
{
MainV2.speechEngine.SpeakAsync("WARNING No Data for " + (int)(DateTime.Now - comPort.lastvalidpacket).TotalSeconds + " Seconds");
nodatawarning = DateTime.Now;
}
}
}
// get home point on armed status change.
if (armedstatus != MainV2.comPort.MAV.cs.armed)
{
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")));
}
}
}
// actualy read the packets
while (comPort.BaseStream.BytesToRead > minbytes && comPort.giveComport == false)
{
try
{
comPort.readPacket();
}
catch { }
}
// update currentstate of main port
try
{
comPort.MAV.cs.UpdateCurrentSettings(null, false, comPort);
}
catch { }
// read the other interfaces
foreach (var port in Comports)
{
if (!port.BaseStream.IsOpen)
{
// modify array and drop out
Comports.Remove(port);
break;
}
// skip primary interface
if (port == comPort)
continue;
while (port.BaseStream.BytesToRead > minbytes)
{
try
{
port.readPacket();
}
catch { }
}
// update currentstate of port
try
{
port.MAV.cs.UpdateCurrentSettings(null, false, port);
}
catch { }
}
}
catch (Exception e)
{
log.Error("Serial Reader fail :" + e.ToString());
try
{
comPort.Close();
}
catch { }
}
}
SerialThreadrunner.Set();
}
/*
public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow)
{
UpdateCurrentSettings(bs, false, MainV2.comPort);
}
*/
public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow, MAVLink mavinterface)
{
if (DateTime.Now > lastupdate.AddMilliseconds(19) || updatenow) // 50 hz
{
lastupdate = DateTime.Now;
if (DateTime.Now.Second != lastwindcalc.Second)
{
lastwindcalc = DateTime.Now;
dowindcalc();
}
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] != null) // status text
{
string logdata = DateTime.Now + " " + Encoding.ASCII.GetString(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT], 6, mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT].Length - 6);
int ind = logdata.IndexOf('\0');
if (ind != -1)
logdata = logdata.Substring(0, ind);
if (messages.Count > 5)
{
messages.RemoveAt(0);
}
messages.Add(logdata + "\n");
mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] = null;
}
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] != null) // hil
{
var hil = new ArdupilotMega.MAVLink.__mavlink_rc_channels_scaled_t();
object temp = hil;
MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED], ref temp, 6);
hil = (MAVLink.__mavlink_rc_channels_scaled_t)(temp);
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;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] = null;
}
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT] != null)
{
MAVLink.__mavlink_nav_controller_output_t nav = new MAVLink.__mavlink_nav_controller_output_t();
object temp = nav;
MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT], ref temp, 6);
nav = (MAVLink.__mavlink_nav_controller_output_t)(temp);
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;
xtrack_error = nav.xtrack_error;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT] = null;
}
#if MAVLINK10
if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_HEARTBEAT] != null)
{
ArdupilotMega.MAVLink.__mavlink_heartbeat_t hb = new ArdupilotMega.MAVLink.__mavlink_heartbeat_t();
object temp = hb;
MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_HEARTBEAT], ref temp, 6);
hb = (ArdupilotMega.MAVLink.__mavlink_heartbeat_t)(temp);
string oldmode = mode;
mode = "Unknown";
if ((hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) != 0)
{
if (hb.type == (byte)MAVLink.MAV_TYPE.MAV_TYPE_FIXED_WING)
{
switch (hb.custom_mode)
{
case (byte)(Common.apmmodes.MANUAL):
mode = "Manual";
break;
case (byte)(Common.apmmodes.GUIDED):
mode = "Guided";
break;
case (byte)(Common.apmmodes.STABILIZE):
mode = "Stabilize";
break;
case (byte)(Common.apmmodes.FLY_BY_WIRE_A):
mode = "FBW A";
break;
case (byte)(Common.apmmodes.FLY_BY_WIRE_B):
mode = "FBW B";
break;
case (byte)(Common.apmmodes.AUTO):
mode = "Auto";
break;
case (byte)(Common.apmmodes.RTL):
mode = "RTL";
break;
case (byte)(Common.apmmodes.LOITER):
mode = "Loiter";
break;
case (byte)(Common.apmmodes.CIRCLE):
mode = "Circle";
break;
default:
mode = "Unknown";
break;
}
}
else if (hb.type == (byte)MAVLink.MAV_TYPE.MAV_TYPE_QUADROTOR)
{
switch (hb.custom_mode)
{
case (byte)(Common.ac2modes.STABILIZE):
mode = "Stabilize";
break;
case (byte)(Common.ac2modes.ACRO):
mode = "Acro";
break;
case (byte)(Common.ac2modes.ALT_HOLD):
mode = "Alt Hold";
break;
case (byte)(Common.ac2modes.AUTO):
mode = "Auto";
break;
case (byte)(Common.ac2modes.GUIDED):
mode = "Guided";
break;
case (byte)(Common.ac2modes.LOITER):
mode = "Loiter";
break;
case (byte)(Common.ac2modes.RTL):
mode = "RTL";
break;
case (byte)(Common.ac2modes.CIRCLE):
mode = "Circle";
break;
default:
mode = "Unknown";
break;
}
}
}
if (oldmode != mode && MainV2.speechenable && MainV2.getConfig("speechmodeenabled") == "True")
{
MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode")));
}
}
if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] != null)
{
ArdupilotMega.MAVLink.__mavlink_sys_status_t sysstatus = new ArdupilotMega.MAVLink.__mavlink_sys_status_t();
object temp = sysstatus;
MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS], ref temp, 6);
sysstatus = (ArdupilotMega.MAVLink.__mavlink_sys_status_t)(temp);
battery_voltage = sysstatus.voltage_battery;
battery_remaining = sysstatus.battery_remaining;
packetdropremote = sysstatus.drop_rate_comm;
//MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null;
}
#else
if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] != null)
{
ArdupilotMega.MAVLink.__mavlink_sys_status_t sysstatus = new ArdupilotMega.MAVLink.__mavlink_sys_status_t();
object temp = sysstatus;
MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS], ref temp, 6);
sysstatus = (ArdupilotMega.MAVLink.__mavlink_sys_status_t)(temp);
string oldmode = mode;
switch (sysstatus.mode)
{
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_UNINIT:
switch (sysstatus.nav_mode)
{
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_GROUNDED:
mode = "Initialising";
break;
}
break;
case (byte)(100 + Common.ac2modes.STABILIZE):
mode = "Stabilize";
break;
case (byte)(100 + Common.ac2modes.ACRO):
mode = "Acro";
break;
case (byte)(100 + Common.ac2modes.ALT_HOLD):
mode = "Alt Hold";
break;
case (byte)(100 + Common.ac2modes.AUTO):
mode = "Auto";
break;
case (byte)(100 + Common.ac2modes.GUIDED):
mode = "Guided";
break;
case (byte)(100 + Common.ac2modes.LOITER):
mode = "Loiter";
break;
case (byte)(100 + Common.ac2modes.RTL):
mode = "RTL";
break;
case (byte)(100 + Common.ac2modes.CIRCLE):
mode = "Circle";
break;
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_MANUAL:
mode = "Manual";
break;
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_GUIDED:
mode = "Guided";
break;
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST1:
mode = "Stabilize";
break;
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST2:
mode = "FBW A"; // fall though old
switch (sysstatus.nav_mode)
{
case (byte)1:
mode = "FBW A";
break;
case (byte)2:
mode = "FBW B";
break;
case (byte)3:
mode = "FBW C";
break;
}
break;
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST3:
mode = "FBW B";
break;
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_AUTO:
switch (sysstatus.nav_mode)
{
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_WAYPOINT:
mode = "Auto";
break;
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_RETURNING:
mode = "RTL";
break;
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_HOLD:
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LOITER:
mode = "Loiter";
break;
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LIFTOFF:
mode = "Takeoff";
break;
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LANDING:
mode = "Land";
break;
}
break;
default:
mode = "Unknown";
break;
}
battery_voltage = sysstatus.vbat;
battery_remaining = sysstatus.battery_remaining;
packetdropremote = sysstatus.packet_drop;
if (oldmode != mode && MainV2.speechenable && MainV2.getConfig("speechmodeenabled") == "True")
{
MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode")));
}
//MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null;
}
#endif
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE] != null)
{
var att = new ArdupilotMega.MAVLink.__mavlink_attitude_t();
object temp = att;
MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE], ref temp, 6);
att = (MAVLink.__mavlink_attitude_t)(temp);
roll = att.roll * rad2deg;
pitch = att.pitch * rad2deg;
yaw = att.yaw * rad2deg;
// Console.WriteLine(roll + " " + pitch + " " + yaw);
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE] = null;
}
#if MAVLINK10
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT] != null)
{
var gps = new MAVLink.__mavlink_gps_raw_int_t();
object temp = gps;
MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT], ref temp, 6);
gps = (MAVLink.__mavlink_gps_raw_int_t)(temp);
lat = gps.lat * 1.0e-7f;
lng = gps.lon * 1.0e-7f;
// alt = gps.alt; // using vfr as includes baro calc
gpsstatus = gps.fix_type;
// Console.WriteLine("gpsfix {0}",gpsstatus);
gpshdop = gps.eph;
groundspeed = gps.vel * 1.0e-2f;
groundcourse = gps.cog * 1.0e-2f;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null;
}
#else
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] != null)
{
var gps = new MAVLink.__mavlink_gps_raw_t();
object temp = gps;
MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW], ref temp, 6);
gps = (MAVLink.__mavlink_gps_raw_t)(temp);
lat = gps.lat;
lng = gps.lon;
// alt = gps.alt; // using vfr as includes baro calc
gpsstatus = gps.fix_type;
// Console.WriteLine("gpsfix {0}",gpsstatus);
gpshdop = gps.eph;
groundspeed = gps.v;
groundcourse = gps.hdg;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null;
}
#endif
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_STATUS] != null)
{
var gps = new MAVLink.__mavlink_gps_status_t();
object temp = gps;
MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_STATUS], ref temp, 6);
gps = (MAVLink.__mavlink_gps_status_t)(temp);
satcount = gps.satellites_visible;
}
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT] != null)
{
var loc = new MAVLink.__mavlink_global_position_int_t();
object temp = loc;
MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT], ref temp, 6);
loc = (MAVLink.__mavlink_global_position_int_t)(temp);
//alt = loc.alt / 1000.0f;
lat = loc.lat / 10000000.0f;
lng = loc.lon / 10000000.0f;
}
#if MAVLINK10
if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT] != null)
{
ArdupilotMega.MAVLink.__mavlink_mission_current_t wpcur = new ArdupilotMega.MAVLink.__mavlink_mission_current_t();
object temp = wpcur;
MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT], ref temp, 6);
wpcur = (ArdupilotMega.MAVLink.__mavlink_mission_current_t)(temp);
int oldwp = (int)wpno;
wpno = wpcur.seq;
if (oldwp != wpno && MainV2.speechenable && MainV2.getConfig("speechwaypointenabled") == "True")
{
MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint")));
}
//MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] = null;
}
#else
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION] != null)
{
var loc = new MAVLink.__mavlink_global_position_t();
object temp = loc;
MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION], ref temp, 6);
loc = (MAVLink.__mavlink_global_position_t)(temp);
alt = loc.alt;
lat = loc.lat;
lng = loc.lon;
}
if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] != null)
{
ArdupilotMega.MAVLink.__mavlink_waypoint_current_t wpcur = new ArdupilotMega.MAVLink.__mavlink_waypoint_current_t();
object temp = wpcur;
MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT], ref temp, 6);
wpcur = (ArdupilotMega.MAVLink.__mavlink_waypoint_current_t)(temp);
int oldwp = (int)wpno;
wpno = wpcur.seq;
if (oldwp != wpno && MainV2.speechenable && MainV2.getConfig("speechwaypointenabled") == "True")
{
MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint")));
}
//MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] = null;
}
#endif
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] != null)
{
var rcin = new MAVLink.__mavlink_rc_channels_raw_t();
object temp = rcin;
MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW], ref temp, 6);
rcin = (MAVLink.__mavlink_rc_channels_raw_t)(temp);
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;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] = null;
}
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW] != null)
{
var servoout = new MAVLink.__mavlink_servo_output_raw_t();
object temp = servoout;
MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW], ref temp, 6);
servoout = (MAVLink.__mavlink_servo_output_raw_t)(temp);
ch1out = servoout.servo1_raw;
ch2out = servoout.servo2_raw;
ch3out = servoout.servo3_raw;
ch4out = servoout.servo4_raw;
ch5out = servoout.servo5_raw;
ch6out = servoout.servo6_raw;
ch7out = servoout.servo7_raw;
ch8out = servoout.servo8_raw;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW] = null;
}
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] != null)
{
var imu = new MAVLink.__mavlink_raw_imu_t();
object temp = imu;
MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU], ref temp, 6);
imu = (MAVLink.__mavlink_raw_imu_t)(temp);
gx = imu.xgyro;
gy = imu.ygyro;
gz = imu.zgyro;
ax = imu.xacc;
ay = imu.yacc;
az = imu.zacc;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null;
}
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_SCALED_IMU] != null)
{
var imu = new MAVLink.__mavlink_scaled_imu_t();
object temp = imu;
MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_SCALED_IMU], ref temp, 6);
imu = (MAVLink.__mavlink_scaled_imu_t)(temp);
gx = imu.xgyro;
gy = imu.ygyro;
gz = imu.zgyro;
ax = imu.xacc;
ay = imu.yacc;
az = imu.zacc;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null;
}
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD] != null)
{
MAVLink.__mavlink_vfr_hud_t vfr = new MAVLink.__mavlink_vfr_hud_t();
object temp = vfr;
MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD], ref temp, 6);
vfr = (MAVLink.__mavlink_vfr_hud_t)(temp);
groundspeed = vfr.groundspeed;
airspeed = vfr.airspeed;
alt = vfr.alt; // this might include baro
//climbrate = vfr.climb;
if ((DateTime.Now - lastalt).TotalSeconds >= 0.1 && oldalt != alt)
{
climbrate = (alt - oldalt) / (float)(DateTime.Now - lastalt).TotalSeconds;
verticalspeed = (alt - oldalt) / (float)(DateTime.Now - lastalt).TotalSeconds;
if (float.IsInfinity(_verticalspeed))
_verticalspeed = 0;
lastalt = DateTime.Now;
oldalt = alt;
}
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD] = null;
}
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_MEMINFO] != null) // hil
{
var mem = new ArdupilotMega.MAVLink.__mavlink_meminfo_t();
object temp = mem;
MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_MEMINFO], ref temp, 6);
mem = (MAVLink.__mavlink_meminfo_t)(temp);
freemem = mem.freemem;
brklevel = mem.brkval;
}
}
//Console.WriteLine(DateTime.Now.Millisecond + " start ");
// update form
try
{
if (bs != null)
{
//Console.WriteLine(DateTime.Now.Millisecond);
bs.DataSource = this;
//Console.WriteLine(DateTime.Now.Millisecond + " 1 " + updatenow);
bs.ResetBindings(false);
//Console.WriteLine(DateTime.Now.Millisecond + " done ");
}
}
catch { Console.WriteLine("CurrentState Binding error"); }
}
/// <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;
log.Info("Start log: " + DateTime.Now);
MAVLink mine = new MAVLink();
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);
double[] x = LeastSq(data);
System.Console.WriteLine("Old Method {0} {1} {2}", -(maxx + minx) / 2, -(maxy + miny) / 2, -(maxz + minz) / 2);
log.Info("Least Sq Done " + DateTime.Now);
// create a dxf for those who want to "see" the calibration
DxfDocument dxf = new DxfDocument();
Polyline3d polyline = new Polyline3d(vertexes, true);
polyline.Layer = new Layer("polyline3d");
polyline.Layer.Color.Index = 24;
dxf.AddEntity(polyline);
Point pnt = new Point(new Vector3f(-offset.Item1, -offset.Item2, -offset.Item3));
pnt.Layer = new Layer("old offset");
pnt.Layer.Color.Index = 22;
dxf.AddEntity(pnt);
pnt = new Point(new Vector3f(-(float)x[0], -(float)x[1], -(float)x[2]));
pnt.Layer = new Layer("new offset");
pnt.Layer.Color.Index = 21;
dxf.AddEntity(pnt);
dxf.Save("magoffset.dxf", DxfVersion.AutoCad2000);
log.Info("dxf Done " + DateTime.Now);
Array.Resize<double>(ref x, 3);
return x;
}
List<string[]> readLog(string fn)
{
if (logcache.Count > 0)
return logcache;
List<string[]> list = new List<string[]>();
if (fn.ToLower().EndsWith("tlog"))
{
MAVLink mine = new MAVLink();
mine.logplaybackfile = new BinaryReader(File.Open(fn, FileMode.Open, FileAccess.Read, FileShare.Read));
mine.logreadmode = true;
mine.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.alt.ToString(),cs.alt.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();
sr.Dispose();
logcache = list;
return list;
}
/*
public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow)
{
UpdateCurrentSettings(bs, false, MainV2.comPort);
}
*/
public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow, MAVLink mavinterface)
{
lock (locker)
{
if (DateTime.Now > lastupdate.AddMilliseconds(50) || updatenow) // 20 hz
{
lastupdate = DateTime.Now;
//check if valid mavinterface
if (mavinterface != null && mavinterface.packetsnotlost != 0)
linkqualitygcs = (ushort)((mavinterface.packetsnotlost / (mavinterface.packetsnotlost + mavinterface.packetslost)) * 100.0);
if (DateTime.Now.Second != lastsecondcounter.Second)
{
lastsecondcounter = DateTime.Now;
if (lastpos.Lat != 0 && lastpos.Lng != 0)
{
if (!MainV2.comPort.BaseStream.IsOpen && !MainV2.comPort.logreadmode)
distTraveled = 0;
distTraveled += (float)lastpos.GetDistance(new PointLatLngAlt(lat, lng, 0, "")) * multiplierdist;
lastpos = new PointLatLngAlt(lat, lng, 0, "");
}
else
{
lastpos = new PointLatLngAlt(lat, lng, 0, "");
}
// cant use gs, cant use alt,
if (ch3percent > 12)
timeInAir++;
if (!gotwind)
dowindcalc();
}
if (mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] != null) // status text
{
string logdata = Encoding.ASCII.GetString(mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT], 6, mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT].Length - 6);
int ind = logdata.IndexOf('\0');
if (ind != -1)
logdata = logdata.Substring(0, ind);
try
{
while (messages.Count > 50)
{
messages.RemoveAt(0);
}
messages.Add(logdata + "\n");
}
catch { }
mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] = null;
}
byte[] bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED];
if (bytearray != null) // hil mavlink 0.9
{
var hil = bytearray.ByteArrayToStructure<MAVLink.mavlink_rc_channels_scaled_t>(6);
hilch1 = hil.chan1_scaled;
hilch2 = hil.chan2_scaled;
hilch3 = hil.chan3_scaled;
hilch4 = hil.chan4_scaled;
hilch5 = hil.chan5_scaled;
hilch6 = hil.chan6_scaled;
hilch7 = hil.chan7_scaled;
hilch8 = hil.chan8_scaled;
// Console.WriteLine("MAVLINK_MSG_ID_RC_CHANNELS_SCALED Packet");
mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_HIL_CONTROLS];
if (bytearray != null) // hil mavlink 0.9 and 1.0
{
var hil = bytearray.ByteArrayToStructure<MAVLink.mavlink_hil_controls_t>(6);
hilch1 = (int)(hil.roll_ailerons * 10000);
hilch2 = (int)(hil.pitch_elevator * 10000);
hilch3 = (int)(hil.throttle * 10000);
hilch4 = (int)(hil.yaw_rudder * 10000);
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_HIL_CONTROLS] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_HWSTATUS];
if (bytearray != null)
{
var hwstatus = bytearray.ByteArrayToStructure<MAVLink.mavlink_hwstatus_t>(6);
hwvoltage = hwstatus.Vcc / 1000.0f;
i2cerrors = hwstatus.I2Cerr;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_HWSTATUS] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_WIND];
if (bytearray != null)
{
var wind = bytearray.ByteArrayToStructure<MAVLink.mavlink_wind_t>(6);
gotwind = true;
wind_dir = (wind.direction + 360) % 360;
wind_vel = wind.speed * multiplierspeed;
//MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null;
}
#if MAVLINK10
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_HEARTBEAT];
if (bytearray != null)
{
var hb = bytearray.ByteArrayToStructure<MAVLink.mavlink_heartbeat_t>(6);
armed = (hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) == (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED;
failsafe = hb.system_status == (byte)MAVLink.MAV_STATE.CRITICAL;
string oldmode = mode;
mode = "Unknown";
if ((hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED) != 0)
{
if (hb.type == (byte)MAVLink.MAV_TYPE.FIXED_WING)
{
switch (hb.custom_mode)
{
case (byte)(Common.apmmodes.MANUAL):
mode = "Manual";
break;
case (byte)(Common.apmmodes.GUIDED):
mode = "Guided";
break;
case (byte)(Common.apmmodes.STABILIZE):
mode = "Stabilize";
break;
case (byte)(Common.apmmodes.TRAINING):
mode = "Training";
break;
case (byte)(Common.apmmodes.FLY_BY_WIRE_A):
mode = "FBW A";
break;
case (byte)(Common.apmmodes.FLY_BY_WIRE_B):
mode = "FBW B";
break;
case (byte)(Common.apmmodes.AUTO):
mode = "Auto";
break;
case (byte)(Common.apmmodes.RTL):
mode = "RTL";
break;
case (byte)(Common.apmmodes.LOITER):
mode = "Loiter";
break;
case (byte)(Common.apmmodes.CIRCLE):
mode = "Circle";
break;
case (byte)(Common.apmmodes.TAKEOFF):
mode = "Takeoff";
break;
case 16:
mode = "Initialising";
break;
default:
mode = "Unknown";
break;
}
}
else if (hb.type == (byte)MAVLink.MAV_TYPE.QUADROTOR)
{
switch (hb.custom_mode)
{
case (byte)(Common.ac2modes.STABILIZE):
mode = "Stabilize";
break;
case (byte)(Common.ac2modes.ACRO):
mode = "Acro";
break;
case (byte)(Common.ac2modes.ALT_HOLD):
mode = "Alt Hold";
break;
case (byte)(Common.ac2modes.AUTO):
mode = "Auto";
break;
case (byte)(Common.ac2modes.GUIDED):
mode = "Guided";
break;
case (byte)(Common.ac2modes.LOITER):
mode = "Loiter";
break;
case (byte)(Common.ac2modes.RTL):
mode = "RTL";
break;
case (byte)(Common.ac2modes.CIRCLE):
mode = "Circle";
break;
case (byte)(Common.ac2modes.LAND):
mode = "Land";
break;
case (byte)Common.ac2modes.OF_LOITER:
mode = "OF Loiter";
break;
default:
mode = "Unknown";
break;
}
}
}
if (oldmode != mode && MainV2.speechEnable && MainV2.getConfig("speechmodeenabled") == "True")
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode")));
}
}
bytearray = mavinterface.MAV.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS];
if (bytearray != null)
{
var sysstatus = bytearray.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6);
battery_voltage = sysstatus.voltage_battery;
battery_remaining = sysstatus.battery_remaining;
current = sysstatus.current_battery;
packetdropremote = sysstatus.drop_rate_comm;
//MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null;
}
#else
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_SYS_STATUS];
if (bytearray != null)
{
var sysstatus = bytearray.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6);
armed = sysstatus.status;
string oldmode = mode;
switch (sysstatus.mode)
{
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_UNINIT:
switch (sysstatus.nav_mode)
{
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_GROUNDED:
mode = "Initialising";
break;
}
break;
case (byte)(100 + Common.ac2modes.STABILIZE):
mode = EnumTranslator.GetDisplayText(Common.ac2modes.STABILIZE);
break;
case (byte)(100 + Common.ac2modes.ACRO):
mode = EnumTranslator.GetDisplayText(Common.ac2modes.ACRO);
break;
case (byte)(100 + Common.ac2modes.ALT_HOLD):
mode = EnumTranslator.GetDisplayText(Common.ac2modes.ALT_HOLD);
break;
case (byte)(100 + Common.ac2modes.AUTO):
mode = EnumTranslator.GetDisplayText(Common.ac2modes.AUTO);
break;
case (byte)(100 + Common.ac2modes.GUIDED):
mode = EnumTranslator.GetDisplayText(Common.ac2modes.GUIDED);
break;
case (byte)(100 + Common.ac2modes.LOITER):
mode = EnumTranslator.GetDisplayText(Common.ac2modes.LOITER);
break;
case (byte)(100 + Common.ac2modes.RTL):
mode = EnumTranslator.GetDisplayText(Common.ac2modes.RTL);
break;
case (byte)(100 + Common.ac2modes.CIRCLE):
mode = EnumTranslator.GetDisplayText(Common.ac2modes.CIRCLE);
break;
case (byte)(100 + Common.ac2modes.LAND):
mode = EnumTranslator.GetDisplayText(Common.ac2modes.LAND);
break;
case (byte)(100 + Common.ac2modes.POSITION):
mode = EnumTranslator.GetDisplayText(Common.ac2modes.POSITION);
break;
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_MANUAL:
mode = "Manual";
break;
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_GUIDED:
mode = "Guided";
break;
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST1:
mode = "Stabilize";
break;
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST2:
mode = "FBW A"; // fall though old
switch (sysstatus.nav_mode)
{
case (byte)1:
mode = "FBW A";
break;
case (byte)2:
mode = "FBW B";
break;
case (byte)3:
mode = "FBW C";
break;
}
break;
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST3:
mode = "Circle";
break;
case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_AUTO:
switch (sysstatus.nav_mode)
{
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_WAYPOINT:
mode = "Auto";
break;
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_RETURNING:
mode = "RTL";
break;
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_HOLD:
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LOITER:
mode = "Loiter";
break;
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LIFTOFF:
mode = "Takeoff";
break;
case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LANDING:
mode = "Land";
break;
}
break;
default:
mode = "Unknown";
break;
}
battery_voltage = sysstatus.vbat;
battery_remaining = sysstatus.battery_remaining;
packetdropremote = sysstatus.packet_drop;
if (oldmode != mode && MainV2.speechEnable && MainV2.speechEngine != null && MainV2.getConfig("speechmodeenabled") == "True")
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode")));
}
//MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null;
}
#endif
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_SCALED_PRESSURE];
if (bytearray != null)
{
var pres = bytearray.ByteArrayToStructure<MAVLink.mavlink_scaled_pressure_t>(6);
press_abs = pres.press_abs;
press_temp = pres.temperature;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_SENSOR_OFFSETS];
if (bytearray != null)
{
var sensofs = bytearray.ByteArrayToStructure<MAVLink.mavlink_sensor_offsets_t>(6);
mag_ofs_x = sensofs.mag_ofs_x;
mag_ofs_y = sensofs.mag_ofs_y;
mag_ofs_z = sensofs.mag_ofs_z;
mag_declination = sensofs.mag_declination;
raw_press = sensofs.raw_press;
raw_temp = sensofs.raw_temp;
gyro_cal_x = sensofs.gyro_cal_x;
gyro_cal_y = sensofs.gyro_cal_y;
gyro_cal_z = sensofs.gyro_cal_z;
accel_cal_x = sensofs.accel_cal_x;
accel_cal_y = sensofs.accel_cal_y;
accel_cal_z = sensofs.accel_cal_z;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE];
if (bytearray != null)
{
var att = bytearray.ByteArrayToStructure<MAVLink.mavlink_attitude_t>(6);
roll = att.roll * rad2deg;
pitch = att.pitch * rad2deg;
yaw = att.yaw * rad2deg;
// Console.WriteLine(roll + " " + pitch + " " + yaw);
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT];
if (bytearray != null)
{
var gps = bytearray.ByteArrayToStructure<MAVLink.mavlink_gps_raw_int_t>(6);
if (!useLocation)
{
lat = gps.lat * 1.0e-7f;
lng = gps.lon * 1.0e-7f;
}
// alt = gps.alt; // using vfr as includes baro calc
gpsstatus = gps.fix_type;
// Console.WriteLine("gpsfix {0}",gpsstatus);
gpshdop = (float)Math.Round((double)gps.eph / 100.0,2);
satcount = gps.satellites_visible;
groundspeed = gps.vel * 1.0e-2f;
groundcourse = gps.cog * 1.0e-2f;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_GPS_STATUS];
if (bytearray != null)
{
var gps = bytearray.ByteArrayToStructure<MAVLink.mavlink_gps_status_t>(6);
satcount = gps.satellites_visible;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_RADIO];
if (bytearray != null)
{
var radio = bytearray.ByteArrayToStructure<MAVLink.mavlink_radio_t>(6);
rssi = radio.rssi;
remrssi = radio.remrssi;
txbuffer = radio.txbuf;
rxerrors = radio.rxerrors;
noise = radio.noise;
remnoise = radio.remnoise;
fixedp = radio.fixedp;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT];
if (bytearray != null)
{
var loc = bytearray.ByteArrayToStructure<MAVLink.mavlink_global_position_int_t>(6);
// the new arhs deadreckoning may send 0 alt and 0 long. check for and undo
useLocation = true;
if (loc.lat == 0 && loc.lon == 0)
{
useLocation = false;
}
else
{
//alt = loc.alt / 1000.0f;
lat = loc.lat / 10000000.0f;
lng = loc.lon / 10000000.0f;
}
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT];
if (bytearray != null)
{
var wpcur = bytearray.ByteArrayToStructure<MAVLink.mavlink_mission_current_t>(6);
int oldwp = (int)wpno;
wpno = wpcur.seq;
if (oldwp != wpno && MainV2.speechEnable && MainV2.getConfig("speechwaypointenabled") == "True")
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint")));
}
//MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT];
if (bytearray != null)
{
var nav = bytearray.ByteArrayToStructure<MAVLink.mavlink_nav_controller_output_t>(6);
nav_roll = nav.nav_roll;
nav_pitch = nav.nav_pitch;
nav_bearing = nav.nav_bearing;
target_bearing = nav.target_bearing;
wp_dist = nav.wp_dist;
alt_error = nav.alt_error;
aspd_error = nav.aspd_error / 100.0f;
xtrack_error = nav.xtrack_error;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW];
if (bytearray != null)
{
var rcin = bytearray.ByteArrayToStructure<MAVLink.mavlink_rc_channels_raw_t>(6);
ch1in = rcin.chan1_raw;
ch2in = rcin.chan2_raw;
ch3in = rcin.chan3_raw;
ch4in = rcin.chan4_raw;
ch5in = rcin.chan5_raw;
ch6in = rcin.chan6_raw;
ch7in = rcin.chan7_raw;
ch8in = rcin.chan8_raw;
//percent
rxrssi = (float)((rcin.rssi / 255.0) * 100.0);
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW];
if (bytearray != null)
{
var servoout = bytearray.ByteArrayToStructure<MAVLink.mavlink_servo_output_raw_t>(6);
ch1out = servoout.servo1_raw;
ch2out = servoout.servo2_raw;
ch3out = servoout.servo3_raw;
ch4out = servoout.servo4_raw;
ch5out = servoout.servo5_raw;
ch6out = servoout.servo6_raw;
ch7out = servoout.servo7_raw;
ch8out = servoout.servo8_raw;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU];
if (bytearray != null)
{
var imu = bytearray.ByteArrayToStructure<MAVLink.mavlink_raw_imu_t>(6);
gx = imu.xgyro;
gy = imu.ygyro;
gz = imu.zgyro;
ax = imu.xacc;
ay = imu.yacc;
az = imu.zacc;
mx = imu.xmag;
my = imu.ymag;
mz = imu.zmag;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_SCALED_IMU];
if (bytearray != null)
{
var imu = bytearray.ByteArrayToStructure<MAVLink.mavlink_scaled_imu_t>(6);
gx = imu.xgyro;
gy = imu.ygyro;
gz = imu.zgyro;
ax = imu.xacc;
ay = imu.yacc;
az = imu.zacc;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD];
if (bytearray != null)
{
var vfr = bytearray.ByteArrayToStructure<MAVLink.mavlink_vfr_hud_t>(6);
groundspeed = vfr.groundspeed;
airspeed = vfr.airspeed;
alt = vfr.alt; // this might include baro
ch3percent = vfr.throttle;
//Console.WriteLine(alt);
//climbrate = vfr.climb;
// check update rate, and ensure time hasnt gone backwards
if ((datetime - lastalt).TotalSeconds >= 0.2 && oldalt != alt || lastalt > datetime)
{
climbrate = (alt - oldalt) / (float)(datetime - lastalt).TotalSeconds;
verticalspeed = (alt - oldalt) / (float)(datetime - lastalt).TotalSeconds;
if (float.IsInfinity(_verticalspeed))
_verticalspeed = 0;
lastalt = datetime;
oldalt = alt;
}
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD] = null;
}
bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_MEMINFO];
if (bytearray != null)
{
var mem = bytearray.ByteArrayToStructure<MAVLink.mavlink_meminfo_t>(6);
freemem = mem.freemem;
brklevel = mem.brkval;
}
}
//Console.Write(DateTime.Now.Millisecond + " start ");
// update form
try
{
if (bs != null)
{
//System.Diagnostics.Debug.WriteLine(DateTime.Now.Millisecond);
// Console.Write(" "+DateTime.Now.Millisecond);
//bs.DataSource = this;
// Console.Write(" " + DateTime.Now.Millisecond + " 1 " + updatenow + " " + System.Threading.Thread.CurrentThread.Name);
//bs.ResetBindings(false);
//bs.ResetCurrentItem();
// mono workaround - this is alot faster
bs.Add(this);
// Console.WriteLine(" " + DateTime.Now.Millisecond + " done ");
}
}
catch { log.InfoFormat("CurrentState Binding error"); }
}
}
public bool translateMode(string modein, ref MAVLink.mavlink_set_mode_t mode)
{
mode.target_system = MAV.sysid;
try
{
List<KeyValuePair<int,string>> modelist = Common.getModesList();
foreach (KeyValuePair<int, string> pair in modelist)
{
if (pair.Value.ToLower() == modein.ToLower())
{
mode.base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED;
mode.custom_mode = (uint)pair.Key;
}
}
if (mode.base_mode == 0)
{
MessageBox.Show("No Mode Changed " + modein);
return false;
}
}
catch { System.Windows.Forms.MessageBox.Show("Failed to find Mode"); return false; }
return true;
}
public static bool translateMode(string modein, ref MAVLink.mavlink_set_mode_t mode)
{
//MAVLink09.mavlink_set_mode_t mode = new MAVLink09.mavlink_set_mode_t();
mode.target_system = MainV2.comPort.sysid;
try
{
if (Common.getModes() == typeof(Common.apmmodes))
{
switch (EnumTranslator.GetValue<Common.apmmodes>(modein))
{
case (int)Common.apmmodes.MANUAL:
case (int)Common.apmmodes.CIRCLE:
case (int)Common.apmmodes.STABILIZE:
case (int)Common.apmmodes.AUTO:
case (int)Common.apmmodes.RTL:
case (int)Common.apmmodes.LOITER:
case (int)Common.apmmodes.FLY_BY_WIRE_A:
case (int)Common.apmmodes.FLY_BY_WIRE_B:
mode.base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED;
mode.custom_mode = (uint)EnumTranslator.GetValue<Common.apmmodes>(modein);
break;
default:
MessageBox.Show("No Mode Changed " + modein);
return false;
}
}
else if (Common.getModes() == typeof(Common.ac2modes))
{
switch (EnumTranslator.GetValue<Common.ac2modes>(modein))
{
case (int)Common.ac2modes.STABILIZE:
case (int)Common.ac2modes.AUTO:
case (int)Common.ac2modes.RTL:
case (int)Common.ac2modes.LOITER:
case (int)Common.ac2modes.ACRO:
case (int)Common.ac2modes.ALT_HOLD:
case (int)Common.ac2modes.CIRCLE:
case (int)Common.ac2modes.POSITION:
case (int)Common.ac2modes.LAND:
case (int)Common.ac2modes.OF_LOITER:
mode.base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED;
mode.custom_mode = (uint)EnumTranslator.GetValue<Common.ac2modes>(modein);
break;
default:
MessageBox.Show("No Mode Changed " + modein);
return false;
}
}
}
catch { System.Windows.Forms.MessageBox.Show("Failed to find Mode"); return false; }
return true;
}
void dolog()
{
flightdata.Clear();
MAVLink mine = new MAVLink();
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;
}
public static bool translateMode(string modein, ref MAVLink.mavlink_set_nav_mode_t navmode, ref MAVLink.mavlink_set_mode_t mode)
{
//MAVLink.mavlink_set_nav_mode_t navmode = new MAVLink.mavlink_set_nav_mode_t();
navmode.target = MainV2.comPort.sysid;
navmode.nav_mode = 255;
//MAVLink.mavlink_set_mode_t mode = new MAVLink.mavlink_set_mode_t();
mode.target = MainV2.comPort.sysid;
try
{
if (Common.getModes() == typeof(Common.apmmodes))
{
switch (EnumTranslator.GetValue<Common.apmmodes>(modein))
{
case (int)Common.apmmodes.MANUAL:
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_MANUAL;
break;
case (int)Common.apmmodes.GUIDED:
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_GUIDED;
break;
case (int)Common.apmmodes.STABILIZE:
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_TEST1;
break;
// AUTO MODES
case (int)Common.apmmodes.AUTO:
navmode.nav_mode = (byte)MAVLink.MAV_NAV.MAV_NAV_WAYPOINT;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_AUTO;
break;
case (int)Common.apmmodes.RTL:
navmode.nav_mode = (byte)MAVLink.MAV_NAV.MAV_NAV_RETURNING;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_AUTO;
break;
case (int)Common.apmmodes.LOITER:
navmode.nav_mode = (byte)MAVLink.MAV_NAV.MAV_NAV_LOITER;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_AUTO;
break;
// FBW
case (int)Common.apmmodes.FLY_BY_WIRE_A:
navmode.nav_mode = (byte)1;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_TEST2;
break;
case (int)Common.apmmodes.FLY_BY_WIRE_B:
navmode.nav_mode = (byte)2;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_TEST2;
break;
default:
CustomMessageBox.Show("No Mode Changed " + modein);
return false;
}
}
else if (Common.getModes() == typeof(Common.aprovermodes))
{
switch (EnumTranslator.GetValue<Common.aprovermodes>(modein))
{
case (int)Common.aprovermodes.MANUAL:
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_MANUAL;
break;
case (int)Common.aprovermodes.GUIDED:
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_GUIDED;
break;
case (int)Common.aprovermodes.LEARNING:
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_TEST1;
break;
// AUTO MODES
case (int)Common.aprovermodes.AUTO:
navmode.nav_mode = (byte)MAVLink.MAV_NAV.MAV_NAV_WAYPOINT;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_AUTO;
break;
case (int)Common.aprovermodes.RTL:
navmode.nav_mode = (byte)MAVLink.MAV_NAV.MAV_NAV_RETURNING;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_AUTO;
break;
case (int)Common.aprovermodes.LOITER:
navmode.nav_mode = (byte)MAVLink.MAV_NAV.MAV_NAV_LOITER;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_AUTO;
break;
// FBW
case (int)Common.aprovermodes.FLY_BY_WIRE_A:
navmode.nav_mode = (byte)1;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_TEST2;
break;
case (int)Common.aprovermodes.FLY_BY_WIRE_B:
navmode.nav_mode = (byte)2;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_TEST2;
break;
default:
CustomMessageBox.Show("No Mode Changed " + modein);
return false;
}
}
else if (Common.getModes() == typeof(Common.ac2modes))
{
switch (EnumTranslator.GetValue<Common.ac2modes>(modein))
{
case (int)Common.ac2modes.GUIDED:
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_GUIDED;
break;
case (int)Common.ac2modes.STABILIZE:
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_MANUAL;
break;
// AUTO MODES
case (int)Common.ac2modes.AUTO:
navmode.nav_mode = (byte)MAVLink.MAV_NAV.MAV_NAV_WAYPOINT;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_AUTO;
break;
case (int)Common.ac2modes.RTL:
navmode.nav_mode = (byte)MAVLink.MAV_NAV.MAV_NAV_RETURNING;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_AUTO;
break;
case (int)Common.ac2modes.LOITER:
navmode.nav_mode = (byte)MAVLink.MAV_NAV.MAV_NAV_LOITER;
mode.mode = (byte)MAVLink.MAV_MODE.MAV_MODE_AUTO;
break;
default:
CustomMessageBox.Show("No Mode Changed " + modein);
return false;
}
}
}
catch { System.Windows.Forms.CustomMessageBox.Show("Failed to find Mode"); return false; }
return true;
}
private void BUT_humanreadable_Click(object sender, EventArgs e)
{
OpenFileDialog openFileDialog1 = new OpenFileDialog();
openFileDialog1.Filter = "*.tlog|*.tlog";
openFileDialog1.FilterIndex = 2;
openFileDialog1.RestoreDirectory = true;
openFileDialog1.Multiselect = true;
try
{
openFileDialog1.InitialDirectory = Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs" + Path.DirectorySeparatorChar;
}
catch { } // incase dir doesnt exist
if (openFileDialog1.ShowDialog() == DialogResult.OK)
{
foreach (string logfile in openFileDialog1.FileNames)
{
MAVLink mine = new MAVLink();
mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read));
mine.logreadmode = true;
mine.packets.Initialize(); // clear
StreamWriter sw = new StreamWriter(Path.GetDirectoryName(logfile)+ Path.DirectorySeparatorChar + Path.GetFileNameWithoutExtension(logfile) + ".txt");
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
// bar moves to 100 % in this step
progressBar1.Value = (int)((float)mine.logplaybackfile.BaseStream.Position / (float)mine.logplaybackfile.BaseStream.Length * 100.0f / 1.0f);
progressBar1.Refresh();
//Application.DoEvents();
byte[] packet = mine.readPacket();
string text = "";
mine.DebugPacket(packet, ref text);
sw.Write(mine.lastlogread +" "+text);
}
sw.Close();
progressBar1.Value = 100;
mine.logreadmode = false;
mine.logplaybackfile.Close();
mine.logplaybackfile = null;
}
}
}
private void BUT_redokml_Click(object sender, EventArgs e)
{
OpenFileDialog openFileDialog1 = new OpenFileDialog();
openFileDialog1.Filter = "*.tlog|*.tlog";
openFileDialog1.FilterIndex = 2;
openFileDialog1.RestoreDirectory = true;
openFileDialog1.Multiselect = true;
try
{
openFileDialog1.InitialDirectory = Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs" + Path.DirectorySeparatorChar;
}
catch { } // incase dir doesnt exist
if (openFileDialog1.ShowDialog() == DialogResult.OK)
{
foreach (string logfile in openFileDialog1.FileNames)
{
MAVLink mine = new MAVLink();
mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read));
mine.logreadmode = true;
mine.packets.Initialize(); // clear
CurrentState cs = new CurrentState();
float oldlatlngalt = 0;
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
// bar moves to 50 % in this step
progressBar1.Value = (int)((float)mine.logplaybackfile.BaseStream.Position / (float)mine.logplaybackfile.BaseStream.Length * 100.0f / 2.0f);
progressBar1.Refresh();
byte[] packet = mine.readPacket();
cs.datetime = mine.lastlogread;
cs.UpdateCurrentSettings(null, true, mine);
try
{
if (MainV2.talk != null)
MainV2.talk.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) != oldlatlngalt
&& cs.lat != 0 && cs.lng != 0)
{
Console.WriteLine(cs.lat + " " + cs.lng + " " + cs.alt + " lah " + (float)(cs.lat + cs.lng + cs.alt) + "!=" + oldlatlngalt);
CurrentState cs2 = (CurrentState)cs.Clone();
flightdata.Add(cs2);
oldlatlngalt = (cs.lat + cs.lng);
}
}
mine.logreadmode = false;
mine.logplaybackfile.Close();
mine.logplaybackfile = null;
writeKML(logfile + ".kml");
progressBar1.Value = 100;
}
}
}
private void tabControl1_SelectedIndexChanged(object sender, EventArgs e)
{
try
{
comPort = MainV2.comPort;
if (!comPort.BaseStream.IsOpen && !MainV2.comPort.logreadmode)
{
CustomMessageBox.Show("Please connect first");
this.Close();
}
//comPort.DtrEnable = true;
//comPort.Open();
//comPort.stopall(true); // ensure off
Console.WriteLine("Req streams {0} {1}", comPort.bps, DateTime.Now);
//comPort.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS, 0); // mode gps raw
//comPort.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.POSITION, 3); // request location
//comPort.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.EXTRA1, 3); // request attitude
//comPort.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.EXTRA2, 3); // request vfr
comPort.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.cs.ratesensors); // request raw sensor
//comPort.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.RC_CHANNELS, 3); // request rc info
}
catch
{
CustomMessageBox.Show("Comport open failed");
return;
}
timer1.Start();
}
List<string[]> readLog(string fn)
{
List<string[]> list = new List<string[]>();
if (fn.ToLower().EndsWith("tlog"))
{
MAVLink mine = new MAVLink();
mine.logplaybackfile = new BinaryReader(File.Open(fn, FileMode.Open, FileAccess.Read, FileShare.Read));
mine.logreadmode = true;
mine.packets.Initialize(); // clear
CurrentState cs = new CurrentState();
string[] oldvalues = {""};
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
byte[] packet = mine.readPacket();
cs.datetime = mine.lastlogread;
cs.UpdateCurrentSettings(null, true, mine);
// line "GPS: 82686250, 1, 8, -34.1406480, 118.5441900, 0.0000, 309.1900, 315.9500, 0.0000, 279.1200" string
string[] vals = new string[] { "GPS", (cs.datetime - new DateTime(cs.datetime.Year,cs.datetime.Month,cs.datetime.Day,0,0,0,DateTimeKind.Local)).TotalMilliseconds.ToString(), "1",
cs.satcount.ToString(),cs.lat.ToString(),cs.lng.ToString(),"0.0",cs.alt.ToString(),cs.alt.ToString(),"0.0",cs.groundcourse.ToString()};
if (oldvalues.Length > 2 && oldvalues[latpos] == vals[latpos]
&& oldvalues[lngpos] == vals[lngpos]
&& oldvalues[altpos] == vals[altpos])
continue;
oldvalues = vals;
list.Add(vals);
// 4 5 7
Console.Write((mine.logplaybackfile.BaseStream.Position * 100 / mine.logplaybackfile.BaseStream.Length) + " \r");
}
mine.logplaybackfile.Close();
return list;
}
StreamReader sr = new StreamReader(fn);
string lasttime = "0";
while (!sr.EndOfStream)
{
string line = sr.ReadLine();
if (line.ToLower().StartsWith("gps"))
{
string[] vals = line.Split(new char[] {',',':'});
if (lasttime == vals[1])
continue;
lasttime = vals[1];
list.Add(vals);
}
}
sr.Close();
sr.Dispose();
return list;
}
