/// <summary>
/// Button press check with debounce
/// </summary>
/// <param name="buttonno"></param>
/// <returns></returns>
bool getButtonState(JoyButton but, int buttonno)
{
var buts = state.GetButtons();
// button down
bool ans = buts[buttonno] && !buttonpressed[buttonno]; // press check + debounce
if (ans)
{
ButtonDown(but);
}
// button up
ans = !buts[buttonno] && buttonpressed[buttonno];
if (ans)
{
ButtonUp(but);
}
buttonpressed[buttonno] = buts[buttonno]; // set only this button
return(ans);
}
void ProcessButtonEvent(JoyButton but, bool buttondown)
{
if (but.buttonno != -1)
{
// only do_set_relay uses the button up option
if (buttondown == false)
{
if (but.function != buttonfunction.Do_Set_Relay)
{
return;
}
}
switch (but.function)
{
case buttonfunction.ChangeMode:
string mode = but.mode;
if (mode != null)
{
MainV2.instance.BeginInvoke((System.Windows.Forms.MethodInvoker) delegate()
{
try
{
MainV2.comPort.setMode(mode);
}
catch { CustomMessageBox.Show("Failed to change Modes"); }
});
}
break;
case buttonfunction.Mount_Mode:
if (but.p1 != null)
{
MainV2.instance.BeginInvoke((System.Windows.Forms.MethodInvoker) delegate()
{
try
{
MainV2.comPort.setParam("MNT_MODE", but.p1);
}
catch { CustomMessageBox.Show("Failed to change mount mode"); }
});
}
break;
case buttonfunction.Arm:
MainV2.instance.BeginInvoke((System.Windows.Forms.MethodInvoker) delegate()
{
try
{
MainV2.comPort.doARM(true);
}
catch { CustomMessageBox.Show("Failed to Arm"); }
});
break;
case buttonfunction.TakeOff:
MainV2.instance.BeginInvoke((System.Windows.Forms.MethodInvoker) delegate()
{
try
{
MainV2.comPort.setMode("Guided");
if (MainV2.comPort.MAV.cs.firmware == MainV2.Firmwares.ArduCopter2)
{
MainV2.comPort.doCommand(MAVLink.MAV_CMD.TAKEOFF, 0, 0, 0, 0, 0, 0, 2);
}
else
{
MainV2.comPort.doCommand(MAVLink.MAV_CMD.TAKEOFF, 0, 0, 0, 0, 0, 0, 20);
}
}
catch { CustomMessageBox.Show("Failed to takeoff"); }
});
break;
case buttonfunction.Disarm:
MainV2.instance.BeginInvoke((System.Windows.Forms.MethodInvoker) delegate()
{
try
{
MainV2.comPort.doARM(false);
}
catch { CustomMessageBox.Show("Failed to Disarm"); }
});
break;
case buttonfunction.Do_Set_Relay:
MainV2.instance.BeginInvoke((System.Windows.Forms.MethodInvoker) delegate()
{
try
{
int number = (int)but.p1;
int state = buttondown == true ? 1 : 0;
MainV2.comPort.doCommand(MAVLink.MAV_CMD.DO_SET_RELAY, number, state, 0, 0, 0, 0, 0);
}
catch { CustomMessageBox.Show("Failed to DO_SET_RELAY"); }
});
break;
case buttonfunction.Digicam_Control:
MainV2.comPort.setDigicamControl(true);
break;
case buttonfunction.Do_Repeat_Relay:
MainV2.instance.BeginInvoke((System.Windows.Forms.MethodInvoker) delegate()
{
try
{
int relaynumber = (int)but.p1;
int repeat = (int)but.p2;
int time = (int)but.p3;
MainV2.comPort.doCommand(MAVLink.MAV_CMD.DO_REPEAT_RELAY, relaynumber, repeat, time, 0, 0, 0, 0);
}
catch { CustomMessageBox.Show("Failed to DO_REPEAT_RELAY"); }
});
break;
case buttonfunction.Do_Set_Servo:
MainV2.instance.BeginInvoke((System.Windows.Forms.MethodInvoker) delegate()
{
try
{
int channel = (int)but.p1;
int pwm = (int)but.p2;
MainV2.comPort.doCommand(MAVLink.MAV_CMD.DO_SET_SERVO, channel, pwm, 0, 0, 0, 0, 0);
}
catch { CustomMessageBox.Show("Failed to DO_SET_SERVO"); }
});
break;
case buttonfunction.Do_Repeat_Servo:
MainV2.instance.BeginInvoke((System.Windows.Forms.MethodInvoker) delegate()
{
try
{
int channelno = (int)but.p1;
int pwmvalue = (int)but.p2;
int repeattime = (int)but.p3;
int delay_ms = (int)but.p4;
MainV2.comPort.doCommand(MAVLink.MAV_CMD.DO_REPEAT_SERVO, channelno, pwmvalue, repeattime, delay_ms, 0, 0, 0);
}
catch { CustomMessageBox.Show("Failed to DO_REPEAT_SERVO"); }
});
break;
case buttonfunction.Toggle_Pan_Stab:
MainV2.instance.BeginInvoke((System.Windows.Forms.MethodInvoker) delegate()
{
try
{
float current = (float)MainV2.comPort.MAV.param["MNT_STAB_PAN"];
float newvalue = (current > 0) ? 0 : 1;
MainV2.comPort.setParam("MNT_STAB_PAN", newvalue);
}
catch { CustomMessageBox.Show("Failed to Toggle_Pan_Stab"); }
});
break;
case buttonfunction.Gimbal_pnt_track:
MainV2.instance.BeginInvoke((System.Windows.Forms.MethodInvoker) delegate()
{
try
{
MainV2.comPort.doCommand(MAVLink.MAV_CMD.DO_SET_ROI, 0, 0, 0, 0, MainV2.comPort.MAV.cs.gimballat, MainV2.comPort.MAV.cs.gimballng, (float)MainV2.comPort.MAV.cs.GimbalPoint.Alt);
}
catch { CustomMessageBox.Show("Failed to Gimbal_pnt_track"); }
});
break;
}
}
}
public void setButton(int arrayoffset, JoyButton buttonconfig)
{
JoyButtons[arrayoffset] = buttonconfig;
}
void ButtonUp(JoyButton but)
{
ProcessButtonEvent(but, false);
}
void ButtonDown(JoyButton but)
{
ProcessButtonEvent(but, true);
}
void ProcessButtonEvent(JoyButton but, bool buttondown)
{
if (but.buttonno != -1)
{
// only do_set_relay uses the button up option
if (buttondown == false)
{
if (but.function != buttonfunction.Do_Set_Relay)
{
return;
}
}
switch (but.function)
{
case buttonfunction.ChangeMode:
string mode = but.mode;
if (mode != null)
{
MainV2.instance.BeginInvoke((System.Windows.Forms.MethodInvoker) delegate()
{
try
{
MainV2.comPort.setMode(mode);
}
catch { CustomMessageBox.Show("Failed to change Modes"); }
});
}
break;
case buttonfunction.Arm:
MainV2.instance.BeginInvoke((System.Windows.Forms.MethodInvoker) delegate()
{
try
{
MainV2.comPort.doARM(true);
}
catch { CustomMessageBox.Show("Failed to Arm"); }
});
break;
case buttonfunction.Disarm:
MainV2.instance.BeginInvoke((System.Windows.Forms.MethodInvoker) delegate()
{
try
{
MainV2.comPort.doARM(false);
}
catch { CustomMessageBox.Show("Failed to Disarm"); }
});
break;
case buttonfunction.Do_Set_Relay:
MainV2.instance.BeginInvoke((System.Windows.Forms.MethodInvoker) delegate()
{
try
{
int number = (int)but.p1;
int state = buttondown == true ? 1 : 0;
MainV2.comPort.doCommand(MAVLink.MAV_CMD.DO_SET_RELAY, number, state, 0, 0, 0, 0, 0);
}
catch { CustomMessageBox.Show("Failed to DO_SET_RELAY"); }
});
break;
case buttonfunction.Digicam_Control:
MainV2.comPort.setDigicamControl(true);
break;
case buttonfunction.Do_Repeat_Relay:
MainV2.instance.BeginInvoke((System.Windows.Forms.MethodInvoker) delegate()
{
try
{
int relaynumber = (int)but.p1;
int repeat = (int)but.p2;
int time = (int)but.p3;
MainV2.comPort.doCommand(MAVLink.MAV_CMD.DO_REPEAT_RELAY, relaynumber, repeat, time, 0, 0, 0, 0);
}
catch { CustomMessageBox.Show("Failed to DO_REPEAT_RELAY"); }
});
break;
case buttonfunction.Do_Set_Servo:
MainV2.instance.BeginInvoke((System.Windows.Forms.MethodInvoker) delegate()
{
try
{
int channel = (int)but.p1;
int pwm = (int)but.p2;
MainV2.comPort.doCommand(MAVLink.MAV_CMD.DO_SET_SERVO, channel, pwm, 0, 0, 0, 0, 0);
}
catch { CustomMessageBox.Show("Failed to DO_SET_SERVO"); }
});
break;
case buttonfunction.Do_Repeat_Servo:
MainV2.instance.BeginInvoke((System.Windows.Forms.MethodInvoker) delegate()
{
try
{
int channelno = (int)but.p1;
int pwmvalue = (int)but.p2;
int repeattime = (int)but.p3;
int delay_ms = (int)but.p4;
MainV2.comPort.doCommand(MAVLink.MAV_CMD.DO_REPEAT_SERVO, channelno, pwmvalue, repeattime, delay_ms, 0, 0, 0);
}
catch { CustomMessageBox.Show("Failed to DO_REPEAT_SERVO"); }
});
break;
}
}
}
void ProcessButtonEvent(JoyButton but, bool buttondown)
{
if (but.buttonno != -1)
{
// only do_set_relay and Button_axis0-1 uses the button up option
if (buttondown == false)
{
if (but.function != buttonfunction.Do_Set_Relay &&
but.function != buttonfunction.Button_axis0 &&
but.function != buttonfunction.Button_axis1)
{
return;
}
}
switch (but.function)
{
case buttonfunction.ChangeMode:
string mode = but.mode;
if (mode != null)
{
_context.Send(delegate
{
try
{
Interface.setMode((byte)Interface.sysidcurrent, (byte)Interface.compidcurrent, mode);
}
catch
{
CustomMessageBox.Show("Failed to change Modes");
}
}, null);
}
break;
case buttonfunction.Mount_Mode:
_context.Send(delegate
{
try
{
Interface.setParam((byte)Interface.sysidcurrent, (byte)Interface.compidcurrent, "MNT_MODE", but.p1);
}
catch
{
CustomMessageBox.Show("Failed to change mount mode");
}
}, null);
break;
case buttonfunction.Arm:
_context.Send(delegate
{
try
{
Interface.doARM((byte)Interface.sysidcurrent, (byte)Interface.compidcurrent, true);
}
catch
{
CustomMessageBox.Show("Failed to Arm");
}
}, null);
break;
case buttonfunction.TakeOff:
_context.Send(delegate
{
try
{
Interface.setMode("Guided");
if (Interface.MAV.cs.firmware == Firmwares.ArduCopter2)
{
Interface.doCommand((byte)Interface.sysidcurrent, (byte)Interface.compidcurrent, MAVLink.MAV_CMD.TAKEOFF, 0, 0, 0, 0, 0, 0, 2);
}
else
{
Interface.doCommand((byte)Interface.sysidcurrent, (byte)Interface.compidcurrent, MAVLink.MAV_CMD.TAKEOFF, 0, 0, 0, 0, 0, 0, 20);
}
}
catch
{
CustomMessageBox.Show("Failed to takeoff");
}
}, null);
break;
case buttonfunction.Disarm:
_context.Send(delegate
{
try
{
Interface.doARM((byte)Interface.sysidcurrent, (byte)Interface.compidcurrent, false);
}
catch
{
CustomMessageBox.Show("Failed to Disarm");
}
}, null);
break;
case buttonfunction.Do_Set_Relay:
_context.Send(delegate
{
try
{
int number = (int)but.p1;
int state = buttondown == true ? 1 : 0;
Interface.doCommand((byte)Interface.sysidcurrent, (byte)Interface.compidcurrent, MAVLink.MAV_CMD.DO_SET_RELAY, number, state, 0, 0, 0, 0, 0);
}
catch
{
CustomMessageBox.Show("Failed to DO_SET_RELAY");
}
}, null);
break;
case buttonfunction.Digicam_Control:
Interface.setDigicamControl(true);
break;
case buttonfunction.Do_Repeat_Relay:
_context.Send(delegate
{
try
{
int relaynumber = (int)but.p1;
int repeat = (int)but.p2;
int time = (int)but.p3;
Interface.doCommand((byte)Interface.sysidcurrent, (byte)Interface.compidcurrent, MAVLink.MAV_CMD.DO_REPEAT_RELAY, relaynumber, repeat, time, 0,
0, 0, 0);
}
catch
{
CustomMessageBox.Show("Failed to DO_REPEAT_RELAY");
}
}, null);
break;
case buttonfunction.Do_Set_Servo:
_context.Send(delegate
{
try
{
int channel = (int)but.p1;
int pwm = (int)but.p2;
Interface.doCommand((byte)Interface.sysidcurrent, (byte)Interface.compidcurrent, MAVLink.MAV_CMD.DO_SET_SERVO, channel, pwm, 0, 0, 0, 0, 0);
}
catch
{
CustomMessageBox.Show("Failed to DO_SET_SERVO");
}
}, null);
break;
case buttonfunction.Do_Repeat_Servo:
_context.Send(delegate
{
try
{
int channelno = (int)but.p1;
int pwmvalue = (int)but.p2;
int repeattime = (int)but.p3;
int delay_ms = (int)but.p4;
Interface.doCommand((byte)Interface.sysidcurrent, (byte)Interface.compidcurrent, MAVLink.MAV_CMD.DO_REPEAT_SERVO, channelno, pwmvalue,
repeattime, delay_ms, 0, 0, 0);
}
catch
{
CustomMessageBox.Show("Failed to DO_REPEAT_SERVO");
}
}, null);
break;
case buttonfunction.Toggle_Pan_Stab:
_context.Send(delegate
{
try
{
float current = (float)Interface.MAV.param["MNT_STAB_PAN"];
float newvalue = (current > 0) ? 0 : 1;
Interface.setParam((byte)Interface.sysidcurrent, (byte)Interface.compidcurrent, "MNT_STAB_PAN", newvalue);
}
catch
{
CustomMessageBox.Show("Failed to Toggle_Pan_Stab");
}
}, null);
break;
case buttonfunction.Gimbal_pnt_track:
_context.Send(delegate
{
try
{
Interface.doCommandInt((byte)Interface.sysidcurrent, (byte)Interface.compidcurrent, MAVLink.MAV_CMD.DO_SET_ROI, 0, 0, 0, 0,
(int)(Interface.MAV.cs.gimballat * 1e7), (int)(Interface.MAV.cs.gimballng * 1e7),
(float)Interface.MAV.cs.GimbalPoint.Alt);
}
catch
{
CustomMessageBox.Show("Failed to Gimbal_pnt_track");
}
}, null);
break;
case buttonfunction.Mount_Control_0:
_context.Send(delegate
{
try
{
Interface.setMountControl((byte)Interface.sysidcurrent, (byte)Interface.compidcurrent, 0, 0, 0, false);
}
catch
{
CustomMessageBox.Show("Failed to Mount_Control_0");
}
}, null);
break;
case buttonfunction.Button_axis0:
_context.Send(delegate
{
try
{
int pwmmin = (int)but.p1;
int pwmmax = (int)but.p2;
if (buttondown)
{
custom0 = pwmmax;
}
else
{
custom0 = pwmmin;
}
}
catch
{
CustomMessageBox.Show("Failed to Button_axis0");
}
}, null);
break;
case buttonfunction.Button_axis1:
_context.Send(delegate
{
try
{
int pwmmin = (int)but.p1;
int pwmmax = (int)but.p2;
if (buttondown)
{
custom1 = pwmmax;
}
else
{
custom1 = pwmmin;
}
}
catch
{
CustomMessageBox.Show("Failed to Button_axis1");
}
}, null);
break;
}
}
}
