public short getRawValueForChannel(int channel) { if (joystick == null) { return(0); } joystick.Poll(); state = joystick.CurrentJoystickState(); short ans = pickchannel(channel, JoyChannels[channel].axis, false, 0); log.DebugFormat("{0} = {1} = {2}", channel, ans, state.X); return(ans); }
/// <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(); //Console.WriteLine(state); if (getNumberPOV() > 0) { int pov = getHatSwitchDirection(); if (pov != -1) { int angle = pov/100; //0 = down = 18000 //0 = up = 0 // 0 if (angle > 270 || angle < 90) hat1 += 500; // 180 if (angle > 90 && angle < 270) hat1 -= 500; // 90 if (angle > 0 && angle < 180) hat2 += 500; // 270 if (angle > 180 && angle < 360) hat2 -= 500; } } 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); if (getJoystickAxis(1) != Joystick.joystickaxis.None) MainV2.comPort.MAV.cs.rcoverridech1 = (ushort) (BOOL_TO_SIGN(JoyChannels[1].reverse)*((int) (pitch - 1500) - (int) (roll - 1500))/2 + 1500); if (getJoystickAxis(2) != Joystick.joystickaxis.None) MainV2.comPort.MAV.cs.rcoverridech2 = (ushort) (BOOL_TO_SIGN(JoyChannels[2].reverse)*((int) (pitch - 1500) + (int) (roll - 1500))/2 + 1500); } else { if (getJoystickAxis(1) != Joystick.joystickaxis.None) MainV2.comPort.MAV.cs.rcoverridech1 = pickchannel(1, JoyChannels[1].axis, JoyChannels[1].reverse, JoyChannels[1].expo); //(ushort)(((int)state.Rz / 65.535) + 1000); if (getJoystickAxis(2) != Joystick.joystickaxis.None) MainV2.comPort.MAV.cs.rcoverridech2 = pickchannel(2, JoyChannels[2].axis, JoyChannels[2].reverse, JoyChannels[2].expo); //(ushort)(((int)state.Y / 65.535) + 1000); } if (getJoystickAxis(3) != Joystick.joystickaxis.None) MainV2.comPort.MAV.cs.rcoverridech3 = pickchannel(3, JoyChannels[3].axis, JoyChannels[3].reverse, JoyChannels[3].expo); //(ushort)(1000 - ((int)slider[0] / 65.535) + 1000); if (getJoystickAxis(4) != Joystick.joystickaxis.None) MainV2.comPort.MAV.cs.rcoverridech4 = pickchannel(4, JoyChannels[4].axis, JoyChannels[4].reverse, JoyChannels[4].expo); //(ushort)(((int)state.X / 65.535) + 1000); if (getJoystickAxis(5) != Joystick.joystickaxis.None) MainV2.comPort.MAV.cs.rcoverridech5 = pickchannel(5, JoyChannels[5].axis, JoyChannels[5].reverse, JoyChannels[5].expo); if (getJoystickAxis(6) != Joystick.joystickaxis.None) MainV2.comPort.MAV.cs.rcoverridech6 = pickchannel(6, JoyChannels[6].axis, JoyChannels[6].reverse, JoyChannels[6].expo); if (getJoystickAxis(7) != Joystick.joystickaxis.None) MainV2.comPort.MAV.cs.rcoverridech7 = pickchannel(7, JoyChannels[7].axis, JoyChannels[7].reverse, JoyChannels[7].expo); if (getJoystickAxis(8) != Joystick.joystickaxis.None) MainV2.comPort.MAV.cs.rcoverridech8 = pickchannel(8, JoyChannels[8].axis, JoyChannels[8].reverse, JoyChannels[8].expo); // disable button actions when not connected. if (MainV2.comPort.BaseStream.IsOpen) DoJoystickButtonFunction(); //Console.WriteLine("{0} {1} {2} {3}", MainV2.comPort.MAV.cs.rcoverridech1, MainV2.comPort.MAV.cs.rcoverridech2, MainV2.comPort.MAV.cs.rcoverridech3, MainV2.comPort.MAV.cs.rcoverridech4); } catch (SharpDX.SharpDXException ex) { log.Error(ex); clearRCOverride(); MainV2.instance.Invoke((System.Action) delegate { CustomMessageBox.Show("Lost Joystick", "Lost Joystick"); }); return; } catch (Exception ex) { log.Info("Joystick thread error " + ex.ToString()); } // so we cant fall out } }
public ushort getRawValueForChannel(int channel) { if (joystick == null) return 0; joystick.Poll(); state = joystick.CurrentJoystickState(); ushort ans = pickchannel(channel, JoyChannels[channel].axis, false, 0); log.DebugFormat("{0} = {1} = {2}", channel, ans, state.X); return ans; }
public ushort getValueForChannel(int channel, string name) { if (joystick == null) return 0; joystick.Poll(); state = joystick.CurrentJoystickState(); ushort ans = pickchannel(channel, JoyChannels[channel].axis, JoyChannels[channel].reverse, JoyChannels[channel].expo); log.DebugFormat("{0} = {1} = {2}", channel, ans, state.X); return ans; }
/// <summary> /// Updates the rcoverride values and controls the mode changes /// </summary> void mainloop() { while (enabled && joystick != null && !joystick.IsDisposed) { try { System.Threading.Thread.Sleep(50); if (joystick.IsDisposed) { return; } //joystick stuff joystick.Poll(); state = joystick.CurrentJoystickState(); //Console.WriteLine(state); if (getNumberPOV() > 0) { int pov = getHatSwitchDirection(); if (pov != -1) { int angle = pov / 100; //0 = down = 18000 //0 = up = 0 // 0 if (angle > 270 || angle < 90) { hat1 += 500; } // 180 if (angle > 90 && angle < 270) { hat1 -= 500; } // 90 if (angle > 0 && angle < 180) { hat2 += 500; } // 270 if (angle > 180 && angle < 360) { hat2 -= 500; } } } 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); short roll = pickchannel(1, JoyChannels[1].axis, false, JoyChannels[1].expo); short pitch = pickchannel(2, JoyChannels[2].axis, false, JoyChannels[2].expo); if (getJoystickAxis(1) != Joystick.joystickaxis.None) { MainV2.comPort.MAV.cs.rcoverridech1 = (short) (BOOL_TO_SIGN(JoyChannels[1].reverse) * ((int)(pitch - 1500) - (int)(roll - 1500)) / 2 + 1500); } if (getJoystickAxis(2) != Joystick.joystickaxis.None) { MainV2.comPort.MAV.cs.rcoverridech2 = (short) (BOOL_TO_SIGN(JoyChannels[2].reverse) * ((int)(pitch - 1500) + (int)(roll - 1500)) / 2 + 1500); } } else { if (getJoystickAxis(1) != Joystick.joystickaxis.None) { MainV2.comPort.MAV.cs.rcoverridech1 = pickchannel(1, JoyChannels[1].axis, JoyChannels[1].reverse, JoyChannels[1].expo); } //(ushort)(((int)state.Rz / 65.535) + 1000); if (getJoystickAxis(2) != Joystick.joystickaxis.None) { MainV2.comPort.MAV.cs.rcoverridech2 = pickchannel(2, JoyChannels[2].axis, JoyChannels[2].reverse, JoyChannels[2].expo); } //(ushort)(((int)state.Y / 65.535) + 1000); } if (getJoystickAxis(3) != Joystick.joystickaxis.None) { MainV2.comPort.MAV.cs.rcoverridech3 = pickchannel(3, JoyChannels[3].axis, JoyChannels[3].reverse, JoyChannels[3].expo); //(ushort)(1000 - ((int)slider[0] / 65.535) + 1000); } if (getJoystickAxis(4) != Joystick.joystickaxis.None) { MainV2.comPort.MAV.cs.rcoverridech4 = pickchannel(4, JoyChannels[4].axis, JoyChannels[4].reverse, JoyChannels[4].expo); //(ushort)(((int)state.X / 65.535) + 1000); } if (getJoystickAxis(5) != Joystick.joystickaxis.None) { MainV2.comPort.MAV.cs.rcoverridech5 = pickchannel(5, JoyChannels[5].axis, JoyChannels[5].reverse, JoyChannels[5].expo); } if (getJoystickAxis(6) != Joystick.joystickaxis.None) { MainV2.comPort.MAV.cs.rcoverridech6 = pickchannel(6, JoyChannels[6].axis, JoyChannels[6].reverse, JoyChannels[6].expo); } if (getJoystickAxis(7) != Joystick.joystickaxis.None) { MainV2.comPort.MAV.cs.rcoverridech7 = pickchannel(7, JoyChannels[7].axis, JoyChannels[7].reverse, JoyChannels[7].expo); } if (getJoystickAxis(8) != Joystick.joystickaxis.None) { MainV2.comPort.MAV.cs.rcoverridech8 = pickchannel(8, JoyChannels[8].axis, JoyChannels[8].reverse, JoyChannels[8].expo); } // disable button actions when not connected. if (MainV2.comPort.BaseStream.IsOpen) { DoJoystickButtonFunction(); } //Console.WriteLine("{0} {1} {2} {3}", MainV2.comPort.MAV.cs.rcoverridech1, MainV2.comPort.MAV.cs.rcoverridech2, MainV2.comPort.MAV.cs.rcoverridech3, MainV2.comPort.MAV.cs.rcoverridech4); } catch (SharpDX.SharpDXException ex) { log.Error(ex); clearRCOverride(); MainV2.instance.Invoke((System.Action) delegate { CustomMessageBox.Show("Lost Joystick", "Lost Joystick"); }); return; } catch (Exception ex) { log.Info("Joystick thread error " + ex.ToString()); } // so we cant fall out } }