public void timer()
{
Control.CheckForIllegalCrossThreadCalls = false;
videopos = startlogtime.AddMilliseconds(-startlogtime.Millisecond);
hud1.SixteenXNine = true;
while (true)
{
try
{
// GC.Collect();
// long mem = GC.GetTotalMemory(true) / 1024 / 1024;
// Console.WriteLine("mem "+mem);
System.Threading.Thread.Sleep(20); // 25 fps
if (flightdata.Count == 0)
break;
// videopos = videopos.AddMilliseconds(1000 / 25);
// m_mediaseek = m_FilterGraph as IMediaSeeking;
// m_mediapos.put_CurrentPosition((vidpos - startlogtime).TotalSeconds);
//m_mediaseek.SetTimeFormat(TimeFormat.MediaTime);
//long poscurrent = 0;
//long posend = 0;
// m_mediaseek.GetPositions(out poscurrent,out posend);
DateTime cstimestamp =
videopos.AddSeconds(trackBar1.Value).AddMilliseconds(-(videopos.Millisecond%20));
int tb = trackBar1.Value;
if (flightdata.ContainsKey(cstimestamp))
{
cs = flightdata[cstimestamp];
}
else if (flightdata.ContainsKey(cstimestamp.AddMilliseconds(-20)))
{
cs = flightdata[cstimestamp.AddMilliseconds(-20)];
}
else if (flightdata.ContainsKey(cstimestamp.AddMilliseconds(-40)))
{
cs = flightdata[cstimestamp.AddMilliseconds(-40)];
}
// Console.WriteLine("Update CS");
Console.WriteLine("log " + cs.datetime);
hud1.datetime = cs.datetime;
//cs.UpdateCurrentSettings(bindingSource1,true,MainV2.comPort);
bindingSource1.DataSource = cs;
bindingSource1.ResetBindings(false);
}
catch (ThreadAbortException)
{
break;
}
catch
{
}
}
}
// 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;
}
public static List<KeyValuePair<int, string>> getModesList(CurrentState cs)
{
log.Info("getModesList Called");
if (cs.firmware == MainV2.Firmwares.ArduPlane)
{
var flightModes = Utilities.ParameterMetaDataRepository.GetParameterOptionsInt("FLTMODE1",
cs.firmware.ToString());
flightModes.Add(new KeyValuePair<int, string>(16, "INITIALISING"));
return flightModes;
}
else if (cs.firmware == MainV2.Firmwares.Ateryx)
{
var flightModes = Utilities.ParameterMetaDataRepository.GetParameterOptionsInt("FLTMODE1",
cs.firmware.ToString()); //same as apm
return flightModes;
}
else if (cs.firmware == MainV2.Firmwares.ArduCopter2)
{
var flightModes = Utilities.ParameterMetaDataRepository.GetParameterOptionsInt("FLTMODE1",
cs.firmware.ToString());
return flightModes;
}
else if (cs.firmware == MainV2.Firmwares.ArduRover)
{
var flightModes = Utilities.ParameterMetaDataRepository.GetParameterOptionsInt("MODE1",
cs.firmware.ToString());
return flightModes;
}
else if (cs.firmware == MainV2.Firmwares.ArduTracker)
{
var temp = new List<KeyValuePair<int, string>>();
temp.Add(new KeyValuePair<int, string>(0, "MANUAL"));
temp.Add(new KeyValuePair<int, string>(1, "STOP"));
temp.Add(new KeyValuePair<int, string>(2, "SCAN"));
temp.Add(new KeyValuePair<int, string>(3, "SERVO_TEST"));
temp.Add(new KeyValuePair<int, string>(10, "AUTO"));
temp.Add(new KeyValuePair<int, string>(16, "INITIALISING"));
return temp;
}
return null;
}
public static List<KeyValuePair<int, string>> getModesList(CurrentState cs)
{
log.Info("getModesList Called");
// ensure we get the correct list
MainV2.comPort.MAV.cs.firmware = cs.firmware;
if (cs.firmware == MainV2.Firmwares.ArduPlane)
{
var flightModes = Utilities.ParameterMetaDataRepository.GetParameterOptionsInt("FLTMODE1");
flightModes.Add(new KeyValuePair<int, string>(16, "INITIALISING"));
return flightModes;
}
else if (cs.firmware == MainV2.Firmwares.Ateryx)
{
var flightModes = Utilities.ParameterMetaDataRepository.GetParameterOptionsInt("FLTMODE1"); //same as apm
return flightModes;
}
else if (cs.firmware == MainV2.Firmwares.ArduCopter2)
{
var flightModes = Utilities.ParameterMetaDataRepository.GetParameterOptionsInt("FLTMODE1");
return flightModes;
}
else if (cs.firmware == MainV2.Firmwares.ArduRover)
{
var flightModes = Utilities.ParameterMetaDataRepository.GetParameterOptionsInt("MODE1");
return flightModes;
}
return null;
}
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;
}
// 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;
}
public static List<KeyValuePair<int, string>> getModesList(CurrentState cs)
{
log.Info("getModesList Called");
if (cs.firmware == MainV2.Firmwares.PX4)
{
/*
union px4_custom_mode {
struct {
uint16_t reserved;
uint8_t main_mode;
uint8_t sub_mode;
};
uint32_t data;
float data_float;
};
*/
var temp = new List<KeyValuePair<int, string>>()
{
new KeyValuePair<int, string>((int) PX4_CUSTOM_MAIN_MODE.PX4_CUSTOM_MAIN_MODE_MANUAL << 16, "Manual"),
new KeyValuePair<int, string>((int) PX4_CUSTOM_MAIN_MODE.PX4_CUSTOM_MAIN_MODE_ACRO << 16, "Acro"),
new KeyValuePair<int, string>((int) PX4_CUSTOM_MAIN_MODE.PX4_CUSTOM_MAIN_MODE_STABILIZED << 16,
"Stabalized"),
new KeyValuePair<int, string>((int) PX4_CUSTOM_MAIN_MODE.PX4_CUSTOM_MAIN_MODE_RATTITUDE << 16,
"Rattitude"),
new KeyValuePair<int, string>((int) PX4_CUSTOM_MAIN_MODE.PX4_CUSTOM_MAIN_MODE_ALTCTL << 16,
"Altitude Control"),
new KeyValuePair<int, string>((int) PX4_CUSTOM_MAIN_MODE.PX4_CUSTOM_MAIN_MODE_POSCTL << 16,
"Position Control"),
new KeyValuePair<int, string>((int) PX4_CUSTOM_MAIN_MODE.PX4_CUSTOM_MAIN_MODE_OFFBOARD << 16,
"Offboard Control"),
new KeyValuePair<int, string>(
((int) PX4_CUSTOM_MAIN_MODE.PX4_CUSTOM_MAIN_MODE_AUTO << 16) +
(int) PX4_CUSTOM_SUB_MODE_AUTO.PX4_CUSTOM_SUB_MODE_AUTO_READY << 24, "Auto: Ready"),
new KeyValuePair<int, string>(
((int) PX4_CUSTOM_MAIN_MODE.PX4_CUSTOM_MAIN_MODE_AUTO << 16) +
(int) PX4_CUSTOM_SUB_MODE_AUTO.PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF << 24, "Auto: Takeoff"),
new KeyValuePair<int, string>(
((int) PX4_CUSTOM_MAIN_MODE.PX4_CUSTOM_MAIN_MODE_AUTO << 16) +
(int) PX4_CUSTOM_SUB_MODE_AUTO.PX4_CUSTOM_SUB_MODE_AUTO_LOITER << 24, "Loiter"),
new KeyValuePair<int, string>(
((int) PX4_CUSTOM_MAIN_MODE.PX4_CUSTOM_MAIN_MODE_AUTO << 16) +
(int) PX4_CUSTOM_SUB_MODE_AUTO.PX4_CUSTOM_SUB_MODE_AUTO_MISSION << 24, "Auto"),
new KeyValuePair<int, string>(
((int) PX4_CUSTOM_MAIN_MODE.PX4_CUSTOM_MAIN_MODE_AUTO << 16) +
(int) PX4_CUSTOM_SUB_MODE_AUTO.PX4_CUSTOM_SUB_MODE_AUTO_RTL << 24, "RTL"),
new KeyValuePair<int, string>(
((int) PX4_CUSTOM_MAIN_MODE.PX4_CUSTOM_MAIN_MODE_AUTO << 16) +
(int) PX4_CUSTOM_SUB_MODE_AUTO.PX4_CUSTOM_SUB_MODE_AUTO_LAND << 24, "Auto: Landing")
};
return temp;
}
else if (cs.firmware == MainV2.Firmwares.ArduPlane)
{
var flightModes = Utilities.ParameterMetaDataRepository.GetParameterOptionsInt("FLTMODE1",
cs.firmware.ToString());
flightModes.Add(new KeyValuePair<int, string>(16, "INITIALISING"));
flightModes.Add(new KeyValuePair<int, string>(17, "QStabilize"));
flightModes.Add(new KeyValuePair<int, string>(18, "QHover"));
flightModes.Add(new KeyValuePair<int, string>(19, "QLoiter"));
flightModes.Add(new KeyValuePair<int, string>(20, "QLand"));
return flightModes;
}
else if (cs.firmware == MainV2.Firmwares.Ateryx)
{
var flightModes = Utilities.ParameterMetaDataRepository.GetParameterOptionsInt("FLTMODE1",
cs.firmware.ToString()); //same as apm
return flightModes;
}
else if (cs.firmware == MainV2.Firmwares.ArduCopter2)
{
var flightModes = Utilities.ParameterMetaDataRepository.GetParameterOptionsInt("FLTMODE1",
cs.firmware.ToString());
return flightModes;
}
else if (cs.firmware == MainV2.Firmwares.ArduRover)
{
var flightModes = Utilities.ParameterMetaDataRepository.GetParameterOptionsInt("MODE1",
cs.firmware.ToString());
return flightModes;
}
else if (cs.firmware == MainV2.Firmwares.ArduTracker)
{
var temp = new List<KeyValuePair<int, string>>();
temp.Add(new KeyValuePair<int, string>(0, "MANUAL"));
temp.Add(new KeyValuePair<int, string>(1, "STOP"));
temp.Add(new KeyValuePair<int, string>(2, "SCAN"));
temp.Add(new KeyValuePair<int, string>(3, "SERVO_TEST"));
temp.Add(new KeyValuePair<int, string>(10, "AUTO"));
temp.Add(new KeyValuePair<int, string>(16, "INITIALISING"));
return temp;
}
return null;
}
public static List<KeyValuePair<int, string>> getModesList(CurrentState cs)
{
log.Info("getModesList Called");
if (cs.firmware == MainV2.Firmwares.PX4)
{
/*
union px4_custom_mode {
struct {
uint16_t reserved;
uint8_t main_mode;
uint8_t sub_mode;
};
uint32_t data;
float data_float;
};
*/
var temp = new List<KeyValuePair<int, string>>();
temp.Add(new KeyValuePair<int, string>(1 << 8, "Manual"));
temp.Add(new KeyValuePair<int, string>(2 << 8, "Acro"));
temp.Add(new KeyValuePair<int, string>(3 << 8, "Stabalized"));
temp.Add(new KeyValuePair<int, string>(4 << 8, "Rattitude"));
temp.Add(new KeyValuePair<int, string>(5 << 8, "Altitude Control"));
temp.Add(new KeyValuePair<int, string>(6 << 8, "Position Control"));
temp.Add(new KeyValuePair<int, string>(7 << 8, "Offboard Control"));
temp.Add(new KeyValuePair<int, string>(8 << 8 + 1, "Auto: Ready"));
temp.Add(new KeyValuePair<int, string>(8 << 8 + 2, "Auto: Takeoff"));
temp.Add(new KeyValuePair<int, string>(8 << 8 + 3, "Loiter"));
temp.Add(new KeyValuePair<int, string>(8 << 8 + 4, "Auto"));
temp.Add(new KeyValuePair<int, string>(8 << 8 + 5, "RTL"));
temp.Add(new KeyValuePair<int, string>(8 << 8 + 6, "Auto: Landing"));
return temp;
} else if (cs.firmware == MainV2.Firmwares.ArduPlane)
{
var flightModes = Utilities.ParameterMetaDataRepository.GetParameterOptionsInt("FLTMODE1",
cs.firmware.ToString());
flightModes.Add(new KeyValuePair<int, string>(16, "INITIALISING"));
flightModes.Add(new KeyValuePair<int, string>(17, "QStabilize"));
flightModes.Add(new KeyValuePair<int, string>(18, "QHover"));
flightModes.Add(new KeyValuePair<int, string>(19, "QLoiter"));
flightModes.Add(new KeyValuePair<int, string>(20, "QLand"));
return flightModes;
}
else if (cs.firmware == MainV2.Firmwares.Ateryx)
{
var flightModes = Utilities.ParameterMetaDataRepository.GetParameterOptionsInt("FLTMODE1",
cs.firmware.ToString()); //same as apm
return flightModes;
}
else if (cs.firmware == MainV2.Firmwares.ArduCopter2)
{
var flightModes = Utilities.ParameterMetaDataRepository.GetParameterOptionsInt("FLTMODE1",
cs.firmware.ToString());
return flightModes;
}
else if (cs.firmware == MainV2.Firmwares.ArduRover)
{
var flightModes = Utilities.ParameterMetaDataRepository.GetParameterOptionsInt("MODE1",
cs.firmware.ToString());
return flightModes;
}
else if (cs.firmware == MainV2.Firmwares.ArduTracker)
{
var temp = new List<KeyValuePair<int, string>>();
temp.Add(new KeyValuePair<int, string>(0, "MANUAL"));
temp.Add(new KeyValuePair<int, string>(1, "STOP"));
temp.Add(new KeyValuePair<int, string>(2, "SCAN"));
temp.Add(new KeyValuePair<int, string>(3, "SERVO_TEST"));
temp.Add(new KeyValuePair<int, string>(10, "AUTO"));
temp.Add(new KeyValuePair<int, string>(16, "INITIALISING"));
return temp;
}
return null;
}
async private Task <DateTime> GetFirstSustainedTriggerTime(string fn)
{
DateTime SusTrigTime = new DateTime();
Dictionary <long, VehicleLocation> vehicletriggerList = new Dictionary <long, VehicleLocation>();
// Telemetry Log
if (fn.ToLower().EndsWith("tlog"))
{
System.IO.FileStream logplaybackfile = new System.IO.FileStream(fn, FileMode.Open);
MAVLinkInterface mine = new MAVLinkInterface(logplaybackfile);
bool WasLastOn = false;
MissionPlanner.CurrentState cs = new MissionPlanner.CurrentState();
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
{
MAVLink.MAVLinkMessage packet = mine.readPacket();
cs.datetime = mine.lastlogread;
cs.UpdateCurrentSettings(null, true, mine);
VehicleLocation location = new VehicleLocation();
location.Time = cs.datetime;
bool tmponval = false;
if (((cs.ch7in > 1520) || (cs.ch7in < 1480)) && (cs.ch7in != 0)) //if ch7 val is not default we assume camera is on
{
tmponval = true;
if (!WasLastOn)
{
SusTrigTime = cs.datetime;
}
else
{
// logic to check if we have a sustained trigger
System.TimeSpan SusTime = cs.datetime - SusTrigTime;
double Sustimesec = SusTime.TotalSeconds;
if (Sustimesec > 30)
{
mine.logplaybackfile.Close();
return(SusTrigTime);
}
}
WasLastOn = true;
}
else
{
if (cs.ch7in != 0)
{
tmponval = false;
if (WasLastOn)
{
// logic to check if we have a sustained trigger
System.TimeSpan SusTime = cs.datetime - SusTrigTime;
double Sustimesec = SusTime.TotalSeconds;
if (Sustimesec > 30)
{
mine.logplaybackfile.Close();
return(SusTrigTime);
}
}
}
WasLastOn = false;
}
}
mine.logplaybackfile.Close();
}
return(SusTrigTime);
}
