/// <summary>
/// Commands the drone to turn to the specified
/// turn, a.k.a turn * degree interval.
/// </summary>
/// <param name="turn"></param>
/// <returns></returns>
public static bool GoToTurn(int turn)
{
if (turn < 0 || turn >= GetNumberOfTurns())
{
return(false);
}
// THIS WILL BLOCK! hopefully OK
if (MavLinkCom.doCommand(
system_id,
0,
MAVLink.MAV_CMD.CONDITION_YAW,
turn * GetDirectionResolution(), // yaw angle
10, // yaw rate
1, // direction (1 is clockwise)
0, // reference frame (0 is absolute)
0, 0, 0, // unused
false)) // require ack
{
current_turn = turn;
return(true);
}
else
{
return(false);
}
}
private void Lbl_bootloaderupdate_Click(object sender, EventArgs e)
{
// connect to mavlink
var mav = new MAVLinkInterface();
MainV2.instance.doConnect(mav, MainV2._connectionControl.CMB_serialport.Text,
MainV2._connectionControl.CMB_baudrate.Text, false);
if (mav.BaseStream == null || !mav.BaseStream.IsOpen)
{
return;
}
if (CustomMessageBox.Show("Are you sure you want to upgrade the bootloader? This can brick your board",
"BL Update", MessageBoxButtons.YesNo, MessageBoxIcon.Warning) == (int)DialogResult.Yes)
{
if (CustomMessageBox.Show("Are you sure you want to upgrade the bootloader? This can brick your board, Please allow 5 mins for this process",
"BL Update", MessageBoxButtons.YesNo, MessageBoxIcon.Warning) == (int)DialogResult.Yes)
{
if (mav.doCommand(MAVLink.MAV_CMD.FLASH_BOOTLOADER, 0, 0, 0, 0, 290876, 0, 0))
{
CustomMessageBox.Show("Upgraded bootloader");
}
else
{
CustomMessageBox.Show("Failed to upgrade bootloader");
}
}
}
mav?.Close();
}
public void Discover(MAVLinkInterface mint)
{
mint.doCommand(0, 0, MAVLink.MAV_CMD.REQUEST_MESSAGE,
(float)MAVLink.MAVLINK_MSG_ID.GIMBAL_DEVICE_INFORMATION,
0, 0, 0, 0, 0, 0, false);
mint.OnPacketReceived += (sender, message) =>
{
if (message.msgid == (uint)MAVLink.MAVLINK_MSG_ID.GIMBAL_DEVICE_INFORMATION)
{
var gi = (MAVLink.mavlink_gimbal_device_information_t)message.data;
lock (List)
{
List[gi.uid] = message;
}
}
};
}
private void FWUpload(object?a)
{
try
{
Tuple <int, string> passin = (Tuple <int, string>)a;
var index = passin.Item1;
var portname = passin.Item2;
var tryupload = true;
lock (ports)
if (ports.Contains(portname))
{
tryupload = false;
}
if (tryupload)
{
var fw = Firmware.ProcessFirmware(_args[0]);
Uploader up = null;
try
{
up = new px4uploader.Uploader(new SerialPort(portname, 115200));
}
catch
{
}
if (up != null)
{
up.ProgressEvent += completed =>
{
/* this.BeginInvoke(new Action(() =>
* {
* textBox1.AppendText(portname + " " + (int)completed + "\r\n");
* }));*/
};
bool flashit = false;
bool validcomms = false;
try
{
up.identify();
validcomms = true;
up.currentChecksum(fw);
flashit = true;
}
catch (Exception ex)
{
AppendLine(ex.Message);
}
if (flashit)
{
this.BeginInvoke(new Action(() => { textBox1.AppendText(portname + " Flashing fw\r\n"); }));
up.upload(fw);
}
else
{
this.BeginInvoke(new Action(() => { textBox1.AppendText(portname + " reboot\r\n"); }));
up.__reboot();
}
lock (ports)
ports.Add(portname);
up.close();
this.BeginInvoke(new Action(() => { textBox1.AppendText(portname + " Done fw\r\n"); }));
if (validcomms)
{
return;
}
}
}
var mav = new MAVLinkInterface();
mav.BaseStream = new SerialPort((string)portname, 115200);
var bldone = false;
mav.OnPacketReceived += (sender, message) =>
{
if (message.msgid == (ulong)MAVLink.MAVLINK_MSG_ID.STATUSTEXT)
{
if (ASCIIEncoding.ASCII.GetString(message.ToStructure <MAVLink.mavlink_statustext_t>().text).Trim().Contains("Bootloader up-to-date") ||
ASCIIEncoding.ASCII.GetString(message.ToStructure <MAVLink.mavlink_statustext_t>().text).Trim().Contains("Flash OK"))
{
bldone = true;
}
DisplayText(message);
}
};
try
{
mav.BaseStream.Open();
mav.getHeartBeat();
mav.doCommand(MAVLink.MAV_CMD.FLASH_BOOTLOADER, 0, 0, 0, 0, 290876, 0, 0, false);
mav.getHeartBeat();
if (!bldone)
{
mav.getHeartBeat();
}
if (!bldone)
{
mav.getHeartBeat();
}
if (!bldone)
{
mav.getHeartBeat();
}
if (!bldone)
{
mav.getHeartBeat();
}
if (!bldone)
{
mav.getHeartBeat();
}
if (!bldone)
{
mav.getHeartBeat();
}
if (!bldone)
{
mav.getHeartBeat();
}
if (!bldone)
{
mav.getHeartBeat();
}
mav.Close();
this.BeginInvoke(new Action(() =>
{
textBox1.AppendText(portname + " Done bl " + (bldone ? "OK" : "FAIL") + "\r\n");
}));
}
catch (Exception ex)
{
AppendLine(ex.Message);
}
}
catch (Exception ex)
{
AppendLine(ex.Message);
}
}
public bool SetMotorState(MAVLinkInterface mint, byte sysid, byte compid, control_motor_t type)
{
return(mint.doCommand(sysid, compid, MAVLink.MAV_CMD.USER_1, 0, 0, 0,
0, 0, 0, (byte)type));
}
public bool SetRCMode(MAVLinkInterface mint, byte sysid, byte compid)
{
return(mint.doCommand(sysid, compid, MAVLink.MAV_CMD.DO_MOUNT_CONFIGURE,
(int)MAVLink.MAV_MOUNT_MODE.RC_TARGETING, 0, 0,
0, 0, 0, 0));
}
public bool Reboot(MAVLinkInterface mint, byte sysid, byte compid)
{
return(mint.doCommand(sysid, compid, MAVLink.MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN, 0, 0, 0, 1, 0, 0, 0));
}
private void FWUpload(object a)
{
Tuple <int, string> passin = (Tuple <int, string>)a;
var index = passin.Item1;
var portname = passin.Item2;
Console.WriteLine("Thread start for " + portname);
try
{
if (!portstatus.ContainsKey(portname))
{
portstatus[portname] = "";
}
{
Uploader up = null;
try
{
up = new px4uploader.Uploader(new SerialPort(portname, 115200));
}
catch
{
}
if (up != null)
{
up.ProgressEvent += completed =>
{
/* this.BeginInvoke(new Action(() =>
* {
* textBox1.AppendText(portname + " " + (int)completed + "\r\n");
* }));*/
};
bool flashit = false;
bool validcomms = false;
Firmware fw = null;
try
{
up.identify();
validcomms = true;
portstatus[portname] += "BL ";
UpdateTextBox();
fw = Firmware.ProcessFirmware(_args[0]);
up.currentChecksum(fw);
fw.board_id = up.board_type;
flashit = true;
portstatus[portname] += "CS ";
UpdateTextBox();
}
catch (Exception ex)
{
if (ex.Message.Contains("Same Firmware"))
{
portstatus[portname] += "BLUPD OK ";
}
else
{
portstatus[portname] += "BLEX ";
}
UpdateTextBox();
}
if (flashit)
{
DateTime ts = DateTime.Now;
up.ProgressEvent += completed =>
{
if (ts.Second != DateTime.Now.Second)
{
Console.WriteLine("{0}: {1}", portname, completed);
ts = DateTime.Now;
}
};
up.upload(fw);
portstatus[portname] += "BLUPD OK ";
UpdateTextBox();
}
else
{
up.__reboot();
UpdateTextBox();
}
up.close();
if (validcomms)
{
return;
}
}
}
var ports = getPortByVPidInt(txt_vid.Text, txt_pid.Text, txt_int.Text);
if (!ports.Contains(portname))
{
portstatus[portname] += "NOTML OK ";
UpdateTextBox();
return;
}
var mav = new MAVLinkInterface();
mav.BaseStream = new SerialPort((string)portname, 115200);
var bldone = false;
mav.OnPacketReceived += (sender, message) =>
{
if (message.msgid == (ulong)MAVLink.MAVLINK_MSG_ID.STATUSTEXT)
{
if (ASCIIEncoding.ASCII.GetString(message.ToStructure <MAVLink.mavlink_statustext_t>().text).Trim().Contains("Bootloader up-to-date") ||
ASCIIEncoding.ASCII.GetString(message.ToStructure <MAVLink.mavlink_statustext_t>().text).Trim().Contains("Flash OK"))
{
bldone = true;
portstatus[portname] += ASCIIEncoding.ASCII
.GetString(message.ToStructure <MAVLink.mavlink_statustext_t>().text).Trim()
.TrimEnd(new char[] { '\0', '\r', '\n' });
UpdateTextBox();
}
//DisplayText(message);
}
};
try
{
mav.BaseStream.Open();
mav.getHeartBeat();
mav.doCommand(MAVLink.MAV_CMD.FLASH_BOOTLOADER, 0, 0, 0, 0, 290876, 0, 0, false);
mav.getHeartBeat();
if (!bldone)
{
mav.getHeartBeat();
}
if (!bldone)
{
mav.getHeartBeat();
}
if (!bldone)
{
mav.getHeartBeat();
}
if (!bldone)
{
mav.getHeartBeat();
}
if (!bldone)
{
mav.getHeartBeat();
}
if (!bldone)
{
mav.getHeartBeat();
}
if (!bldone)
{
mav.getHeartBeat();
}
if (!bldone)
{
mav.getHeartBeat();
}
mav.Close();
if (bldone)
{
portstatus[portname] += "MLBLUPD OK";
}
UpdateTextBox();
}
catch (Exception ex)
{
portstatus[portname] += "MLEX ";
UpdateTextBox();
}
}
catch (Exception ex)
{
portstatus[portname] += "EX!! ";
UpdateTextBox();
}
UpdateTextBox();
}
