private void bt_closeall_Click(object sender, EventArgs e)
{
try
{
UDPComm.oktoReceive = false;
closeThread(tstartMissonPlannerConn);
closeThread(tstartmyServerConn);
closeThread(tstartTrimaresConn);
closeThread(tstartExoReadings);
closeThread(Treadings);
if (TCPClientConn != null)
TCPClientConn = null;
if (TCPClientData != null)
TCPClientData = null;
if (TCPMavLinkClient != null)
TCPMavLinkClient = null;
if (TrimaresUDPServerConn != null)
TrimaresUDPServerConn = null;
if (TrimaresUDPServerData != null)
TrimaresUDPServerData = null;
if (TrimaresUDPMavLinkServer != null)
TrimaresUDPMavLinkServer = null;
}
catch { }
//Application.ExitThread();
//Application.Exit();
}
public void setMavlinkPointer(ref MAVLink newMav)
{
newMav = myLink;
}
/// <summary>
/// inicializa comunicacao do router com o trimares
/// </summary>
private void startTrimaresConn()
{
try
{
TrimaresServer = new MAVcomm.Comms.UdpSerialServer();
TrimaresServer.HostIP = tx_udpIP.Text;
TrimaresServer.Port = tx_udpport.Text;
TrimaresServer.newMsg += TrimaresUDP_PacketReceived;
TrimaresServer.Open();
TrimaresServer.rMessage += showMessages;
//TrimaresUDPServerData = new MainCrossData();
//TrimaresUDPServerConn = new MavLinkConnection("UDPSERVER", ref TrimaresUDPServerData);
//TrimaresUDPServerConn.mydata.setMavlinkPointer(ref TrimaresUDPMavLinkServer);
//TrimaresUDPServerConn.mydata.myLink.BaseStream.HostIP = tx_udpIP.Text;
//TrimaresUDPServerConn.mydata.myLink.BaseStream.Port = tx_udpport.Text;
//TrimaresUDPServerConn.PacketReceived += TrimaresUDP_PacketReceived;
//TrimaresUDPServerConn.mydata.myLink.BaseStream.Open();
//TrimaresUDPServerConn.mydata.myLink.BaseStream.rMessage += showMessages;
}
catch
{
TrimaresServer.Close();
TrimaresServer = null;
TrimaresUDPMavLinkServer = null;
TrimaresUDPServerData = null;
TrimaresUDPServerConn = null;
}
}
private void Form1_FormClosing(object sender, FormClosingEventArgs e)
{
try
{
if (tstartMissonPlannerConn != null)
{
tstartMissonPlannerConn.Abort();
Thread.Sleep(100);
tstartMissonPlannerConn = null;
}
closeThread(tstartMissonPlannerConn);
closeThread(tstartmyServerConn);
closeThread(tstartTrimaresConn);
closeThread(tstartExoReadings);
closeThread(Treadings);
UDPComm.oktoReceive = false;
if (MissionPlannerServer != null)
{
MissionPlannerServer.Close();
MissionPlannerServer = null;
}
if (TrimaresServer != null)
{
TrimaresServer.Close();
TrimaresServer = null;
}
if (TCPClientConn != null)
{
CloseStream(TCPClientConn.mydata.myLink.BaseStream);
TCPClientConn = null;
}
if (TCPMavLinkClient != null)
{
TCPMavLinkClient = null;
}
if (TCPClientData != null)
{
TCPClientData = null;
}
if (TrimaresUDPServerConn != null)
{
CloseStream(TrimaresUDPServerConn.mydata.myLink.BaseStream);
TrimaresUDPServerConn = null;
}
if (TrimaresUDPServerData != null)
TrimaresUDPServerData = null;
if (TrimaresUDPMavLinkServer != null)
TrimaresUDPMavLinkServer = null;
foreach (Process p in System.Diagnostics.Process.GetProcessesByName("AP_Mavlink_router.exe"))
{
p.Kill();
p.WaitForExit();
}
}
catch
{
Application.ExitThread();
Application.Exit();
}
Application.ExitThread();
Application.Exit();
}
private void deal_Commad_long(byte[] newPacket, ref MAVLink myServer)
{
mavlink_command_long_t command = newPacket.ByteArrayToStructure<mavlink_command_long_t>(6);
Console.WriteLine(command.command);
switch (command.command)
{
case (int)MAV_CMD.MISSION_START:
Console.WriteLine("MISSION_START");
break;
case (int)MAV_CMD.COMPONENT_ARM_DISARM:
Console.WriteLine("ARM DISARM");
armed = armed == true ? false : true;
doARM(armed);
break;
case (int)MAV_CMD.CONDITION_CHANGE_ALT:
Console.WriteLine("change prof");
break;
case (int)MAV_CMD.CONDITION_YAW:
Console.WriteLine("change prof");
break;
case (int)MAV_CMD.DO_CHANGE_SPEED:
Console.WriteLine("change speed");
break;
case (int)MAV_CMD.DO_JUMP:
Console.WriteLine("jump");
break;
case (int)MAV_CMD.DO_SET_HOME:
Console.WriteLine("set home");
break;
case (int)MAV_CMD.DO_SET_MODE:
Console.WriteLine("set mode");
break;
case (int)MAV_CMD.TAKEOFF:
Console.WriteLine("submerge");
break;
case (int)MAV_CMD.WAYPOINT:
Console.WriteLine("waypoint");
break;
default:
break;
}
}
void startAPMsimulation()
{
try
{
TrimaresUDPServerData = new MainCrossData();
TrimaresUDPServerConn = new MavLinkConnection(tx_APcomport.Text, ref TrimaresUDPServerData);
// TrimaresUDPServerConn.mydata.setMavlinkPointer(ref TrimaresUDPMavLinkServer);
TrimaresUDPServerConn.connect();
TrimaresUDPServerConn.PacketReceived += TrimaresUDP_PacketReceived;
// TrimaresUDPServerConn.mydata.myLink.BaseStream.rMessage += showMessages;
TrimaresUDPServerConn.mydata.myLink.requestDatastream(MAVLink.MAV_DATA_STREAM.ALL, MainCrossData.cs.rateattitude);
}
catch
{
TrimaresUDPMavLinkServer = null;
TrimaresUDPServerData = null;
TrimaresUDPServerConn = null;
}
//TCPClientData = new MainCrossData();
//TCPClientConn = new MavLinkConnection(tx_APcomport.Text, ref TCPClientData);
//TCPClientConn.mydata.setMavlinkPointer(ref TCPMavLinkClient);
////TCPClientConn.mydata.myLink.BaseStream.Open();
//TCPClientConn.connect();
//TCPClientConn.mydata.myLink.BaseStream.rMessage += showMessages;
//TCPClientConn.PacketReceived += APSerialConn_PacketReceived;
//TCPClientData.myLink.requestDatastream(MAVLink.MAV_DATA_STREAM.ALL, MainCrossData.cs.rateattitude);
gb_robot.Enabled = false;
}
public bool translateMode(string modein, ref MAVLink.mavlink_set_mode_t mode)
{
mode.target_system = MAV.sysid;
try
{
List<KeyValuePair<int, string>> modelist = Common.getModesList(MAV.cs);
foreach (KeyValuePair<int, string> pair in modelist)
{
if (pair.Value.ToLower() == modein.ToLower())
{
mode.base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED;
mode.custom_mode = (uint)pair.Key;
}
}
if (mode.base_mode == 0)
{
MessageBox.Show("No Mode Changed " + modein);
return false;
}
}
catch { System.Windows.Forms.MessageBox.Show("Failed to find Mode"); return false; }
return true;
}
public void HandlePacket(byte[] newPacket, ref MAVLink myServer) //, ICommsSerial externalMavLink
{
// List<int> msg = new List<int> { 0 };
var newmsg = from n in pks
where n == Convert.ToInt16(newPacket[5])
select n;
if (newmsg.Count<int>() == 0)
{
// msg.Add(newPacket[5]);
Console.WriteLine(newPacket[5]);
}
switch (newPacket[5])
{
case 0:
{ //ARM - DISARM
//heartbeat
Console.WriteLine("==> HB ==>");
}
break;
case MAVLINK_MSG_ID_EXO_WATERQUALITY://248
{
// mavlink_exo_waterquality_t pacote = newPacket.ByteArrayToStructure<mavlink_exo_waterquality_t>(6);
}
break;
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: //43
{
mavlink_mission_request_list_t pacote = newPacket.ByteArrayToStructure<mavlink_mission_request_list_t>(6);
Console.WriteLine(" requisitando missao ");
}
break;
case MAVLINK_MSG_ID_MISSION_ITEM: //39
{
mavlink_mission_item_t pacote = newPacket.ByteArrayToStructure<mavlink_mission_item_t>(6);
Console.WriteLine(" ENVIANDO ITEM {0}: ({1},{2})",pacote.seq,pacote.x,pacote.y);
}
break;
case MAVLINK_MSG_ID_MISSION_REQUEST: // 40
{
mavlink_mission_request_t pacote = newPacket.ByteArrayToStructure<mavlink_mission_request_t>(6);
Console.WriteLine(" Requisitando item {0} da missao ", pacote.seq);
}
break;
case MAVLINK_MSG_ID_MISSION_COUNT: //44
{
mavlink_mission_count_t pacote = newPacket.ByteArrayToStructure<mavlink_mission_count_t>(6);
Console.WriteLine(" === resposta a pedido de missao : cliente possui = {0} itens de missao ", pacote.count);
}
break;
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: //66
{
mavlink_request_data_stream_t pacote = newPacket.ByteArrayToStructure<mavlink_request_data_stream_t>(6);
Console.WriteLine(" MAVLINK_MSG_ID_REQUEST_DATA_STREAM = {0}", pacote.req_stream_id);
}
break;
case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: //#70
{
mavlink_rc_channels_override_t pacote = newPacket.ByteArrayToStructure<mavlink_rc_channels_override_t>(6);
Console.WriteLine("RC para controle: {0},{1},{2},{3}",pacote.chan1_raw,pacote.chan2_raw,pacote.chan3_raw,pacote.chan4_raw);
}
break;
case MAVLINK_MSG_ID_COMMAND_LONG: // 76 DO COMMAND ARM - DISARM
{
mavlink_command_long_t pacote = newPacket.ByteArrayToStructure<mavlink_command_long_t>(6);
Console.WriteLine(" MAVLINK_MSG_ID_COMMAND_LONG = {0}",pacote.command);
deal_Commad_long(newPacket, ref myServer);
}
break;
case 77://ack
{
// externalMavLink.Write(newPacket,0,newPacket.Length);
Console.WriteLine(" === ACK RECEBIDO === ");
}
break;
case MAVLINK_MSG_ID_STATUSTEXT: //#253
{
//mavlink_statustext_t pacote = newPacket.ByteArrayToStructure<mavlink_statustext_t>(6);
//byte[] b = pacote.text;
//string x = b.ToString();
//Console.WriteLine(" seveiry {0}=> {1}", pacote.severity,x);
}
break;
default:
{
}
break;
}
}
void getDatastream(MAVLink.MAV_DATA_STREAM id, byte hzrate)
{
mavlink_request_data_stream_t req = new mavlink_request_data_stream_t();
req.target_system = sysid;
req.target_component = compid;
req.req_message_rate = hzrate;
req.start_stop = 1; // start
req.req_stream_id = (byte)id; // id
// send each one twice.
generatePacket(MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req);
generatePacket(MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req);
}
public void requestDatastream(MAVLink.MAV_DATA_STREAM id, byte hzrate)
{
double pps = 0;
switch (id)
{
case MAVLink.MAV_DATA_STREAM.ALL:
break;
case MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS:
if (packetspersecondbuild[MAVLINK_MSG_ID_SYS_STATUS] < DateTime.Now.AddSeconds(-2))
break;
pps = packetspersecond[MAVLINK_MSG_ID_SYS_STATUS];
if (hzratecheck(pps, hzrate))
{
return;
}
break;
case MAVLink.MAV_DATA_STREAM.EXTRA1:
if (packetspersecondbuild[MAVLINK_MSG_ID_ATTITUDE] < DateTime.Now.AddSeconds(-2))
break;
pps = packetspersecond[MAVLINK_MSG_ID_ATTITUDE];
if (hzratecheck(pps, hzrate))
{
return;
}
break;
case MAVLink.MAV_DATA_STREAM.EXTRA2:
if (packetspersecondbuild[MAVLINK_MSG_ID_VFR_HUD] < DateTime.Now.AddSeconds(-2))
break;
pps = packetspersecond[MAVLINK_MSG_ID_VFR_HUD];
if (hzratecheck(pps, hzrate))
{
return;
}
break;
case MAVLink.MAV_DATA_STREAM.EXTRA3:
if (packetspersecondbuild[MAVLINK_MSG_ID_AHRS] < DateTime.Now.AddSeconds(-2))
break;
pps = packetspersecond[MAVLINK_MSG_ID_AHRS];
if (hzratecheck(pps, hzrate))
{
return;
}
break;
case MAVLink.MAV_DATA_STREAM.POSITION:
if (packetspersecondbuild[MAVLINK_MSG_ID_GLOBAL_POSITION_INT] < DateTime.Now.AddSeconds(-2))
break;
pps = packetspersecond[MAVLINK_MSG_ID_GLOBAL_POSITION_INT];
if (hzratecheck(pps, hzrate))
{
return;
}
break;
case MAVLink.MAV_DATA_STREAM.RAW_CONTROLLER:
if (packetspersecondbuild[MAVLINK_MSG_ID_RC_CHANNELS_SCALED] < DateTime.Now.AddSeconds(-2))
break;
pps = packetspersecond[MAVLINK_MSG_ID_RC_CHANNELS_SCALED];
if (hzratecheck(pps, hzrate))
{
return;
}
break;
case MAVLink.MAV_DATA_STREAM.RAW_SENSORS:
if (packetspersecondbuild[MAVLINK_MSG_ID_RAW_IMU] < DateTime.Now.AddSeconds(-2))
break;
pps = packetspersecond[MAVLINK_MSG_ID_RAW_IMU];
if (hzratecheck(pps, hzrate))
{
return;
}
break;
case MAVLink.MAV_DATA_STREAM.RC_CHANNELS:
if (packetspersecondbuild[MAVLINK_MSG_ID_RC_CHANNELS_RAW] < DateTime.Now.AddSeconds(-2))
break;
pps = packetspersecond[MAVLINK_MSG_ID_RC_CHANNELS_RAW];
if (hzratecheck(pps, hzrate))
{
return;
}
break;
}
//packetspersecond[temp[5]];
if (pps == 0 && hzrate == 0)
{
return;
}
//log.InfoFormat("Request stream {0} at {1} hz", Enum.Parse(typeof(MAV_DATA_STREAM), id.ToString()), hzrate);
getDatastream(id, hzrate);
}
public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow, MAVLink mavinterface)
{
lock (locker)
{
if (DateTime.Now > lastupdate.AddMilliseconds(19) || updatenow) // 50 hz
{
lastupdate = DateTime.Now;
if (DateTime.Now.Second != lastsecondcounter.Second)
{
lastsecondcounter = DateTime.Now;
if (lastpos.Lat != 0 && lastpos.Lng != 0)
{
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, "");
}
// cant use gs, cant use alt,
if (ch3percent > 12)
timeInAir++;
if (!gotwind)
dowindcalc();
}
if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] != null) // status text
{
string logdata = DateTime.Now + " " + Encoding.ASCII.GetString(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT], 6, mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT].Length - 6);
int ind = logdata.IndexOf('\0');
if (ind != -1)
logdata = logdata.Substring(0, ind);
try
{
while (messages.Count > 5)
{
messages.RemoveAt(0);
}
messages.Add(logdata + "\n");
}
catch { }
mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] = null;
}
byte[] bytearray = mavinterface.packets[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;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] = null;
}
bytearray = mavinterface.packets[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[MAVLink.MAVLINK_MSG_ID_HIL_CONTROLS] = null;
}
bytearray = mavinterface.packets[MAVLink.MAVLINK_MSG_ID_HWSTATUS];
if (bytearray != null)
{
var hwstatus = bytearray.ByteArrayToStructure<MAVLink.mavlink_hwstatus_t>(6);
hwvoltage = hwstatus.Vcc;
i2cerrors = hwstatus.I2Cerr;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_HWSTATUS] = null;
}
bytearray = mavinterface.packets[MAVcomm.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;
//MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null;
}
#if MAVLINK10
bytearray = mavinterface.packets[MAVLink.MAVLINK_MSG_ID_HEARTBEAT];
if (bytearray != null)
{
var hb = bytearray.ByteArrayToStructure<MAVLink.mavlink_heartbeat_t>(6);
armed = (hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) == (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED ? 4 : 3;
string oldmode = mode;
mode = "Unknown";
if ((hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED) != 0)
{
if (hb.type == (byte)MAVLink.MAV_TYPE.FIXED_WING)
{
switch (hb.custom_mode)
{
case (byte)(Common.apmmodes.MANUAL):
mode = "Manual";
break;
case (byte)(Common.apmmodes.GUIDED):
mode = "Guided";
break;
case (byte)(Common.apmmodes.STABILIZE):
mode = "Stabilize";
break;
case (byte)(Common.apmmodes.FLY_BY_WIRE_A):
mode = "FBW A";
break;
case (byte)(Common.apmmodes.FLY_BY_WIRE_B):
mode = "FBW B";
break;
case (byte)(Common.apmmodes.AUTO):
mode = "Auto";
break;
case (byte)(Common.apmmodes.RTL):
mode = "RTL";
break;
case (byte)(Common.apmmodes.LOITER):
mode = "Loiter";
break;
case (byte)(Common.apmmodes.CIRCLE):
mode = "Circle";
break;
case 16:
mode = "Initialising";
break;
default:
mode = "Unknown";
break;
}
}
else if (hb.type == (byte)MAVLink.MAV_TYPE.QUADROTOR)
{
switch (hb.custom_mode)
{
case (byte)(Common.ac2modes.STABILIZE):
mode = "Stabilize";
break;
case (byte)(Common.ac2modes.ACRO):
mode = "Acro";
break;
case (byte)(Common.ac2modes.ALT_HOLD):
mode = "Alt Hold";
break;
case (byte)(Common.ac2modes.AUTO):
mode = "Auto";
break;
case (byte)(Common.ac2modes.GUIDED):
mode = "Guided";
break;
case (byte)(Common.ac2modes.LOITER):
mode = "Loiter";
break;
case (byte)(Common.ac2modes.RTL):
mode = "RTL";
break;
case (byte)(Common.ac2modes.CIRCLE):
mode = "Circle";
break;
case (byte)(Common.ac2modes.LAND):
mode = "Land";
break;
default:
mode = "Unknown";
break;
}
}
}
}
bytearray = mavinterface.packets[MAVcomm.MAVLink.MAVLINK_MSG_ID_SYS_STATUS];
if (bytearray != null)
{
var sysstatus = bytearray.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6);
battery_voltage = sysstatus.voltage_battery;
battery_remaining = sysstatus.battery_remaining;
current = sysstatus.current_battery;
packetdropremote = sysstatus.drop_rate_comm;
//MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null;
}
#endif
bytearray = mavinterface.packets[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 = mavinterface.packets[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 = mavinterface.packets[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;
}
#if MAVLINK10
bytearray = mavinterface.packets[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-7f;
lng = gps.lon * 1.0e-7f;
}
// 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[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null;
}
#endif
bytearray = mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_STATUS];
if (bytearray != null)
{
var gps = bytearray.ByteArrayToStructure<MAVLink.mavlink_gps_status_t>(6);
satcount = gps.satellites_visible;
}
bytearray = mavinterface.packets[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.fixedp;
}
bytearray = mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT];
if (bytearray != null)
{
var loc = bytearray.ByteArrayToStructure<MAVLink.mavlink_global_position_int_t>(6);
useLocation = true;
//alt = loc.alt / 1000.0f;
lat = loc.lat / 10000000.0f;
lng = loc.lon / 10000000.0f;
}
#if MAVLINK10
bytearray = mavinterface.packets[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;
//MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] = null;
}
#endif
bytearray = mavinterface.packets[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;
xtrack_error = nav.xtrack_error;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT] = null;
}
bytearray = mavinterface.packets[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;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] = null;
}
bytearray = mavinterface.packets[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;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW] = null;
}
bytearray = mavinterface.packets[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;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null;
}
bytearray = mavinterface.packets[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;
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null;
}
bytearray = mavinterface.packets[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;
if ((DateTime.Now - lastalt).TotalSeconds >= 0.2 && oldalt != alt)
{
climbrate = (alt - oldalt) / (float)(DateTime.Now - lastalt).TotalSeconds;
verticalspeed = (alt - oldalt) / (float)(DateTime.Now - lastalt).TotalSeconds;
if (float.IsInfinity(_verticalspeed))
_verticalspeed = 0;
lastalt = DateTime.Now;
oldalt = alt;
}
//MAVLink.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD] = null;
}
bytearray = mavinterface.packets[MAVLink.MAVLINK_MSG_ID_MEMINFO];
if (bytearray != null)
{
var mem = bytearray.ByteArrayToStructure<MAVLink.mavlink_meminfo_t>(6);
freemem = mem.freemem;
brklevel = mem.brkval;
}
}
//Console.WriteLine(DateTime.Now.Millisecond + " start ");
// update form
try
{
if (bs != null)
{
//System.Diagnostics.Debug.WriteLine(DateTime.Now.Millisecond);
//Console.WriteLine(DateTime.Now.Millisecond);
bs.DataSource = this;
// Console.WriteLine(DateTime.Now.Millisecond + " 1 " + updatenow + " " + System.Threading.Thread.CurrentThread.Name);
bs.ResetBindings(false);
//Console.WriteLine(DateTime.Now.Millisecond + " done ");
}
}
catch { //log.InfoFormat("CurrentState Binding error");
}
}
}
/*
public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow)
{
UpdateCurrentSettings(bs, false, MainCrossData.comPort);
}
*/
public object ReturnMavType(byte mavType, ref MAVLink myLink)
{
switch (mavType)
{
case (byte)(MAVLink.MAVLINK_MSG_ID_REQUEST_DATA_STREAM):
var msgid = new MAVLink.mavlink_request_data_stream_t();
msgid.target_system = myLink.sysid;
msgid.target_component = myLink.compid;
msgid.req_message_rate = ratestatus;
msgid.start_stop = 1;
msgid.req_stream_id = 0;
return msgid;
case (byte)(MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED):
var hil = new MAVLink.mavlink_rc_channels_scaled_t();
hil.chan1_scaled = (short)hilch1;
hil.chan2_scaled = (short)hilch2;
hil.chan3_scaled = (short)hilch3;
hil.chan4_scaled = (short)hilch4;
hil.chan5_scaled = (short)hilch5;
hil.chan6_scaled = (short)hilch6;
hil.chan7_scaled = (short)hilch7;
hil.chan8_scaled = (short)hilch8;
return hil;
case (byte)(MAVLink.MAVLINK_MSG_ID_HIL_CONTROLS):
var hil2 = new MAVLink.mavlink_hil_controls_t();
hil2.roll_ailerons = (float)((float)hilch1 / 10000.0);
hil2.pitch_elevator = (float)((float)hilch2 / 10000.0);
hil2.throttle = (float)((float)hilch3 / 10000.0);
hil2.yaw_rudder = (float)((float)hilch4 / 10000.0);
return hil2;
case (byte)(MAVLink.MAVLINK_MSG_ID_HWSTATUS):
var hwstatus = new MAVLink.mavlink_hwstatus_t();
hwstatus.Vcc = hwvoltage;
hwstatus.I2Cerr = (byte)i2cerrors;
return hwstatus;
}
//myData.myLink.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.POSITION, MainCrossData.cs.rateposition); // request gps
//myData.myLink.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.EXTRA1, MainCrossData.cs.rateattitude); // request attitude
//myData.myLink.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.EXTRA2, MainCrossData.cs.rateattitude); // request vfr
//myData.myLink.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.EXTRA3, MainCrossData.cs.ratesensors); // request extra stuff - tridge
//myData.myLink.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainCrossData.cs.ratesensors); // request raw sensor
//myData.myLink.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.RC_CHANNELS, MainCrossData.cs.raterc); // request rc info
//myData.myLink.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS, MainCrossData.cs.ratestatus); // mode
// bytearray = mavinterface.packets[ArdupilotMega.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;
// //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null;
// }
//#if MAVLINK10
// bytearray = mavinterface.packets[MAVLink.MAVLINK_MSG_ID_HEARTBEAT];
// if (bytearray != null)
// {
// var hb = bytearray.ByteArrayToStructure<MAVLink.mavlink_heartbeat_t>(6);
// armed = (hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED) == (byte)MAVLink.MAV_MODE_FLAG.SAFETY_ARMED ? 4 : 3;
// string oldmode = mode;
// mode = "Unknown";
// if ((hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED) != 0)
// {
//
// else if (hb.type == (byte)MAVLink.MAV_TYPE.QUADROTOR)
// {
// switch (hb.custom_mode)
// {
// case (byte)(Common.ac2modes.STABILIZE):
// mode = "Stabilize";
// break;
// case (byte)(Common.ac2modes.ACRO):
// mode = "Acro";
// break;
// case (byte)(Common.ac2modes.ALT_HOLD):
// mode = "Alt Hold";
// break;
// case (byte)(Common.ac2modes.AUTO):
// mode = "Auto";
// break;
// case (byte)(Common.ac2modes.GUIDED):
// mode = "Guided";
// break;
// case (byte)(Common.ac2modes.LOITER):
// mode = "Loiter";
// break;
// case (byte)(Common.ac2modes.RTL):
// mode = "RTL";
// break;
// case (byte)(Common.ac2modes.CIRCLE):
// mode = "Circle";
// break;
// case (byte)(Common.ac2modes.LAND):
// mode = "Land";
// break;
// default:
// mode = "Unknown";
// break;
// }
// }
// }
// }
// bytearray = mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS];
// if (bytearray != null)
// {
// var sysstatus = bytearray.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6);
// battery_voltage = sysstatus.voltage_battery;
// battery_remaining = sysstatus.battery_remaining;
// current = sysstatus.current_battery;
// packetdropremote = sysstatus.drop_rate_comm;
// //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null;
// }
//#endif
// bytearray = mavinterface.packets[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 = mavinterface.packets[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 = mavinterface.packets[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;
// }
//#if MAVLINK10
// bytearray = mavinterface.packets[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-7f;
// lng = gps.lon * 1.0e-7f;
// }
// // 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[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null;
// }
//#endif
// bytearray = mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_STATUS];
// if (bytearray != null)
// {
// var gps = bytearray.ByteArrayToStructure<MAVLink.mavlink_gps_status_t>(6);
// satcount = gps.satellites_visible;
// }
// bytearray = mavinterface.packets[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.fixedp;
// }
// bytearray = mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT];
// if (bytearray != null)
// {
// var loc = bytearray.ByteArrayToStructure<MAVLink.mavlink_global_position_int_t>(6);
// useLocation = true;
// //alt = loc.alt / 1000.0f;
// lat = loc.lat / 10000000.0f;
// lng = loc.lon / 10000000.0f;
// }
//#if MAVLINK10
// bytearray = mavinterface.packets[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;
// //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] = null;
// }
//#endif
// bytearray = mavinterface.packets[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;
// xtrack_error = nav.xtrack_error;
// //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT] = null;
// }
// bytearray = mavinterface.packets[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;
// //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] = null;
// }
// bytearray = mavinterface.packets[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;
// //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW] = null;
// }
// bytearray = mavinterface.packets[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;
// //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null;
// }
// bytearray = mavinterface.packets[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;
// //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] = null;
// }
// bytearray = mavinterface.packets[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;
// if ((DateTime.Now - lastalt).TotalSeconds >= 0.2 && oldalt != alt)
// {
// climbrate = (alt - oldalt) / (float)(DateTime.Now - lastalt).TotalSeconds;
// verticalspeed = (alt - oldalt) / (float)(DateTime.Now - lastalt).TotalSeconds;
// if (float.IsInfinity(_verticalspeed))
// _verticalspeed = 0;
// lastalt = DateTime.Now;
// oldalt = alt;
// }
// //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD] = null;
// }
// bytearray = mavinterface.packets[MAVLink.MAVLINK_MSG_ID_MEMINFO];
// if (bytearray != null)
// {
// var mem = bytearray.ByteArrayToStructure<MAVLink.mavlink_meminfo_t>(6);
// freemem = mem.freemem;
// brklevel = mem.brkval;
// }
// }
return null;
}
public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, ref MAVLink myLink)
{
UpdateCurrentSettings(bs, false, myLink);
}