public void clearRCOverride()
{
// disable it, before continuing
this.enabled = false;
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);
System.Threading.Thread.Sleep(20);
MainV2.comPort.sendPacket(rc);
System.Threading.Thread.Sleep(20);
MainV2.comPort.sendPacket(rc);
System.Threading.Thread.Sleep(20);
MainV2.comPort.sendPacket(rc);
MainV2.comPort.sendPacket(rc);
MainV2.comPort.sendPacket(rc);
MainV2.comPort.sendPacket(rc);
}
public void sendRcOverride(MAVLink.mavlink_rc_channels_override_t input)
{
input.target_component = (byte)this.componentId;
input.target_component = (byte)this.systemId;
byte[] packet = this.mavlinkParse.GenerateMAVLinkPacket(MAVLink.MAVLINK_MSG_ID.RC_CHANNELS_OVERRIDE, input);
this.port.Write(packet, 0, packet.Length);
}
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 { }
// all config is loaded from the xmls
Joystick joy = new Joystick();
joy.elevons = CHK_elevons.Checked;
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";
}
}
public JoystickViewModel(IVehicleComponent vehicleComponent)
{
VehicleComponent = vehicleComponent;
Messenger.Default.Register <VehicleSelected>(this, VehicleSelectedAction);
SenderTimer = new DispatcherTimer {
Interval = new TimeSpan(0, 0, 0, 0, 1000 / 12)
};
UiThread = new DispatcherTimer {
Interval = new TimeSpan(0, 0, 0, 0, 1000 / 12)
};
UiThread.Tick += UiThread_Tick;
UiThread.Start();
_joystick = new MAVLink.mavlink_rc_channels_override_t();
// make sure that DirectInput has been initialized
var dinput = new DirectInput();
// search for devices
foreach (DeviceInstance device in dinput.GetDevices(DeviceClass.GameController, DeviceEnumerationFlags.AttachedOnly))
{
// create the device
try
{
joystick = new Joystick(dinput, device.InstanceGuid);
break;
}
catch (DirectInputException exception)
{
Logger.Error("JoystickViewModel", exception);
}
}
if (joystick == null)
{
//MessageBox.Show("There are no joysticks attached to the system.");
return;
}
foreach (DeviceObjectInstance deviceObject in joystick.GetObjects())
{
if ((deviceObject.ObjectType & ObjectDeviceType.Axis) != 0)
{
joystick.GetObjectPropertiesById((int)deviceObject.ObjectType).SetRange(MinJoystickRange, MaxJoystickRange);
}
}
Equalisers = new ObservableCollection <Equaliser>();
for (int i = 0; i < 12; i++)
{
Equalisers.Add(new Equaliser {
Id = i + 1, Channel = 1, Value = 0, IsChecked = false, Receiver = 0
});
}
}
public void clearRCOverride()
{
// disable it, before continuing
this.enabled = false;
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.MAV.cs.rcoverridech1 = 0;
MainV2.comPort.MAV.cs.rcoverridech2 = 0;
MainV2.comPort.MAV.cs.rcoverridech3 = 0;
MainV2.comPort.MAV.cs.rcoverridech4 = 0;
MainV2.comPort.MAV.cs.rcoverridech5 = 0;
MainV2.comPort.MAV.cs.rcoverridech6 = 0;
MainV2.comPort.MAV.cs.rcoverridech7 = 0;
MainV2.comPort.MAV.cs.rcoverridech8 = 0;
try
{
MainV2.comPort.sendPacket(rc, rc.target_system, rc.target_component);
System.Threading.Thread.Sleep(20);
MainV2.comPort.sendPacket(rc, rc.target_system, rc.target_component);
System.Threading.Thread.Sleep(20);
MainV2.comPort.sendPacket(rc, rc.target_system, rc.target_component);
System.Threading.Thread.Sleep(20);
MainV2.comPort.sendPacket(rc, rc.target_system, rc.target_component);
System.Threading.Thread.Sleep(20);
MainV2.comPort.sendPacket(rc, rc.target_system, rc.target_component);
System.Threading.Thread.Sleep(20);
MainV2.comPort.sendPacket(rc, rc.target_system, rc.target_component);
MainV2.comPort.sendPacket(rc, rc.target_system, rc.target_component);
MainV2.comPort.sendPacket(rc, rc.target_system, rc.target_component);
MainV2.comPort.sendPacket(rc, rc.target_system, rc.target_component);
}
catch (Exception ex)
{
log.Error(ex);
}
}
public void EnableRcOverride()
{
// setup initial values for 'virtual radio'
MAVLink.mavlink_rc_channels_override_t neutralRadio = new MAVLink.mavlink_rc_channels_override_t();
neutralRadio.chan1_raw = 1100;
neutralRadio.chan2_raw = 1100;
neutralRadio.chan3_raw = 1100;
neutralRadio.chan4_raw = 1100;
neutralRadio.chan5_raw = 1100;
neutralRadio.chan6_raw = 1100;
neutralRadio.chan7_raw = 1100;
neutralRadio.chan8_raw = 1100;
// set radio input and enable broadcast
this.connection.RcInput = neutralRadio;
this.connection.EnableRcOverride = true;
}
private void joysticksend()
{
float rate = 50;
DateTime lastratechange = DateTime.Now;
while (true)
{
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;
rc.chan1_raw = (ushort)joystick_reverse(800, 2200, joystick.State.X); // 에일러론
rc.chan2_raw = (ushort)joystick_reverse(800, 2200, joystick.State.Y); // 엘리베이터
rc.chan3_raw = (ushort)joystick_reverse(800, 2200, joystick.State.Z); // 스로틀
rc.chan4_raw = (ushort)joystick_reverse(800, 2200, joystick.State.Rz); // 러더 + 랜딩기어
rc.chan5_raw = (ushort)flap; // 플랩
rc.chan6_raw = (ushort)servo1; // 카메라 1
rc.chan7_raw = (ushort)servo2; // 카메라 2
rc.chan8_raw = (ushort)mode; // 모드
// 에일러론 한계치 조정
if (rc.chan1_raw > 1800)
{
rc.chan1_raw = (ushort)1800;
}
else if (rc.chan1_raw < 1200)
{
rc.chan1_raw = 1200;
}
if (lastjoystick.AddMilliseconds(rate) < DateTime.Now)
{
comPort.sendPacket(rc);
lastjoystick = DateTime.Now;
}
Thread.Sleep(20);
}
}
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 { }
// all config is loaded from the xmls
Joystick joy = new Joystick();
joy.elevons = CHK_elevons.Checked;
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";
}
}
public void clearRCOverride()
{
// disable it, before continuing
this.enabled = false;
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;
try
{
MainV2.comPort.sendPacket(rc);
System.Threading.Thread.Sleep(20);
MainV2.comPort.sendPacket(rc);
System.Threading.Thread.Sleep(20);
MainV2.comPort.sendPacket(rc);
System.Threading.Thread.Sleep(20);
MainV2.comPort.sendPacket(rc);
System.Threading.Thread.Sleep(20);
MainV2.comPort.sendPacket(rc);
System.Threading.Thread.Sleep(20);
MainV2.comPort.sendPacket(rc);
MainV2.comPort.sendPacket(rc);
MainV2.comPort.sendPacket(rc);
MainV2.comPort.sendPacket(rc);
}
catch (Exception ex) { log.Error(ex); }
}
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
{
MAVLink.mavlink_rc_channels_override_t rc = new MAVLink.mavlink_rc_channels_override_t();
rc.target_component = MainV2.comPort.compid;
rc.target_system = MainV2.comPort.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();
MainV2.joystick.enabled = false;
MainV2.joystick = null;
BUT_enable.Text = "Enable";
}
}
public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow, MAVLinkInterface mavinterface, MAVState MAV)
{
lock (this)
{
int override_counter = 1;
if (ch7in > 1700 && override_counter==1)
{
override_counter += 1;
MAVLink.mavlink_rc_channels_override_t rc_over = new MAVLink.mavlink_rc_channels_override_t();
rcoverridech1 = 0;
rc_over.chan1_raw = 0;
rcoverridech2 = 0;
rc_over.chan2_raw = 0;
rcoverridech3 = 0;
rc_over.chan3_raw = 0;
rcoverridech4 = 0;
rc_over.chan4_raw = 0;
rcoverridech5 = 0;
rc_over.chan5_raw = 0;
rcoverridech6 = 0;
rc_over.chan6_raw = 0;
rc_over.target_component = MainV2.comPort.MAV.compid;
rc_over.target_system = MainV2.comPort.MAV.sysid;
MainV2.comPort.sendPacket(rc_over);
System.Threading.Thread.Sleep(20);
MainV2.comPort.sendPacket(rc_over);
}
if (DateTime.Now > lastupdate.AddMilliseconds(50) || updatenow) // 20 hz
{
lastupdate = DateTime.Now;
//check if valid mavinterface
if (parent != null && parent.packetsnotlost != 0)
{
if ((DateTime.Now - MAV.lastvalidpacket).TotalSeconds > 10)
{
linkqualitygcs = 0;
}
else
{
linkqualitygcs = (ushort)((parent.packetsnotlost / (parent.packetsnotlost + parent.packetslost)) * 100.0);
}
if (linkqualitygcs > 100)
linkqualitygcs = 100;
}
if (datetime.Second != lastsecondcounter.Second)
{
lastsecondcounter = datetime;
if (lastpos.Lat != 0 && lastpos.Lng != 0 && armed)
{
if (!mavinterface.BaseStream.IsOpen && !mavinterface.logreadmode)
distTraveled = 0;
distTraveled += (float)lastpos.GetDistance(new PointLatLngAlt(lat, lng, 0, "")) * multiplierdist;
lastpos = new PointLatLngAlt(lat, lng, 0, "");
}
else
{
lastpos = new PointLatLngAlt(lat, lng, 0, "");
}
// throttle is up, or groundspeed is > 3 m/s
if (ch3percent > 12 || _groundspeed > 3.0)
timeInAir++;
if (!gotwind)
dowindcalc();
}
// re-request streams
if (!(lastdata.AddSeconds(8) > DateTime.Now) && mavinterface.BaseStream.IsOpen)
{
try
{
mavinterface.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS, MAV.cs.ratestatus, MAV.sysid); // mode
mavinterface.requestDatastream(MAVLink.MAV_DATA_STREAM.POSITION, MAV.cs.rateposition, MAV.sysid); // request gps
mavinterface.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTRA1, MAV.cs.rateattitude, MAV.sysid); // request attitude
mavinterface.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTRA2, MAV.cs.rateattitude, MAV.sysid); // request vfr
mavinterface.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTRA3, MAV.cs.ratesensors, MAV.sysid); // request extra stuff - tridge
mavinterface.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MAV.cs.ratesensors, MAV.sysid); // request raw sensor
mavinterface.requestDatastream(MAVLink.MAV_DATA_STREAM.RC_CHANNELS, MAV.cs.raterc, MAV.sysid); // request rc info
}
catch { log.Error("Failed to request rates"); }
lastdata = DateTime.Now.AddSeconds(30); // prevent flooding
}
byte[] bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.RC_CHANNELS_SCALED];
if (bytearray != null) // hil mavlink 0.9
{
var hil = bytearray.ByteArrayToStructure<MAVLink.mavlink_rc_channels_scaled_t>(6);
hilch1 = hil.chan1_scaled;
hilch2 = hil.chan2_scaled;
hilch3 = hil.chan3_scaled;
hilch4 = hil.chan4_scaled;
hilch5 = hil.chan5_scaled;
hilch6 = hil.chan6_scaled;
hilch7 = hil.chan7_scaled;
hilch8 = hil.chan8_scaled;
// Console.WriteLine("RC_CHANNELS_SCALED Packet");
MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.RC_CHANNELS_SCALED] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.FENCE_STATUS];
if (bytearray != null)
{
var fence = bytearray.ByteArrayToStructure<MAVLink.mavlink_fence_status_t>(6);
if (fence.breach_status != (byte)MAVLink.FENCE_BREACH.NONE)
{
// fence breached
messageHigh = "Fence Breach";
messageHighTime = DateTime.Now;
}
MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.FENCE_STATUS] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.HIL_CONTROLS];
if (bytearray != null) // hil mavlink 0.9 and 1.0
{
var hil = bytearray.ByteArrayToStructure<MAVLink.mavlink_hil_controls_t>(6);
hilch1 = (int)(hil.roll_ailerons * 10000);
hilch2 = (int)(hil.pitch_elevator * 10000);
hilch3 = (int)(hil.throttle * 10000);
hilch4 = (int)(hil.yaw_rudder * 10000);
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.HIL_CONTROLS] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.OPTICAL_FLOW];
if (bytearray != null)
{
var optflow = bytearray.ByteArrayToStructure<MAVLink.mavlink_optical_flow_t>(6);
opt_m_x = optflow.flow_comp_m_x;
opt_m_y = optflow.flow_comp_m_x;
opt_x = optflow.flow_x;
opt_y = optflow.flow_y;
opt_qua = optflow.quality;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.MOUNT_STATUS];
if (bytearray != null)
{
var status = bytearray.ByteArrayToStructure<MAVLink.mavlink_mount_status_t>(6);
campointa = status.pointing_a / 100.0f;
campointb = status.pointing_b / 100.0f;
campointc = status.pointing_c / 100.0f;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.VIBRATION];
if (bytearray != null)
{
var vibe = bytearray.ByteArrayToStructure<MAVLink.mavlink_vibration_t>(6);
vibeclip0 = vibe.clipping_0;
vibeclip1 = vibe.clipping_1;
vibeclip2 = vibe.clipping_2;
vibex = vibe.vibration_x;
vibey = vibe.vibration_y;
vibez = vibe.vibration_z;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.AIRSPEED_AUTOCAL];
if (bytearray != null)
{
var asac = bytearray.ByteArrayToStructure<MAVLink.mavlink_airspeed_autocal_t>(6);
asratio = asac.ratio;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SYSTEM_TIME];
if (bytearray != null)
{
var systime = bytearray.ByteArrayToStructure<MAVLink.mavlink_system_time_t>(6);
DateTime date1 = new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc);
try
{
date1 = date1.AddMilliseconds(systime.time_unix_usec/1000);
gpstime = date1;
}
catch { }
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.HWSTATUS];
if (bytearray != null)
{
var hwstatus = bytearray.ByteArrayToStructure<MAVLink.mavlink_hwstatus_t>(6);
hwvoltage = hwstatus.Vcc / 1000.0f;
i2cerrors = hwstatus.I2Cerr;
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.HWSTATUS] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.EKF_STATUS_REPORT];
if (bytearray != null)
{
var ekfstatus = bytearray.ByteArrayToStructure<MAVLink.mavlink_ekf_status_report_t>(6);
// > 1, between 0-1 typical > 1 = reject measurement - red
// 0.5 > amber
ekfvelv = ekfstatus.velocity_variance;
ekfcompv = ekfstatus.compass_variance;
ekfposhor = ekfstatus.pos_horiz_variance;
ekfposvert = ekfstatus.pos_vert_variance;
ekfteralt = ekfstatus.terrain_alt_variance;
//TODO need to localize - wait for testing to complete first
if (ekfvelv >= 1)
{
messageHigh = Strings.ERROR + " " + "velocity variance";
messageHighTime = DateTime.Now;
}
if (ekfcompv >= 1)
{
messageHigh = Strings.ERROR + " " + "compass variance";
messageHighTime = DateTime.Now;
}
if (ekfposhor >= 1)
{
messageHigh = Strings.ERROR + " " + "pos horiz variance";
messageHighTime = DateTime.Now;
}
if (ekfposvert >= 1)
{
messageHigh = Strings.ERROR + " " + "pos vert variance";
messageHighTime = DateTime.Now;
}
if (ekfteralt >= 1)
{
messageHigh = Strings.ERROR + " " + "terrain alt variance";
messageHighTime = DateTime.Now;
}
for (int a = 1; a < (int)MAVLink.EKF_STATUS_FLAGS.ENUM_END; a = a << 1)
{
int currentbit = (ekfstatus.flags & a);
if (currentbit == 0)
{
var currentflag =
(MAVLink.EKF_STATUS_FLAGS)
Enum.Parse(typeof(MAVLink.EKF_STATUS_FLAGS), a.ToString());
switch (currentflag)
{
case MAVLink.EKF_STATUS_FLAGS.EKF_ATTITUDE: // step 1
case MAVLink.EKF_STATUS_FLAGS.EKF_VELOCITY_HORIZ: // with pos
case MAVLink.EKF_STATUS_FLAGS.EKF_VELOCITY_VERT: // with pos
//case MAVLink.EKF_STATUS_FLAGS.EKF_POS_HORIZ_REL: // optical flow
case MAVLink.EKF_STATUS_FLAGS.EKF_POS_HORIZ_ABS: // step 1
case MAVLink.EKF_STATUS_FLAGS.EKF_POS_VERT_ABS: // step 1
//case MAVLink.EKF_STATUS_FLAGS.EKF_POS_VERT_AGL: // range finder
//case MAVLink.EKF_STATUS_FLAGS.EKF_CONST_POS_MODE: // never true when absolute - non gps
//case MAVLink.EKF_STATUS_FLAGS.EKF_PRED_POS_HORIZ_REL: // optical flow
case MAVLink.EKF_STATUS_FLAGS.EKF_PRED_POS_HORIZ_ABS: // ekf has origin - post arm
//messageHigh = Strings.ERROR + " " + currentflag.ToString().Replace("_", " ");
//messageHighTime = DateTime.Now;
break;
default:
break;
}
}
}
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.RANGEFINDER];
if (bytearray != null)
{
var sonar = bytearray.ByteArrayToStructure<MAVLink.mavlink_rangefinder_t>(6);
sonarrange = sonar.distance;
sonarvoltage = sonar.voltage;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.POWER_STATUS];
if (bytearray != null)
{
var power = bytearray.ByteArrayToStructure<MAVLink.mavlink_power_status_t>(6);
boardvoltage = power.Vcc;
servovoltage = power.Vservo;
voltageflag = (MAVLink.MAV_POWER_STATUS) power.flags;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.WIND];
if (bytearray != null)
{
var wind = bytearray.ByteArrayToStructure<MAVLink.mavlink_wind_t>(6);
gotwind = true;
wind_dir = (wind.direction + 360) % 360;
wind_vel = wind.speed * multiplierspeed;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.HEARTBEAT];
if (bytearray != null)
{
var hb = bytearray.ByteArrayToStructure<MAVLink.mavlink_heartbeat_t>(6);
if (hb.type == (byte)MAVLink.MAV_TYPE.GCS)
{
// skip gcs hb's
// only happens on log playback - and shouldnt get them here
}
else
{
armed = (hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) == (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED;
// for future use
landed = hb.system_status == (byte)MAVLink.MAV_STATE.STANDBY;
failsafe = hb.system_status == (byte)MAVLink.MAV_STATE.CRITICAL;
string oldmode = mode;
if ((hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED) != 0)
{
// prevent running thsi unless we have to
if (_mode != hb.custom_mode)
{
List<KeyValuePair<int, string>> modelist = Common.getModesList(this);
bool found = false;
foreach (KeyValuePair<int, string> pair in modelist)
{
if (pair.Key == hb.custom_mode)
{
mode = pair.Value.ToString();
_mode = hb.custom_mode;
found = true;
break;
}
}
if (!found)
{
log.Warn("Mode not found bm:" + hb.base_mode + " cm:" + hb.custom_mode);
_mode = hb.custom_mode;
}
}
}
if (oldmode != mode && MainV2.speechEnable && MainV2.comPort.MAV.cs == this && MainV2.getConfig("speechmodeenabled") == "True")
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode")));
}
}
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SYS_STATUS];
if (bytearray != null)
{
var sysstatus = bytearray.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6);
load = (float)sysstatus.load / 10.0f;
battery_voltage = (float)sysstatus.voltage_battery / 1000.0f;
battery_remaining = sysstatus.battery_remaining;
current = (float)sysstatus.current_battery / 100.0f;
packetdropremote = sysstatus.drop_rate_comm;
Mavlink_Sensors sensors_enabled = new Mavlink_Sensors(sysstatus.onboard_control_sensors_enabled);
Mavlink_Sensors sensors_health = new Mavlink_Sensors(sysstatus.onboard_control_sensors_health);
Mavlink_Sensors sensors_present = new Mavlink_Sensors(sysstatus.onboard_control_sensors_present);
terrainactive = sensors_health.terrain && sensors_enabled.terrain && sensors_present.terrain;
if (sensors_health.gps != sensors_enabled.gps && sensors_present.gps)
{
messageHigh = Strings.BadGPSHealth;
messageHighTime = DateTime.Now;
}
else if (sensors_health.gyro != sensors_enabled.gyro && sensors_present.gyro)
{
messageHigh = Strings.BadGyroHealth;
messageHighTime = DateTime.Now;
}
else if (sensors_health.accelerometer != sensors_enabled.accelerometer && sensors_present.accelerometer)
{
messageHigh = Strings.BadAccelHealth;
messageHighTime = DateTime.Now;
}
else if (sensors_health.compass != sensors_enabled.compass && sensors_present.compass)
{
messageHigh = Strings.BadCompassHealth;
messageHighTime = DateTime.Now;
}
else if (sensors_health.barometer != sensors_enabled.barometer && sensors_present.barometer)
{
messageHigh = Strings.BadBaroHealth;
messageHighTime = DateTime.Now;
}
else if (sensors_health.LASER_POSITION != sensors_enabled.LASER_POSITION && sensors_present.LASER_POSITION)
{
messageHigh = Strings.BadLiDARHealth;
messageHighTime = DateTime.Now;
}
else if (sensors_health.optical_flow != sensors_enabled.optical_flow && sensors_present.optical_flow)
{
messageHigh = Strings.BadOptFlowHealth;
messageHighTime = DateTime.Now;
}
else if (sensors_health.terrain != sensors_enabled.terrain && sensors_present.terrain)
{
messageHigh = Strings.BadorNoTerrainData;
messageHighTime = DateTime.Now;
}
else if (sensors_health.geofence != sensors_enabled.geofence && sensors_present.geofence)
{
messageHigh = Strings.GeofenceBreach;
messageHighTime = DateTime.Now;
}
else if (sensors_health.ahrs != sensors_enabled.ahrs && sensors_present.ahrs)
{
messageHigh = Strings.BadAHRS;
messageHighTime = DateTime.Now;
}
else if (sensors_health.rc_receiver != sensors_enabled.rc_receiver && sensors_present.rc_receiver)
{
messageHigh = Strings.NORCReceiver;
messageHighTime = DateTime.Now;
}
MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SYS_STATUS] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.BATTERY2];
if (bytearray != null)
{
var bat = bytearray.ByteArrayToStructure<MAVLink.mavlink_battery2_t>(6);
_battery_voltage2 = bat.voltage;
current2 = bat.current_battery;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SCALED_PRESSURE];
if (bytearray != null)
{
var pres = bytearray.ByteArrayToStructure<MAVLink.mavlink_scaled_pressure_t>(6);
press_abs = pres.press_abs;
press_temp = pres.temperature;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.TERRAIN_REPORT];
if (bytearray != null)
{
var terrainrep = bytearray.ByteArrayToStructure<MAVLink.mavlink_terrain_report_t>(6);
ter_curalt = terrainrep.current_height;
ter_alt = terrainrep.terrain_height;
ter_load = terrainrep.loaded;
ter_pend = terrainrep.pending;
ter_space = terrainrep.spacing;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SENSOR_OFFSETS];
if (bytearray != null)
{
var sensofs = bytearray.ByteArrayToStructure<MAVLink.mavlink_sensor_offsets_t>(6);
mag_ofs_x = sensofs.mag_ofs_x;
mag_ofs_y = sensofs.mag_ofs_y;
mag_ofs_z = sensofs.mag_ofs_z;
mag_declination = sensofs.mag_declination;
raw_press = sensofs.raw_press;
raw_temp = sensofs.raw_temp;
gyro_cal_x = sensofs.gyro_cal_x;
gyro_cal_y = sensofs.gyro_cal_y;
gyro_cal_z = sensofs.gyro_cal_z;
accel_cal_x = sensofs.accel_cal_x;
accel_cal_y = sensofs.accel_cal_y;
accel_cal_z = sensofs.accel_cal_z;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.ATTITUDE];
if (bytearray != null)
{
var att = bytearray.ByteArrayToStructure<MAVLink.mavlink_attitude_t>(6);
roll = att.roll * rad2deg;
pitch = att.pitch * rad2deg;
yaw = att.yaw * rad2deg;
//Console.WriteLine(MAV.sysid + " " +roll + " " + pitch + " " + yaw);
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.ATTITUDE] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT];
if (bytearray != null)
{
var loc = bytearray.ByteArrayToStructure<MAVLink.mavlink_global_position_int_t>(6);
// the new arhs deadreckoning may send 0 alt and 0 long. check for and undo
alt = loc.relative_alt / 1000.0f;
useLocation = true;
if (loc.lat == 0 && loc.lon == 0)
{
useLocation = false;
}
else
{
lat = loc.lat / 10000000.0;
lng = loc.lon / 10000000.0;
altasl = loc.alt / 1000.0f;
}
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.GPS_RAW_INT];
if (bytearray != null)
{
var gps = bytearray.ByteArrayToStructure<MAVLink.mavlink_gps_raw_int_t>(6);
if (!useLocation)
{
lat = gps.lat * 1.0e-7;
lng = gps.lon * 1.0e-7;
altasl = gps.alt / 1000.0f;
// alt = gps.alt; // using vfr as includes baro calc
}
gpsstatus = gps.fix_type;
// Console.WriteLine("gpsfix {0}",gpsstatus);
gpshdop = (float)Math.Round((double)gps.eph / 100.0, 2);
satcount = gps.satellites_visible;
groundspeed = gps.vel * 1.0e-2f;
groundcourse = gps.cog * 1.0e-2f;
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.GPS_RAW] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.GPS2_RAW];
if (bytearray != null)
{
var gps = bytearray.ByteArrayToStructure<MAVLink.mavlink_gps2_raw_t>(6);
lat2 = gps.lat * 1.0e-7;
lng2 = gps.lon * 1.0e-7;
altasl2 = gps.alt / 1000.0f;
gpsstatus2 = gps.fix_type;
gpshdop2 = (float)Math.Round((double)gps.eph / 100.0, 2);
satcount2 = gps.satellites_visible;
groundspeed2 = gps.vel * 1.0e-2f;
groundcourse2 = gps.cog * 1.0e-2f;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.GPS_STATUS];
if (bytearray != null)
{
var gps = bytearray.ByteArrayToStructure<MAVLink.mavlink_gps_status_t>(6);
satcount = gps.satellites_visible;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.RADIO];
if (bytearray != null)
{
var radio = bytearray.ByteArrayToStructure<MAVLink.mavlink_radio_t>(6);
rssi = radio.rssi;
remrssi = radio.remrssi;
txbuffer = radio.txbuf;
rxerrors = radio.rxerrors;
noise = radio.noise;
remnoise = radio.remnoise;
fixedp = radio.@fixed;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.RADIO_STATUS];
if (bytearray != null)
{
var radio = bytearray.ByteArrayToStructure<MAVLink.mavlink_radio_status_t>(6);
rssi = radio.rssi;
remrssi = radio.remrssi;
txbuffer = radio.txbuf;
rxerrors = radio.rxerrors;
noise = radio.noise;
remnoise = radio.remnoise;
fixedp = radio.@fixed;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.MISSION_CURRENT];
if (bytearray != null)
{
var wpcur = bytearray.ByteArrayToStructure<MAVLink.mavlink_mission_current_t>(6);
int oldwp = (int)wpno;
wpno = wpcur.seq;
if (mode.ToLower() == "auto" && wpno != 0)
{
lastautowp = (int)wpno;
}
if (oldwp != wpno && MainV2.speechEnable && MainV2.comPort.MAV.cs == this && MainV2.getConfig("speechwaypointenabled") == "True")
{
MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint")));
}
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.WAYPOINT_CURRENT] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.NAV_CONTROLLER_OUTPUT];
if (bytearray != null)
{
var nav = bytearray.ByteArrayToStructure<MAVLink.mavlink_nav_controller_output_t>(6);
nav_roll = nav.nav_roll;
nav_pitch = nav.nav_pitch;
nav_bearing = nav.nav_bearing;
target_bearing = nav.target_bearing;
wp_dist = nav.wp_dist;
alt_error = nav.alt_error;
aspd_error = nav.aspd_error / 100.0f;
xtrack_error = nav.xtrack_error;
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.NAV_CONTROLLER_OUTPUT] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.RC_CHANNELS_RAW];
if (bytearray != null)
{
var rcin = bytearray.ByteArrayToStructure<MAVLink.mavlink_rc_channels_raw_t>(6);
ch1in = rcin.chan1_raw;
ch2in = rcin.chan2_raw;
ch3in = rcin.chan3_raw;
ch4in = rcin.chan4_raw;
ch5in = rcin.chan5_raw;
ch6in = rcin.chan6_raw;
ch7in = rcin.chan7_raw;
ch8in = rcin.chan8_raw;
//percent
rxrssi = (int)((rcin.rssi / 255.0) * 100.0);
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.RC_CHANNELS_RAW] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.RC_CHANNELS];
if (bytearray != null)
{
var rcin = bytearray.ByteArrayToStructure<MAVLink.mavlink_rc_channels_t>(6);
ch1in = rcin.chan1_raw;
ch2in = rcin.chan2_raw;
ch3in = rcin.chan3_raw;
ch4in = rcin.chan4_raw;
ch5in = rcin.chan5_raw;
ch6in = rcin.chan6_raw;
ch7in = rcin.chan7_raw;
ch8in = rcin.chan8_raw;
ch9in = rcin.chan9_raw;
ch10in = rcin.chan10_raw;
ch11in = rcin.chan11_raw;
ch12in = rcin.chan12_raw;
ch13in = rcin.chan13_raw;
ch14in = rcin.chan14_raw;
ch15in = rcin.chan15_raw;
ch16in = rcin.chan16_raw;
//percent
rxrssi = (int)((rcin.rssi / 255.0) * 100.0);
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.RC_CHANNELS_RAW] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SERVO_OUTPUT_RAW];
if (bytearray != null)
{
var servoout = bytearray.ByteArrayToStructure<MAVLink.mavlink_servo_output_raw_t>(6);
ch1out = servoout.servo1_raw;
ch2out = servoout.servo2_raw;
ch3out = servoout.servo3_raw;
ch4out = servoout.servo4_raw;
ch5out = servoout.servo5_raw;
ch6out = servoout.servo6_raw;
ch7out = servoout.servo7_raw;
ch8out = servoout.servo8_raw;
MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SERVO_OUTPUT_RAW] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.RAW_IMU];
if (bytearray != null)
{
var imu = bytearray.ByteArrayToStructure<MAVLink.mavlink_raw_imu_t>(6);
gx = imu.xgyro;
gy = imu.ygyro;
gz = imu.zgyro;
ax = imu.xacc;
ay = imu.yacc;
az = imu.zacc;
mx = imu.xmag;
my = imu.ymag;
mz = imu.zmag;
var timesec = imu.time_usec*1.0e-6;
var deltawall = (DateTime.Now - lastimutime).TotalSeconds;
var deltaimu = timesec - imutime;
//Console.WriteLine( + " " + deltawall + " " + deltaimu + " " + System.Threading.Thread.CurrentThread.Name);
if (speedup > 0)
speedup = (float)(speedup * 0.95 + (deltaimu / deltawall) * 0.05);
imutime = timesec;
lastimutime = DateTime.Now;
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.RAW_IMU] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SCALED_IMU];
if (bytearray != null)
{
var imu = bytearray.ByteArrayToStructure<MAVLink.mavlink_scaled_imu_t>(6);
gx = imu.xgyro;
gy = imu.ygyro;
gz = imu.zgyro;
ax = imu.xacc;
ay = imu.yacc;
az = imu.zacc;
mx = imu.xmag;
my = imu.ymag;
mz = imu.zmag;
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.RAW_IMU] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.SCALED_IMU2];
if (bytearray != null)
{
var imu2 = bytearray.ByteArrayToStructure<MAVLink.mavlink_scaled_imu2_t>(6);
gx2 = imu2.xgyro;
gy2 = imu2.ygyro;
gz2 = imu2.zgyro;
ax2 = imu2.xacc;
ay2 = imu2.yacc;
az2 = imu2.zacc;
mx2 = imu2.xmag;
my2 = imu2.ymag;
mz2 = imu2.zmag;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.PID_TUNING];
if (bytearray != null)
{
var pid = bytearray.ByteArrayToStructure<MAVLink.mavlink_pid_tuning_t>(6);
//todo: currently only deals with single axis at once
pidff = pid.FF;
pidP = pid.P;
pidI = pid.I;
pidD = pid.D;
pidaxis = pid.axis;
piddesired = pid.desired;
pidachieved = pid.achieved;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.VFR_HUD];
if (bytearray != null)
{
var vfr = bytearray.ByteArrayToStructure<MAVLink.mavlink_vfr_hud_t>(6);
groundspeed = vfr.groundspeed;
airspeed = vfr.airspeed;
//alt = vfr.alt; // this might include baro
ch3percent = vfr.throttle;
//Console.WriteLine(alt);
//climbrate = vfr.climb;
// heading = vfr.heading;
//MAVLink.packets[(byte)MAVLink.MSG_NAMES.VFR_HUD] = null;
}
bytearray = MAV.packets[(byte)MAVLink.MAVLINK_MSG_ID.MEMINFO];
if (bytearray != null)
{
var mem = bytearray.ByteArrayToStructure<MAVLink.mavlink_meminfo_t>(6);
freemem = mem.freemem;
brklevel = mem.brkval;
}
}
try
{
if (csCallBack != null)
csCallBack(this, null);
}
catch { }
//Console.Write(DateTime.Now.Millisecond + " start ");
// update form
try
{
if (bs != null)
{
bs.DataSource = this;
bs.ResetBindings(false);
return;
/*
if (bs.Count > 200)
{
while (bs.Count > 3)
bs.RemoveAt(1);
//bs.Clear();
}
bs.Add(this);
/*
return;
bs.DataSource = this;
bs.ResetBindings(false);
return;
hires.Stopwatch sw = new hires.Stopwatch();
sw.Start();
bs.DataSource = this;
bs.ResetBindings(false);
sw.Stop();
var elaps = sw.Elapsed;
Console.WriteLine("1 " + elaps.ToString("0.#####") + " done ");
sw.Start();
bs.SuspendBinding();
bs.Clear();
bs.ResumeBinding();
bs.Add(this);
sw.Stop();
elaps = sw.Elapsed;
Console.WriteLine("2 " + elaps.ToString("0.#####") + " done ");
sw.Start();
if (bs.Count > 100)
bs.Clear();
bs.Add(this);
sw.Stop();
elaps = sw.Elapsed;
Console.WriteLine("3 " + elaps.ToString("0.#####") + " done ");
*/
}
}
catch { log.InfoFormat("CurrentState Binding error"); }
}
}
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();
try
{
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));
}
catch { CustomMessageBox.Show("Bad Channel Setting"); return; }
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";
}
}
void UpdateInformation()
{
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;
//this.label1.Text = "1) Включить режим MANUAL. ";
if (count == 1)
{
MainV2.comPort.setMode("Manual");
MainV2.comPort.MAV.cs.rcoverridech1 = pickChannel(1, -1);
rc.chan1_raw = pickChannel(1, -1);
sendPackets(rc);
this.label1.Text = "1) Проверьте, что левый элерон отклонился вверх, правый вниз.";
}
if (count == 2)
{
MainV2.comPort.MAV.cs.rcoverridech1 = pickChannel(1, 1);
rc.chan1_raw = pickChannel(1, 1);
MainV2.comPort.sendPacket(rc);
this.label1.Text = "2) Проверьте, что правый элерон отклоняется вверх, левый вниз.";
sendPackets(rc);
}
if (count == 3)
{
// Recover state of roll
MainV2.comPort.MAV.cs.rcoverridech1 = pickChannel(1, 0);
MainV2.comPort.MAV.cs.rcoverridech2 = pickChannel(2, -1);
rc.chan1_raw = pickChannel(1, 0);
rc.chan2_raw = pickChannel(2, -1);
sendPackets(rc);
this.label1.Text = "3) Проверьте, что рули высоты отклоняются вверх.";
}
if (count == 4)
{
MainV2.comPort.MAV.cs.rcoverridech2 = pickChannel(2, 1);
rc.chan2_raw = pickChannel(2, 1);
sendPackets(rc);
this.label1.Text = "4) Проверьте, что рули высоты отклоняются вниз.";
}
if (count == 5)
{
// Recover state of pitch
MainV2.comPort.MAV.cs.rcoverridech2 = pickChannel(2, 0);
rc.chan2_raw = pickChannel(2, 0);
// Change the State of rudder
MainV2.comPort.MAV.cs.rcoverridech4 = pickChannel(4, -1);
rc.chan4_raw = pickChannel(4, -1);
sendPackets(rc);
this.label1.Text = "5) Проверьте, что рули высоты отклоняются влево.";
}
if (count == 6)
{
MainV2.comPort.MAV.cs.rcoverridech4 = pickChannel(4, 1);
rc.chan4_raw = pickChannel(4, 1);
sendPackets(rc);
this.label1.Text = "6) Проверьте, что рули высоты отклоняются вправо.";
}
if (count == 7)
{
MainV2.comPort.MAV.cs.rcoverridech4 = pickChannel(4, 0);
rc.chan4_raw = pickChannel(4, 0);
MainV2.comPort.setMode("FBWA");
this.label1.Text = "7) Проверьте, что левый элерон отклоняется вверх, правый вниз, рули высоты влево.";
}
if (count == 8)
{
MainV2.comPort.setMode("FBWA");
this.label1.Text = "8) Проверьте, что правый элерон отклоняется вверх, левый вниз, рули высоты вправо.";
}
if (count == 9)
{
this.label1.Text = "9) Проверьте, что рули высоты отклоняются вверх.";
}
if (count == 10)
{
this.label1.Text = "10)Проверьте, что рули высоты отклоняются вниз.";
}
if (count == 11)
{
this.label1.Text = "11) Наклонить самолет влево – левый элерон отклоняется вниз, правый вверх, рули высоты вправо.\n Наклонить самолет вправо – правый элерон отклоняется вниз, левый вверх, рули высоты влево.";
}
if (count == 12)
{
this.label1.Text = "12) Наклонить самолет носом вниз – рули высоты отклоняются вверх. \n Наклонить самолет носом вверх – рули высоты отклоняются вниз.";
}
}
/// <summary>
/// thread used to send joystick packets to the MAV
/// </summary>
private void joysticksend()
{
float rate = 50; // 1000 / 50 = 20 hz
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
}
}
public void clearRCOverride()
{
// disable it, before continuing
this.enabled = false;
MAVLink.mavlink_rc_channels_override_t rc = new MAVLink.mavlink_rc_channels_override_t();
rc.target_component = Interface.MAV.compid;
rc.target_system = Interface.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;
rc.chan9_raw = 0;
rc.chan10_raw = 0;
rc.chan11_raw = 0;
rc.chan12_raw = 0;
rc.chan13_raw = 0;
rc.chan14_raw = 0;
rc.chan15_raw = 0;
rc.chan16_raw = 0;
rc.chan17_raw = 0;
rc.chan18_raw = 0;
Interface.MAV.cs.rcoverridech1 = 0;
Interface.MAV.cs.rcoverridech2 = 0;
Interface.MAV.cs.rcoverridech3 = 0;
Interface.MAV.cs.rcoverridech4 = 0;
Interface.MAV.cs.rcoverridech5 = 0;
Interface.MAV.cs.rcoverridech6 = 0;
Interface.MAV.cs.rcoverridech7 = 0;
Interface.MAV.cs.rcoverridech8 = 0;
Interface.MAV.cs.rcoverridech9 = 0;
Interface.MAV.cs.rcoverridech10 = 0;
Interface.MAV.cs.rcoverridech11 = 0;
Interface.MAV.cs.rcoverridech12 = 0;
Interface.MAV.cs.rcoverridech13 = 0;
Interface.MAV.cs.rcoverridech14 = 0;
Interface.MAV.cs.rcoverridech15 = 0;
Interface.MAV.cs.rcoverridech16 = 0;
Interface.MAV.cs.rcoverridech17 = 0;
Interface.MAV.cs.rcoverridech18 = 0;
try
{
Interface.sendPacket(rc, rc.target_system, rc.target_component);
System.Threading.Thread.Sleep(20);
Interface.sendPacket(rc, rc.target_system, rc.target_component);
System.Threading.Thread.Sleep(20);
Interface.sendPacket(rc, rc.target_system, rc.target_component);
System.Threading.Thread.Sleep(20);
Interface.sendPacket(rc, rc.target_system, rc.target_component);
System.Threading.Thread.Sleep(20);
Interface.sendPacket(rc, rc.target_system, rc.target_component);
System.Threading.Thread.Sleep(20);
Interface.sendPacket(rc, rc.target_system, rc.target_component);
Interface.sendPacket(rc, rc.target_system, rc.target_component);
Interface.sendPacket(rc, rc.target_system, rc.target_component);
Interface.sendPacket(rc, rc.target_system, rc.target_component);
}
catch (Exception ex)
{
log.Error(ex);
}
}
/// <summary>
/// thread used to send joystick packets to the MAV
/// </summary>
private void joysticksend()
{
float rate = 50; // 1000 / 50 = 20 hz
int count = 0;
DateTime lastratechange = DateTime.Now;
joystickthreadrun = true;
while (joystickthreadrun)
{
joysendThreadExited = false;
//so we know this thread is stil alive.
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.Joystick.joystickaxis.None)
rc.chan1_raw = MainV2.comPort.MAV.cs.rcoverridech1;
if (joystick.getJoystickAxis(2) != Joystick.Joystick.joystickaxis.None)
rc.chan2_raw = MainV2.comPort.MAV.cs.rcoverridech2;
if (joystick.getJoystickAxis(3) != Joystick.Joystick.joystickaxis.None)
rc.chan3_raw = MainV2.comPort.MAV.cs.rcoverridech3;
if (joystick.getJoystickAxis(4) != Joystick.Joystick.joystickaxis.None)
rc.chan4_raw = MainV2.comPort.MAV.cs.rcoverridech4;
if (joystick.getJoystickAxis(5) != Joystick.Joystick.joystickaxis.None)
rc.chan5_raw = MainV2.comPort.MAV.cs.rcoverridech5;
if (joystick.getJoystickAxis(6) != Joystick.Joystick.joystickaxis.None)
rc.chan6_raw = MainV2.comPort.MAV.cs.rcoverridech6;
if (joystick.getJoystickAxis(7) != Joystick.Joystick.joystickaxis.None)
rc.chan7_raw = MainV2.comPort.MAV.cs.rcoverridech7;
if (joystick.getJoystickAxis(8) != Joystick.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);
//Console.WriteLine("Joystick btw " + comPort.BaseStream.BytesToWrite);
if (!comPort.BaseStream.IsOpen)
continue;
if (comPort.BaseStream.BytesToWrite < 50)
{
if (sitl)
{
MissionPlanner.Controls.SITL.rcinput();
}
else
{
comPort.sendPacket(rc);
}
count++;
lastjoystick = DateTime.Now;
}
}
}
}
Thread.Sleep(20);
}
catch
{
} // cant fall out
}
joysendThreadExited = true;//so we know this thread exited.
}
private void label1_DoubleClick(object sender, EventArgs e)
{
foreach (var port in MainV2.Comports)
{
MAVLink.mavlink_rc_channels_override_t rc = new MAVLink.mavlink_rc_channels_override_t();
rc.target_component = port.MAV.compid;
rc.target_system = port.MAV.sysid;
ushort pwm = 0;
rc.chan3_raw = pwm;
label1.Text = pwm.ToString();
port.sendPacket(rc);
}
}
