mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
dc42103fa1
add elevons to joystick control. fixup possible autodetect problem. (was occuring when an axis was not centered on detect start) add save mode confirmation
660 lines
21 KiB
C#
660 lines
21 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections;
|
|
using System.Linq;
|
|
using System.Text;
|
|
using Microsoft.DirectX.DirectInput;
|
|
using System.Reflection;
|
|
|
|
namespace ArdupilotMega
|
|
{
|
|
public class Joystick
|
|
{
|
|
Device joystick;
|
|
JoystickState state;
|
|
public bool enabled = false;
|
|
byte[] buttonpressed = new byte[128];
|
|
public string name;
|
|
public bool elevons = false;
|
|
|
|
public static Joystick self;
|
|
|
|
struct JoyChannel
|
|
{
|
|
public int channel;
|
|
public joystickaxis axis;
|
|
public bool reverse;
|
|
public int expo;
|
|
}
|
|
|
|
struct JoyButton
|
|
{
|
|
public int buttonno;
|
|
public string mode;
|
|
}
|
|
|
|
~Joystick()
|
|
{
|
|
try
|
|
{
|
|
joystick.Unacquire();
|
|
}
|
|
catch { }
|
|
}
|
|
|
|
public Joystick()
|
|
{
|
|
self = this;
|
|
}
|
|
|
|
JoyChannel[] JoyChannels = new JoyChannel[5]; // we are base 1
|
|
JoyButton[] JoyButtons = new JoyButton[128]; // base 0
|
|
|
|
public static DeviceList getDevices()
|
|
{
|
|
return Manager.GetDevices(DeviceClass.GameControl, EnumDevicesFlags.AttachedOnly);
|
|
}
|
|
|
|
public bool start(string name)
|
|
{
|
|
self.name = name;
|
|
DeviceList joysticklist = Manager.GetDevices(DeviceClass.GameControl, EnumDevicesFlags.AttachedOnly);
|
|
|
|
bool found = false;
|
|
|
|
foreach (DeviceInstance device in joysticklist)
|
|
{
|
|
if (device.ProductName == name)
|
|
{
|
|
joystick = new Device(device.InstanceGuid);
|
|
found = true;
|
|
break;
|
|
}
|
|
}
|
|
if (!found)
|
|
return false;
|
|
|
|
joystick.SetDataFormat(DeviceDataFormat.Joystick);
|
|
|
|
joystick.Acquire();
|
|
|
|
enabled = true;
|
|
|
|
System.Threading.Thread t11 = new System.Threading.Thread(new System.Threading.ThreadStart(mainloop)) {
|
|
Name = "Joystick loop",
|
|
Priority = System.Threading.ThreadPriority.AboveNormal,
|
|
IsBackground = true
|
|
};
|
|
t11.Start();
|
|
|
|
return true;
|
|
}
|
|
|
|
public static joystickaxis getMovingAxis(string name, int threshold)
|
|
{
|
|
self.name = name;
|
|
DeviceList joysticklist = Manager.GetDevices(DeviceClass.GameControl, EnumDevicesFlags.AttachedOnly);
|
|
|
|
bool found = false;
|
|
|
|
Device joystick = null;
|
|
|
|
foreach (DeviceInstance device in joysticklist)
|
|
{
|
|
if (device.ProductName == name)
|
|
{
|
|
joystick = new Device(device.InstanceGuid);
|
|
found = true;
|
|
break;
|
|
}
|
|
}
|
|
if (!found)
|
|
return joystickaxis.ARx;
|
|
|
|
joystick.SetDataFormat(DeviceDataFormat.Joystick);
|
|
|
|
joystick.Acquire();
|
|
|
|
System.Windows.Forms.MessageBox.Show("Please ensure you have calibrated your joystick in Windows first");
|
|
|
|
joystick.Poll();
|
|
|
|
JoystickState obj = joystick.CurrentJoystickState;
|
|
Hashtable values = new Hashtable();
|
|
|
|
Type type = obj.GetType();
|
|
PropertyInfo[] properties = type.GetProperties();
|
|
foreach (PropertyInfo property in properties)
|
|
{
|
|
values[property.Name] = int.Parse(property.GetValue(obj, null).ToString());
|
|
}
|
|
values["Slider1"] = obj.GetSlider()[0];
|
|
values["Slider2"] = obj.GetSlider()[1];
|
|
|
|
System.Windows.Forms.MessageBox.Show("Please move the joystick axis you want assigned to this function after clicking ok");
|
|
|
|
DateTime start = DateTime.Now;
|
|
|
|
while (start.AddSeconds(10) > DateTime.Now)
|
|
{
|
|
joystick.Poll();
|
|
JoystickState nextstate = joystick.CurrentJoystickState;
|
|
|
|
int[] slider = nextstate.GetSlider();
|
|
|
|
type = nextstate.GetType();
|
|
properties = type.GetProperties();
|
|
foreach (PropertyInfo property in properties)
|
|
{
|
|
//Console.WriteLine("Name: " + property.Name + ", Value: " + property.GetValue(obj, null));
|
|
|
|
Console.WriteLine("test name {0} old {1} new {2} ", property.Name, values[property.Name], int.Parse(property.GetValue(nextstate, null).ToString()));
|
|
Console.WriteLine("{0} {1}", (int)values[property.Name], (int.Parse(property.GetValue(nextstate, null).ToString()) + threshold));
|
|
if ((int)values[property.Name] > (int.Parse(property.GetValue(nextstate, null).ToString()) + threshold) ||
|
|
(int)values[property.Name] < (int.Parse(property.GetValue(nextstate, null).ToString()) - threshold))
|
|
{
|
|
Console.WriteLine("{0}", property.Name);
|
|
joystick.Unacquire();
|
|
return (joystickaxis)Enum.Parse(typeof(joystickaxis), property.Name);
|
|
}
|
|
}
|
|
|
|
// slider1
|
|
if ((int)values["Slider1"] > (slider[0] + threshold) ||
|
|
(int)values["Slider1"] < (slider[0] - threshold))
|
|
{
|
|
joystick.Unacquire();
|
|
return joystickaxis.Slider1;
|
|
}
|
|
|
|
// slider2
|
|
if ((int)values["Slider2"] > (slider[1] + threshold) ||
|
|
(int)values["Slider2"] < (slider[1] - threshold))
|
|
{
|
|
joystick.Unacquire();
|
|
return joystickaxis.Slider2;
|
|
}
|
|
}
|
|
|
|
System.Windows.Forms.MessageBox.Show("No valid option was detected");
|
|
|
|
return joystickaxis.None;
|
|
}
|
|
|
|
public static int getPressedButton(string name)
|
|
{
|
|
self.name = name;
|
|
DeviceList joysticklist = Manager.GetDevices(DeviceClass.GameControl, EnumDevicesFlags.AttachedOnly);
|
|
|
|
bool found = false;
|
|
|
|
Device joystick = null;
|
|
|
|
foreach (DeviceInstance device in joysticklist)
|
|
{
|
|
if (device.ProductName == name)
|
|
{
|
|
joystick = new Device(device.InstanceGuid);
|
|
found = true;
|
|
break;
|
|
}
|
|
}
|
|
if (!found)
|
|
return -1;
|
|
|
|
joystick.SetDataFormat(DeviceDataFormat.Joystick);
|
|
|
|
joystick.Acquire();
|
|
|
|
joystick.Poll();
|
|
|
|
System.Windows.Forms.MessageBox.Show("Please press the joystick button you want assigned to this function after clicking ok");
|
|
|
|
DateTime start = DateTime.Now;
|
|
|
|
while (start.AddSeconds(10) > DateTime.Now)
|
|
{
|
|
joystick.Poll();
|
|
JoystickState nextstate = joystick.CurrentJoystickState;
|
|
|
|
byte[] buttons = nextstate.GetButtons();
|
|
|
|
for (int a = 0; a < joystick.Caps.NumberButtons; a++)
|
|
{
|
|
if (buttons[a] > 0)
|
|
return a;
|
|
}
|
|
}
|
|
|
|
System.Windows.Forms.MessageBox.Show("No valid option was detected");
|
|
|
|
return -1;
|
|
}
|
|
|
|
public void setReverse(int channel, bool reverse)
|
|
{
|
|
JoyChannels[channel].reverse = reverse;
|
|
}
|
|
|
|
public void setAxis(int channel, joystickaxis axis)
|
|
{
|
|
JoyChannels[channel].axis = axis;
|
|
}
|
|
|
|
public void setChannel(int channel, joystickaxis axis, bool reverse, int expo)
|
|
{
|
|
JoyChannel joy = new JoyChannel();
|
|
joy.axis = axis;
|
|
joy.channel = channel;
|
|
joy.expo = expo;
|
|
joy.reverse = reverse;
|
|
|
|
JoyChannels[channel] = joy;
|
|
}
|
|
|
|
public void setButton(int arrayoffset,int buttonid,string mode1)
|
|
{
|
|
JoyButtons[arrayoffset] = new JoyButton()
|
|
{
|
|
buttonno = buttonid,
|
|
mode = mode1
|
|
};
|
|
}
|
|
|
|
public void changeButton(int buttonid, int newid)
|
|
{
|
|
JoyButtons[buttonid].buttonno = newid;
|
|
}
|
|
|
|
int BOOL_TO_SIGN(bool input)
|
|
{
|
|
if (input == true)
|
|
{
|
|
return -1;
|
|
}
|
|
else
|
|
{
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Updates the rcoverride values and controls the mode changes
|
|
/// </summary>
|
|
void mainloop()
|
|
{
|
|
while (enabled)
|
|
{
|
|
try
|
|
{
|
|
System.Threading.Thread.Sleep(50);
|
|
//joystick stuff
|
|
joystick.Poll();
|
|
state = joystick.CurrentJoystickState;
|
|
|
|
int[] slider = state.GetSlider();
|
|
|
|
if (elevons)
|
|
{
|
|
//g.channel_roll.set_pwm(BOOL_TO_SIGN(g.reverse_elevons) * (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) - BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500);
|
|
//g.channel_pitch.set_pwm( (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) + BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500);
|
|
ushort roll = pickchannel(1, JoyChannels[1].axis, false, JoyChannels[1].expo);
|
|
ushort pitch = pickchannel(2, JoyChannels[2].axis, false, JoyChannels[2].expo);
|
|
|
|
MainV2.cs.rcoverridech1 = (ushort)(BOOL_TO_SIGN(JoyChannels[1].reverse) * ((int)(pitch - 1500) - (int)(roll - 1500)) / 2 + 1500);
|
|
MainV2.cs.rcoverridech2 = (ushort)(BOOL_TO_SIGN(JoyChannels[2].reverse) * ((int)(pitch - 1500) + (int)(roll - 1500)) / 2 + 1500);
|
|
}
|
|
else
|
|
{
|
|
MainV2.cs.rcoverridech1 = pickchannel(1, JoyChannels[1].axis, JoyChannels[1].reverse, JoyChannels[1].expo);//(ushort)(((int)state.Rz / 65.535) + 1000);
|
|
MainV2.cs.rcoverridech2 = pickchannel(2, JoyChannels[2].axis, JoyChannels[2].reverse, JoyChannels[2].expo);//(ushort)(((int)state.Y / 65.535) + 1000);
|
|
}
|
|
MainV2.cs.rcoverridech3 = pickchannel(3, JoyChannels[3].axis, JoyChannels[3].reverse, JoyChannels[3].expo);//(ushort)(1000 - ((int)slider[0] / 65.535) + 1000);
|
|
MainV2.cs.rcoverridech4 = pickchannel(4, JoyChannels[4].axis, JoyChannels[4].reverse, JoyChannels[4].expo);//(ushort)(((int)state.X / 65.535) + 1000);
|
|
|
|
foreach (JoyButton but in JoyButtons)
|
|
{
|
|
if (but.buttonno != -1 && getButtonState(but.buttonno))
|
|
{
|
|
MainV2.instance.Invoke((System.Windows.Forms.MethodInvoker)delegate()
|
|
{
|
|
try
|
|
{
|
|
MAVLink.__mavlink_set_nav_mode_t navmode = new MAVLink.__mavlink_set_nav_mode_t();
|
|
|
|
MAVLink.__mavlink_set_mode_t mode = new MAVLink.__mavlink_set_mode_t();
|
|
|
|
if (Common.translateMode(but.mode, ref navmode, ref mode))
|
|
{
|
|
MainV2.comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_SET_NAV_MODE, navmode);
|
|
System.Threading.Thread.Sleep(10);
|
|
MainV2.comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_SET_NAV_MODE, navmode);
|
|
System.Threading.Thread.Sleep(10);
|
|
MainV2.comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_SET_MODE, mode);
|
|
System.Threading.Thread.Sleep(10);
|
|
MainV2.comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_SET_MODE, mode);
|
|
}
|
|
}
|
|
catch { System.Windows.Forms.MessageBox.Show("Failed to change Modes"); }
|
|
});
|
|
}
|
|
}
|
|
|
|
//Console.WriteLine("{0} {1} {2} {3}", MainV2.cs.rcoverridech1, MainV2.cs.rcoverridech2, MainV2.cs.rcoverridech3, MainV2.cs.rcoverridech4);
|
|
}
|
|
catch (Exception ex) { Console.WriteLine("Joystick thread error "+ex.ToString()); } // so we cant fall out
|
|
}
|
|
}
|
|
|
|
public enum joystickaxis
|
|
{
|
|
None,
|
|
ARx,
|
|
ARy,
|
|
ARz,
|
|
AX,
|
|
AY,
|
|
AZ,
|
|
FRx,
|
|
FRy,
|
|
FRz,
|
|
FX,
|
|
FY,
|
|
FZ,
|
|
Rx,
|
|
Ry,
|
|
Rz,
|
|
VRx,
|
|
VRy,
|
|
VRz,
|
|
VX,
|
|
VY,
|
|
VZ,
|
|
X,
|
|
Y,
|
|
Z,
|
|
Slider1,
|
|
Slider2
|
|
}
|
|
|
|
const int RESXu = 1024;
|
|
const int RESXul = 1024;
|
|
const int RESXl = 1024;
|
|
const int RESKul = 100;
|
|
/*
|
|
|
|
ushort expou(ushort x, ushort k)
|
|
{
|
|
// k*x*x*x + (1-k)*x
|
|
return ((ulong)x*x*x/0x10000*k/(RESXul*RESXul/0x10000) + (RESKul-k)*x+RESKul/2)/RESKul;
|
|
}
|
|
// expo-funktion:
|
|
// ---------------
|
|
// kmplot
|
|
// f(x,k)=exp(ln(x)*k/10) ;P[0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20]
|
|
// f(x,k)=x*x*x*k/10 + x*(1-k/10) ;P[0,1,2,3,4,5,6,7,8,9,10]
|
|
// f(x,k)=x*x*k/10 + x*(1-k/10) ;P[0,1,2,3,4,5,6,7,8,9,10]
|
|
// f(x,k)=1+(x-1)*(x-1)*(x-1)*k/10 + (x-1)*(1-k/10) ;P[0,1,2,3,4,5,6,7,8,9,10]
|
|
|
|
short expo(short x, short k)
|
|
{
|
|
if (k == 0) return x;
|
|
short y;
|
|
bool neg = x < 0;
|
|
if (neg) x = -x;
|
|
if (k < 0)
|
|
{
|
|
y = RESXu - expou((ushort)(RESXu - x), (ushort)-k);
|
|
}
|
|
else
|
|
{
|
|
y = expou((ushort)x, (ushort)k);
|
|
}
|
|
return neg ? -y : y;
|
|
}
|
|
|
|
*/
|
|
|
|
public Device AcquireJoystick(string name)
|
|
{
|
|
DeviceList joysticklist = Manager.GetDevices(DeviceClass.GameControl, EnumDevicesFlags.AttachedOnly);
|
|
|
|
bool found = false;
|
|
|
|
foreach (DeviceInstance device in joysticklist)
|
|
{
|
|
if (device.ProductName == name)
|
|
{
|
|
joystick = new Device(device.InstanceGuid);
|
|
found = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (!found)
|
|
return null;
|
|
|
|
joystick.SetDataFormat(DeviceDataFormat.Joystick);
|
|
|
|
joystick.Acquire();
|
|
|
|
return joystick;
|
|
}
|
|
|
|
public void UnAcquireJoyStick()
|
|
{
|
|
joystick.Unacquire();
|
|
}
|
|
|
|
bool getButtonState(int buttonno)
|
|
{
|
|
byte[] buts = state.GetButtons();
|
|
|
|
bool ans = buts[buttonno] > 0 && buttonpressed[buttonno] == 0; // press check + debounce
|
|
|
|
buttonpressed[buttonno] = buts[buttonno]; // set only this button
|
|
|
|
return ans;
|
|
}
|
|
|
|
public int getNumButtons()
|
|
{
|
|
return joystick.Caps.NumberButtons;
|
|
}
|
|
|
|
public bool isButtonPressed(int buttonno)
|
|
{
|
|
byte[] buts = state.GetButtons();
|
|
|
|
if (buts == null)
|
|
return false;
|
|
|
|
return buts[JoyButtons[buttonno].buttonno] > 0;
|
|
}
|
|
|
|
public ushort getValueForChannel(int channel, string name)
|
|
{
|
|
joystick.Poll();
|
|
|
|
state = joystick.CurrentJoystickState;
|
|
|
|
ushort ans = pickchannel(channel, JoyChannels[channel].axis, JoyChannels[channel].reverse, JoyChannels[channel].expo);
|
|
|
|
return ans;
|
|
}
|
|
|
|
ushort pickchannel(int chan, joystickaxis axis, bool rev, int expo)
|
|
{
|
|
int min, max, trim = 0;
|
|
|
|
if (MainV2.comPort.param.Count > 0)
|
|
{
|
|
min = (int)(float)(MainV2.comPort.param["RC" + chan + "_MIN"]);
|
|
max = (int)(float)(MainV2.comPort.param["RC" + chan + "_MAX"]);
|
|
trim = (int)(float)(MainV2.comPort.param["RC" + chan + "_TRIM"]);
|
|
}
|
|
else
|
|
{
|
|
min = 1000;
|
|
max = 2000;
|
|
trim = 1500;
|
|
}
|
|
if (chan == 3)
|
|
{
|
|
// trim = min; // throttle
|
|
}
|
|
|
|
int range = Math.Abs(max - min);
|
|
|
|
int working = 0;
|
|
|
|
switch (axis)
|
|
{
|
|
case joystickaxis.ARx:
|
|
working = state.ARx;
|
|
break;
|
|
|
|
case joystickaxis.ARy:
|
|
working = state.ARy;
|
|
break;
|
|
|
|
case joystickaxis.ARz:
|
|
working = state.ARz;
|
|
break;
|
|
|
|
case joystickaxis.AX:
|
|
working = state.AX;
|
|
break;
|
|
|
|
case joystickaxis.AY:
|
|
working = state.AY;
|
|
break;
|
|
|
|
case joystickaxis.AZ:
|
|
working = state.AZ;
|
|
break;
|
|
|
|
case joystickaxis.FRx:
|
|
working = state.FRx;
|
|
break;
|
|
|
|
case joystickaxis.FRy:
|
|
working = state.FRy;
|
|
break;
|
|
|
|
case joystickaxis.FRz:
|
|
working = state.FRz;
|
|
break;
|
|
|
|
case joystickaxis.FX:
|
|
working = state.FX;
|
|
break;
|
|
|
|
case joystickaxis.FY:
|
|
working = state.FY;
|
|
break;
|
|
|
|
case joystickaxis.FZ:
|
|
working = state.FZ;
|
|
break;
|
|
|
|
case joystickaxis.Rx:
|
|
working = state.Rx;
|
|
break;
|
|
|
|
case joystickaxis.Ry:
|
|
working = state.Ry;
|
|
break;
|
|
|
|
case joystickaxis.Rz:
|
|
working = state.Rz;
|
|
break;
|
|
|
|
case joystickaxis.VRx:
|
|
working = state.VRx;
|
|
break;
|
|
|
|
case joystickaxis.VRy:
|
|
working = state.VRy;
|
|
break;
|
|
|
|
case joystickaxis.VRz:
|
|
working = state.VRz;
|
|
break;
|
|
|
|
case joystickaxis.VX:
|
|
working = state.VX;
|
|
break;
|
|
|
|
case joystickaxis.VY:
|
|
working = state.VY;
|
|
break;
|
|
|
|
case joystickaxis.VZ:
|
|
working = state.VZ;
|
|
break;
|
|
|
|
case joystickaxis.X:
|
|
working = state.X;
|
|
break;
|
|
|
|
case joystickaxis.Y:
|
|
working = state.Y;
|
|
break;
|
|
|
|
case joystickaxis.Z:
|
|
working = state.Z;
|
|
break;
|
|
|
|
case joystickaxis.Slider1:
|
|
int[] slider = state.GetSlider();
|
|
working = slider[0];
|
|
break;
|
|
|
|
case joystickaxis.Slider2:
|
|
int[] slider1 = state.GetSlider();
|
|
working = slider1[1];
|
|
break;
|
|
}
|
|
// between 0 and 65535 - convert to int -500 to 500
|
|
working = (int)(working / 65.535) - 500;
|
|
|
|
if (rev)
|
|
working *= -1;
|
|
|
|
// calc scale from actualy pwm range
|
|
float scale = range / 1000.0f;
|
|
|
|
// save for later
|
|
int raw = working;
|
|
|
|
|
|
double B = 4 * (expo / 100.0);
|
|
double A = 1 - 0.25*B;
|
|
|
|
double t_in = working / 1000.0;
|
|
double t_out = 0;
|
|
double mid = trim / 1000.0;
|
|
|
|
t_out = A * (t_in) + B * Math.Pow((t_in), 3);
|
|
|
|
t_out = mid + t_out * scale;
|
|
|
|
// Console.WriteLine("tin {0} tout {1}",t_in,t_out);
|
|
|
|
working = (int)(t_out * 1000);
|
|
|
|
if (expo == 0)
|
|
{
|
|
working = (int)(raw) + trim;
|
|
}
|
|
|
|
//add limits to movement
|
|
working = Math.Max(min, working);
|
|
working = Math.Min(max, working);
|
|
|
|
return (ushort)working;
|
|
}
|
|
}
|
|
}
|