/// <summary>
/// thread used to send joystick packets to the MAV
/// </summary>
private void joysticksend()
{
float rate = 50;
int count = 0;
DateTime lastratechange = DateTime.Now;
while (true)
{
try
{
if (MONO)
{
log.Error("Mono: closing joystick thread");
break;
}
if (!MONO)
{
//joystick stuff
if (joystick != null && joystick.enabled)
{
MAVLink.mavlink_rc_channels_override_t rc = new MAVLink.mavlink_rc_channels_override_t();
rc.target_component = comPort.MAV.compid;
rc.target_system = comPort.MAV.sysid;
if (joystick.getJoystickAxis(1) != Joystick.joystickaxis.None)
rc.chan1_raw = MainV2.comPort.MAV.cs.rcoverridech1;//(ushort)(((int)state.Rz / 65.535) + 1000);
if (joystick.getJoystickAxis(2) != Joystick.joystickaxis.None)
rc.chan2_raw = MainV2.comPort.MAV.cs.rcoverridech2;//(ushort)(((int)state.Y / 65.535) + 1000);
if (joystick.getJoystickAxis(3) != Joystick.joystickaxis.None)
rc.chan3_raw = MainV2.comPort.MAV.cs.rcoverridech3;//(ushort)(1000 - ((int)slider[0] / 65.535 ) + 1000);
if (joystick.getJoystickAxis(4) != Joystick.joystickaxis.None)
rc.chan4_raw = MainV2.comPort.MAV.cs.rcoverridech4;//(ushort)(((int)state.X / 65.535) + 1000);
if (joystick.getJoystickAxis(5) != Joystick.joystickaxis.None)
rc.chan5_raw = MainV2.comPort.MAV.cs.rcoverridech5;
if (joystick.getJoystickAxis(6) != Joystick.joystickaxis.None)
rc.chan6_raw = MainV2.comPort.MAV.cs.rcoverridech6;
if (joystick.getJoystickAxis(7) != Joystick.joystickaxis.None)
rc.chan7_raw = MainV2.comPort.MAV.cs.rcoverridech7;
if (joystick.getJoystickAxis(8) != Joystick.joystickaxis.None)
rc.chan8_raw = MainV2.comPort.MAV.cs.rcoverridech8;
if (lastjoystick.AddMilliseconds(rate) < DateTime.Now)
{
/*
if (MainV2.comPort.MAV.cs.rssi > 0 && MainV2.comPort.MAV.cs.remrssi > 0)
{
if (lastratechange.Second != DateTime.Now.Second)
{
if (MainV2.comPort.MAV.cs.txbuffer > 90)
{
if (rate < 20)
rate = 21;
rate--;
if (MainV2.comPort.MAV.cs.linkqualitygcs < 70)
rate = 50;
}
else
{
if (rate > 100)
rate = 100;
rate++;
}
lastratechange = DateTime.Now;
}
}
*/
// Console.WriteLine(DateTime.Now.Millisecond + " {0} {1} {2} {3} {4}", rc.chan1_raw, rc.chan2_raw, rc.chan3_raw, rc.chan4_raw,rate);
comPort.sendPacket(rc);
count++;
lastjoystick = DateTime.Now;
}
}
}
Thread.Sleep(20);
}
catch
{
} // cant fall out
}
}
private void BUT_enable_Click(object sender, EventArgs e)
{
if (MainV2.joystick == null || MainV2.joystick.enabled == false)
{
try
{
if (MainV2.joystick != null)
MainV2.joystick.UnAcquireJoyStick();
}
catch { }
Joystick joy = new Joystick();
joy.setChannel(1, (Joystick.joystickaxis)Enum.Parse(typeof(Joystick.joystickaxis), CMB_CH1.Text), revCH1.Checked, int.Parse(expo_ch1.Text));
joy.setChannel(2, (Joystick.joystickaxis)Enum.Parse(typeof(Joystick.joystickaxis), CMB_CH2.Text), revCH2.Checked, int.Parse(expo_ch2.Text));
joy.setChannel(3, (Joystick.joystickaxis)Enum.Parse(typeof(Joystick.joystickaxis), CMB_CH3.Text), revCH3.Checked, int.Parse(expo_ch3.Text));
joy.setChannel(4, (Joystick.joystickaxis)Enum.Parse(typeof(Joystick.joystickaxis), CMB_CH4.Text), revCH4.Checked, int.Parse(expo_ch4.Text));
joy.setChannel(5, (Joystick.joystickaxis)Enum.Parse(typeof(Joystick.joystickaxis), CMB_CH5.Text), revCH5.Checked, int.Parse(expo_ch5.Text));
joy.setChannel(6, (Joystick.joystickaxis)Enum.Parse(typeof(Joystick.joystickaxis), CMB_CH6.Text), revCH6.Checked, int.Parse(expo_ch6.Text));
joy.setChannel(7, (Joystick.joystickaxis)Enum.Parse(typeof(Joystick.joystickaxis), CMB_CH7.Text), revCH7.Checked, int.Parse(expo_ch7.Text));
joy.setChannel(8, (Joystick.joystickaxis)Enum.Parse(typeof(Joystick.joystickaxis), CMB_CH8.Text), revCH8.Checked, int.Parse(expo_ch8.Text));
joy.elevons = CHK_elevons.Checked;
for (int f = 0; f < noButtons; f++)
{
string name = (f + 1).ToString();
try
{
joy.setButton(f, int.Parse(this.Controls.Find("cmbbutton" + name, false)[0].Text), this.Controls.Find("cmbaction" + name, false)[0].Text);
}
catch { CustomMessageBox.Show("Set Button "+ name + " Failed"); }
}
joy.start(CMB_joysticks.Text);
MainV2.joystick = joy;
MainV2.joystick.enabled = true;
BUT_enable.Text = "Disable";
//timer1.Start();
}
else
{
MainV2.joystick.enabled = false;
MainV2.joystick = null;
MAVLink.mavlink_rc_channels_override_t rc = new MAVLink.mavlink_rc_channels_override_t();
rc.target_component = MainV2.comPort.MAV.compid;
rc.target_system = MainV2.comPort.MAV.sysid;
rc.chan1_raw = 0;
rc.chan2_raw = 0;
rc.chan3_raw = 0;
rc.chan4_raw = 0;
rc.chan5_raw = 0;
rc.chan6_raw = 0;
rc.chan7_raw = 0;
rc.chan8_raw = 0;
MainV2.comPort.sendPacket(rc);
System.Threading.Thread.Sleep(20);
MainV2.comPort.sendPacket(rc);
System.Threading.Thread.Sleep(20);
MainV2.comPort.sendPacket(rc);
//timer1.Stop();
BUT_enable.Text = "Enable";
}
}
/// <summary>
/// thread used to send joystick packets to the MAV
/// </summary>
private void joysticksend()
{
float rate = 50;
int count = 0;
DateTime lastratechange = DateTime.Now;
while (true)
{
try
{
if (MONO)
{
log.Error("Mono: closing joystick thread");
break;
}
if (!MONO)
{
//joystick stuff
Byte[] hidOutputReportBuffer = null;
if (MainV2.comPort.MAV.cs.controller_override)
{
FindHid();
if (hid.deviceDetected)
{
double[] reportBuffer = new double[hid.Capabilities.OutputReportByteLength];
reportBuffer[0] = 0;
reportBuffer[1] = (MainV2.comPort.MAV.cs.controlleroverridech3 / (double)(65535 / (double)1023));
reportBuffer[2] = (MainV2.comPort.MAV.cs.controlleroverridech3 / (double)(65535 / (double)1023));
reportBuffer[3] = (MainV2.comPort.MAV.cs.controlleroverridech1 / (double)(65535 / (double)1023)) + MainV2.comPort.MAV.cs.trim_x;
reportBuffer[4] = (MainV2.comPort.MAV.cs.controlleroverridech1 / (double)(65535 / (double)1023)) + MainV2.comPort.MAV.cs.trim_x;
reportBuffer[5] = (MainV2.comPort.MAV.cs.controlleroverridech2 / (double)(65535 / (double)1023)) + MainV2.comPort.MAV.cs.trim_y;
reportBuffer[6] = (MainV2.comPort.MAV.cs.controlleroverridech2 / (double)(65535 / (double)1023)) + MainV2.comPort.MAV.cs.trim_y;
reportBuffer[7] = (MainV2.comPort.MAV.cs.controlleroverridech4 / (double)(65535 / (double)1023));
reportBuffer[8] = (MainV2.comPort.MAV.cs.controlleroverridech4 / (double)(65535 / (double)1023));
hidOutputReportBuffer = new Byte[hid.Capabilities.OutputReportByteLength];
hidOutputReportBuffer[0] = 0;
hidOutputReportBuffer[1] = (byte)(Math.Max(0, Math.Min(1023, reportBuffer[1])) % 256);
hidOutputReportBuffer[2] = (byte)(Math.Max(0, Math.Min(1023, reportBuffer[2])) / 256);
hidOutputReportBuffer[3] = (byte)(Math.Max(0, Math.Min(1023, reportBuffer[3])) % 256);
hidOutputReportBuffer[4] = (byte)(Math.Max(0, Math.Min(1023, reportBuffer[4])) / 256);
hidOutputReportBuffer[5] = (byte)(Math.Max(0, Math.Min(1023, reportBuffer[5])) % 256);
hidOutputReportBuffer[6] = (byte)(Math.Max(0, Math.Min(1023, reportBuffer[6])) / 256);
hidOutputReportBuffer[7] = (byte)(Math.Max(0, Math.Min(1023, reportBuffer[7])) % 256);
hidOutputReportBuffer[8] = (byte)(Math.Max(0, Math.Min(1023, reportBuffer[8])) / 256);
}
}
if (joystick != null && joystick.enabled)
{
MAVLink.mavlink_rc_channels_override_t rc = new MAVLink.mavlink_rc_channels_override_t();
rc.target_component = comPort.MAV.compid;
rc.target_system = comPort.MAV.sysid;
if (joystick.getJoystickAxis(1) != Joystick.joystickaxis.None)
{
if (!MainV2.comPort.MAV.cs.controller_override)
rc.chan1_raw = MainV2.comPort.MAV.cs.rcoverridech1;//(ushort)(((int)state.Rz / 65.535) + 1000);
}
if (joystick.getJoystickAxis(2) != Joystick.joystickaxis.None)
{
if (!MainV2.comPort.MAV.cs.controller_override)
rc.chan2_raw = MainV2.comPort.MAV.cs.rcoverridech2;//(ushort)(((int)state.Y / 65.535) + 1000);
}
if (joystick.getJoystickAxis(3) != Joystick.joystickaxis.None)
{
if (!MainV2.comPort.MAV.cs.controller_override)
rc.chan3_raw = MainV2.comPort.MAV.cs.rcoverridech3;//(ushort)(1000 - ((int)slider[0] / 65.535 ) + 1000);
}
if (joystick.getJoystickAxis(4) != Joystick.joystickaxis.None)
{
if (!MainV2.comPort.MAV.cs.controller_override)
rc.chan4_raw = MainV2.comPort.MAV.cs.rcoverridech4;//(ushort)(((int)state.X / 65.535) + 1000);
}
if (joystick.getJoystickAxis(5) != Joystick.joystickaxis.None)
{
if (!MainV2.comPort.MAV.cs.controller_override)
rc.chan5_raw = MainV2.comPort.MAV.cs.rcoverridech5;
}
if (joystick.getJoystickAxis(6) != Joystick.joystickaxis.None)
{
if (!MainV2.comPort.MAV.cs.controller_override)
rc.chan6_raw = MainV2.comPort.MAV.cs.rcoverridech6;
}
if (joystick.getJoystickAxis(7) != Joystick.joystickaxis.None)
{
if (!MainV2.comPort.MAV.cs.controller_override)
rc.chan7_raw = MainV2.comPort.MAV.cs.rcoverridech7;
}
if (joystick.getJoystickAxis(8) != Joystick.joystickaxis.None)
{
if (!MainV2.comPort.MAV.cs.controller_override)
rc.chan8_raw = MainV2.comPort.MAV.cs.rcoverridech8;
}
if (lastjoystick.AddMilliseconds(rate) < DateTime.Now)
{
/*
if (MainV2.comPort.MAV.cs.rssi > 0 && MainV2.comPort.MAV.cs.remrssi > 0)
{
if (lastratechange.Second != DateTime.Now.Second)
{
if (MainV2.comPort.MAV.cs.txbuffer > 90)
{
if (rate < 20)
rate = 21;
rate--;
if (MainV2.comPort.MAV.cs.linkqualitygcs < 70)
rate = 50;
}
else
{
if (rate > 100)
rate = 100;
rate++;
}
lastratechange = DateTime.Now;
}
}
*/
// Console.WriteLine(DateTime.Now.Millisecond + " {0} {1} {2} {3} {4}", rc.chan1_raw, rc.chan2_raw, rc.chan3_raw, rc.chan4_raw,rate);
if (MainV2.comPort.MAV.cs.controller_override)
{
Hid.OutputReportViaInterruptTransfer outputReport = new Hid.OutputReportViaInterruptTransfer();
outputReport.Write(hidOutputReportBuffer, hid.writeHandle);
}
else
{
comPort.sendPacket(rc);
}
count++;
lastjoystick = DateTime.Now;
}
}
}
Thread.Sleep(5);
}
catch
{
} // cant fall out
}
}