public static string ValueToDescripton(string value)
{
if (Firmwares.ContainsKey(value))
{
return(Firmwares[value]);
}
if (SoundCards.ContainsKey(value))
{
return(SoundCards[value]);
}
if (NICs.ContainsKey(value))
{
return(NICs[value]);
}
return("");
}
public static List <Firmwares> Listar(string fabricante, string modelo)
{
List <Firmwares> listaFirmwares = new List <Firmwares>();
DataTable dtFirmwares = DAO.retornadt(ConfigurationManager.ConnectionStrings["dnaprint"].ToString(), string.Format("select distinct idPerfil, firmware from cadastroperfiloid where fabricante = '{0}' and modelo = '{1}'", fabricante, modelo));
if (dtFirmwares.Rows.Count > 0)
{
foreach (DataRow p in dtFirmwares.Rows)
{
Firmwares firm = new Firmwares();
firm.IdFirmware = p["idPerfil"].ToString();
firm.Firmwares1 = p["firmware"].ToString();
listaFirmwares.Add(firm);
}
}
return(listaFirmwares);
}
public static List <KeyValuePair <int, string> > getModesList(Firmwares firmware)
{
log.Info("getModesList Called");
if (firmware == 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 (firmware == Firmwares.ArduPlane)
{
var flightModes = Utilities.ParameterMetaDataRepository.GetParameterOptionsInt("FLTMODE1",
firmware.ToString());
flightModes.Add(new KeyValuePair <int, string>(16, "INITIALISING"));
return(flightModes);
}
else if (firmware == Firmwares.Ateryx)
{
var flightModes = Utilities.ParameterMetaDataRepository.GetParameterOptionsInt("FLTMODE1",
firmware.ToString()); //same as apm
return(flightModes);
}
else if (firmware == Firmwares.ArduCopter2)
{
var flightModes = Utilities.ParameterMetaDataRepository.GetParameterOptionsInt("FLTMODE1",
firmware.ToString());
return(flightModes);
}
else if (firmware == Firmwares.ArduRover)
{
var flightModes = Utilities.ParameterMetaDataRepository.GetParameterOptionsInt("MODE1",
firmware.ToString());
return(flightModes);
}
else if (firmware == 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 async Task StartSwarmSeperate(Firmwares firmware)
{
var max = 10;
if (InputBox.Show("how many?", "how many?", ref max) != DialogResult.OK)
{
return;
}
// kill old session
try
{
simulator.ForEach(a =>
{
try
{
a.Kill();
}
catch { }
});
}
catch
{
}
Task <string> exepath;
string model = "";
if (firmware == Firmwares.ArduPlane)
{
exepath = CheckandGetSITLImage("ArduPlane.elf");
model = "plane";
}
else
if (firmware == Firmwares.ArduRover)
{
exepath = CheckandGetSITLImage("ArduRover.elf");
model = "rover";
}
else // (firmware == Firmwares.ArduCopter2)
{
exepath = CheckandGetSITLImage("ArduCopter.elf");
model = "+";
}
var config = await GetDefaultConfig(model);
max--;
for (int a = (int)max; a >= 0; a--)
{
var extra = " --disable-fgview -r50 ";
if (!string.IsNullOrEmpty(config))
{
extra += @" --defaults """ + config + @",identity.parm"" -P SERIAL0_PROTOCOL=2 -P SERIAL1_PROTOCOL=2 ";
}
var home = new PointLatLngAlt(markeroverlay.Markers[0].Position).newpos((double)NUM_heading.Value, a * 4);
if (max == a)
{
extra += String.Format(
" -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ",
a, "", a + 1, BuildHomeLocation(home, (int)NUM_heading.Value), model);
}
else
{
extra += String.Format(
" -M{4} -s1 --home {3} --instance {0} --uartA tcp:0 {1} -P SYSID_THISMAV={2} ",
a, "" /*"--uartD tcpclient:127.0.0.1:" + (5770 + 10 * a)*/, a + 1,
BuildHomeLocation(home, (int)NUM_heading.Value), model);
}
string simdir = sitldirectory + model + (a + 1) + Path.DirectorySeparatorChar;
Directory.CreateDirectory(simdir);
File.WriteAllText(simdir + "identity.parm", String.Format(@"SERIAL0_PROTOCOL=2
SERIAL1_PROTOCOL=2
SYSID_THISMAV={0}
SIM_TERRAIN=0
TERRAIN_ENABLE=0
SCHED_LOOP_RATE=50
SIM_RATE_HZ=400
SIM_DRIFT_SPEED=0
SIM_DRIFT_TIME=0
", a + 1));
string path = Environment.GetEnvironmentVariable("PATH");
Environment.SetEnvironmentVariable("PATH", sitldirectory + ";" + simdir + ";" + path,
EnvironmentVariableTarget.Process);
Environment.SetEnvironmentVariable("HOME", simdir, EnvironmentVariableTarget.Process);
ProcessStartInfo exestart = new ProcessStartInfo();
exestart.FileName = await exepath;
exestart.Arguments = extra;
exestart.WorkingDirectory = simdir;
exestart.WindowStyle = ProcessWindowStyle.Minimized;
exestart.UseShellExecute = true;
simulator.Add(System.Diagnostics.Process.Start(exestart));
await Task.Delay(100);
}
await Task.Delay(2000);
MainV2.View.ShowScreen(MainV2.View.screens[0].Name);
try
{
Parallel.For(0, max + 1, (a) =>
//for (int a = (int)max; a >= 0; a--)
{
var mav = new MAVLinkInterface();
var client = new Comms.TcpSerial();
client.client = new TcpClient("127.0.0.1", 5760 + (10 * (a)));
mav.BaseStream = client;
//SITLSEND = new UdpClient("127.0.0.1", 5501);
Thread.Sleep(200);
this.InvokeIfRequired(() => { MainV2.instance.doConnect(mav, "preset", "5760", false); });
lock (this)
MainV2.Comports.Add(mav);
try
{
_ = mav.getParamListMavftpAsync((byte)mav.sysidcurrent, (byte)mav.compidcurrent);
}
catch
{
}
}
);
return;
}
catch (Exception ex)
{
log.Error(ex);
CustomMessageBox.Show(Strings.Failed_to_connect_to_SITL_instance +
ex.InnerException?.Message, Strings.ERROR);
return;
}
}
public static List <KeyValuePair <int, string> > getModesList(Firmwares csFirmware)
{
return(ArduPilot.Common.getModesList(csFirmware));
}
public List <Firmwares> RetornaFirmwares(string fabricante, string modelo)
{
List <Firmwares> listaFirmwares = Firmwares.Listar(fabricante, modelo);
return(listaFirmwares);
}
public void Update(UasHeartbeat hb)
{
Autopilot = (MavAutopilot)hb.Autopilot;
Type = (MavType)hb.Type;
switch (Autopilot)
{
case MavAutopilot.Ardupilotmega:
switch (Type)
{
case MavType.FixedWing:
Firmware = Firmwares.ArduPlane;
break;
case MavType.Quadrotor:
case MavType.Coaxial:
case MavType.Helicopter:
case MavType.Octorotor:
case MavType.Tricopter:
case MavType.Hexarotor:
Firmware = Firmwares.ArduCopter2;
break;
case MavType.AntennaTracker:
Firmware = Firmwares.ArduTracker;
break;
case MavType.GroundRover:
case MavType.SurfaceBoat:
Firmware = Firmwares.ArduRover;
break;
case MavType.Submarine:
Firmware = Firmwares.ArduSub;
break;
default:
Firmware = Firmwares.Unknown;
break;
}
break;
case MavAutopilot.Px4:
Firmware = Firmwares.PX4;
break;
case MavAutopilot.Generic:
case MavAutopilot.Reserved:
case MavAutopilot.Slugs:
case MavAutopilot.Openpilot:
case MavAutopilot.GenericWaypointsOnly:
case MavAutopilot.GenericWaypointsAndSimpleNavigationOnly:
case MavAutopilot.GenericMissionFull:
case MavAutopilot.Invalid:
case MavAutopilot.Ppz:
case MavAutopilot.Udb:
case MavAutopilot.Fp:
case MavAutopilot.Smaccmpilot:
case MavAutopilot.Autoquad:
case MavAutopilot.Armazila:
case MavAutopilot.Aerob:
case MavAutopilot.Asluav:
case MavAutopilot.Smartap:
case MavAutopilot.Airrails:
Firmware = Firmwares.Unknown;
break;
}
Armed = (hb.BaseMode & (byte)MavModeFlag.SafetyArmed) == (byte)MavModeFlag.SafetyArmed;
Guided = (hb.BaseMode & (byte)MavModeFlag.GuidedEnabled) == (byte)MavModeFlag.GuidedEnabled;
HardwareInLoopSimulation = (hb.BaseMode & (byte)MavModeFlag.HilEnabled) == (byte)MavModeFlag.HilEnabled;
Autonomous = (hb.BaseMode & (byte)MavModeFlag.AutoEnabled) == (byte)MavModeFlag.AutoEnabled;
Stabilized = (hb.BaseMode & (byte)MavModeFlag.StabilizeEnabled) == (byte)MavModeFlag.StabilizeEnabled;
Manual = (hb.BaseMode & (byte)MavModeFlag.ManualInputEnabled) == (byte)MavModeFlag.ManualInputEnabled;
var customModeEnabled = (hb.BaseMode & (byte)MavModeFlag.CustomModeEnabled) == (byte)MavModeFlag.CustomModeEnabled;
if (customModeEnabled)
{
if (_customModeType != hb.CustomMode)
{
_customModeType = hb.CustomMode;
var modelist = Modes.getModesList(Firmware);
if (modelist != null)
{
var customMode = modelist.Where(mod => mod.Key == hb.CustomMode).FirstOrDefault();
if (customMode.Key == hb.CustomMode)
{
CustomMode = customMode.Value;
}
else
{
CustomMode = String.Empty;
}
}
}
}
else
{
CustomMode = String.Empty;
}
Standby = hb.SystemStatus == (byte)MavState.Standby;
FlightTermination = hb.SystemStatus == (byte)MavState.FlightTermination;
Unknown = hb.SystemStatus == (byte)MavState.Uninit;
Calibrating = hb.SystemStatus == (byte)MavState.Calibrating;
Critical = hb.SystemStatus == (byte)MavState.Critical;
Emergency = hb.SystemStatus == (byte)MavState.Emergency;
Poweroff = hb.SystemStatus == (byte)MavState.Poweroff;
Active = hb.SystemStatus == (byte)MavState.Active;
Boot = hb.SystemStatus == (byte)MavState.Boot;
GaugeStatus = Emergency || Critical ? GaugeStatus.Error : GaugeStatus.OK;
TimeStamp = DateTime.Now;
}
public static List <KeyValuePair <int, string> > getModesList(Firmwares csFirmware)
{
return(ArduPilot.Common.getModesList(MainV2.comPort.MAV.cs.firmware));
}
public static List <KeyValuePair <int, string> > getModesList(Firmwares firmware)
{
if (firmware == 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)PX4CustomModes.Manual << 16, CoreResources.PX4Mode_Manual),
new KeyValuePair <int, string>((int)PX4CustomModes.Acro << 16, CoreResources.PX4Mode_Acro),
new KeyValuePair <int, string>((int)PX4CustomModes.Stablilized << 16, CoreResources.PX4Mode_Stabilized),
new KeyValuePair <int, string>((int)PX4CustomModes.Rattitude << 16, CoreResources.PX4Mode_Stabilized),
new KeyValuePair <int, string>((int)PX4CustomModes.AltCtl << 16, CoreResources.PX4Mode_AltCtl),
new KeyValuePair <int, string>((int)PX4CustomModes.PosCtl << 16, CoreResources.PX4Mode_PoCtl),
new KeyValuePair <int, string>((int)PX4CustomModes.OffBoard << 16, CoreResources.PX4Mode_Offboard),
new KeyValuePair <int, string>(
((int)PX4CustomModes.Auto << 16) +
(int)PX4CustomSubodes.Ready << 24, CoreResources.PX4Mode_AutoReady),
new KeyValuePair <int, string>(
((int)PX4CustomModes.Auto << 16) +
(int)PX4CustomSubodes.Takeoff << 24, CoreResources.PX4Mode_AutoTakeoff),
new KeyValuePair <int, string>(
((int)PX4CustomModes.Auto << 16) +
(int)PX4CustomSubodes.Loiter << 24, CoreResources.PX4Mode_Loiter),
new KeyValuePair <int, string>(
((int)PX4CustomModes.Auto << 16) +
(int)PX4CustomSubodes.Mission << 24, CoreResources.PX4Mode_Mission),
new KeyValuePair <int, string>(
((int)PX4CustomModes.Auto << 16) +
(int)PX4CustomSubodes.RTL << 24, CoreResources.PX4Mode_RTL),
new KeyValuePair <int, string>(
((int)PX4CustomModes.Auto << 16) +
(int)PX4CustomSubodes.Land << 24, CoreResources.PX4Mode_Landing)
};
return(temp);
}
else if (firmware == Firmwares.ArduPlane)
{
var flightModes = Parameters.GetParameterOptionsInt("FLTMODE1",
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"));
flightModes.Add(new KeyValuePair <int, string>(21, "QRTL"));
return(flightModes);
}
else if (firmware == Firmwares.Ateryx)
{
var flightModes = Parameters.GetParameterOptionsInt("FLTMODE1", firmware.ToString()); //same as apm
return(flightModes);
}
else if (firmware == Firmwares.ArduCopter2)
{
var flightModes = Parameters.GetParameterOptionsInt("FLTMODE1", firmware.ToString());
return(flightModes);
}
else if (firmware == Firmwares.ArduRover)
{
var flightModes = Parameters.GetParameterOptionsInt("MODE1", firmware.ToString());
return(flightModes);
}
else if (firmware == Firmwares.ArduTracker)
{
var temp = new List <KeyValuePair <int, string> >();
temp.Add(new KeyValuePair <int, string>(0, CoreResources.TrackerMode_Manual));
temp.Add(new KeyValuePair <int, string>(1, CoreResources.TrackerMode_Stop));
temp.Add(new KeyValuePair <int, string>(2, CoreResources.TrackerMode_Scan));
temp.Add(new KeyValuePair <int, string>(3, CoreResources.TrackerMode_ServoTest));
temp.Add(new KeyValuePair <int, string>(10, CoreResources.TrackerMode_Auto));
temp.Add(new KeyValuePair <int, string>(16, CoreResources.TrackerMode_Initializing));
return(temp);
}
return(null);
}
private void xmlconfig(bool write)
{
if (write || !File.Exists(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"config.xml"))
{
try
{
//System.Configuration.Configuration appconfig = System.Configuration.ConfigurationManager.OpenExeConfiguration(System.Configuration.ConfigurationUserLevel.None);
XmlTextWriter xmlwriter = new XmlTextWriter(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"config.xml", Encoding.ASCII);
xmlwriter.Formatting = Formatting.Indented;
xmlwriter.WriteStartDocument();
xmlwriter.WriteStartElement("Config");
xmlwriter.WriteElementString("comport", comportname);
xmlwriter.WriteElementString("baudrate", CMB_baudrate.Text);
xmlwriter.WriteElementString("APMFirmware", APMFirmware.ToString());
//appconfig.AppSettings.Settings.Add("comport", comportname);
//appconfig.AppSettings.Settings.Add("baudrate", CMB_baudrate.Text);
//appconfig.AppSettings.Settings.Add("APMFirmware", APMFirmware.ToString());
foreach (string key in config.Keys)
{
try
{
if (key == "")
continue;
xmlwriter.WriteElementString(key, config[key].ToString());
//appconfig.AppSettings.Settings.Add(key, config[key].ToString());
}
catch { }
}
xmlwriter.WriteEndElement();
xmlwriter.WriteEndDocument();
xmlwriter.Close();
//appconfig.Save();
}
catch (Exception ex) { MessageBox.Show(ex.ToString()); }
}
else
{
try
{
using (XmlTextReader xmlreader = new XmlTextReader(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"config.xml"))
{
while (xmlreader.Read())
{
xmlreader.MoveToElement();
try
{
switch (xmlreader.Name)
{
case "comport":
string temp = xmlreader.ReadString();
CMB_serialport.SelectedIndex = CMB_serialport.FindString(temp);
if (CMB_serialport.SelectedIndex == -1)
{
CMB_serialport.Text = temp; // allows ports that dont exist - yet
}
comPort.BaseStream.PortName = temp;
comportname = temp;
break;
case "baudrate":
string temp2 = xmlreader.ReadString();
CMB_baudrate.SelectedIndex = CMB_baudrate.FindString(temp2);
if (CMB_baudrate.SelectedIndex == -1)
{
CMB_baudrate.Text = temp2;
//CMB_baudrate.SelectedIndex = CMB_baudrate.FindString("57600"); ; // must exist
}
//bau = int.Parse(CMB_baudrate.Text);
break;
case "APMFirmware":
string temp3 = xmlreader.ReadString();
TOOL_APMFirmware.SelectedIndex = TOOL_APMFirmware.FindStringExact(temp3);
if (TOOL_APMFirmware.SelectedIndex == -1)
TOOL_APMFirmware.SelectedIndex = 0;
APMFirmware = (MainV2.Firmwares)Enum.Parse(typeof(MainV2.Firmwares), TOOL_APMFirmware.Text);
break;
case "Config":
break;
case "xml":
break;
default:
if (xmlreader.Name == "") // line feeds
break;
config[xmlreader.Name] = xmlreader.ReadString();
break;
}
}
catch (Exception ee) { Console.WriteLine(ee.Message); } // silent fail on bad entry
}
}
}
catch (Exception ex) { Console.WriteLine("Bad Config File: " + ex.ToString()); } // bad config file
}
}
private void TOOL_APMFirmware_SelectedIndexChanged(object sender, EventArgs e)
{
APMFirmware = (MainV2.Firmwares)Enum.Parse(typeof(MainV2.Firmwares), TOOL_APMFirmware.Text);
MainV2.cs.firmware = APMFirmware;
}