/* * public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow) * { * UpdateCurrentSettings(bs, false, MainV2.comPort); * } */ public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow, MAVLink mavinterface) { lock (locker) { if (DateTime.Now > lastupdate.AddMilliseconds(50) || updatenow) // 20 hz { lastupdate = DateTime.Now; //check if valid mavinterface if (mavinterface != null && mavinterface.packetsnotlost != 0) { linkqualitygcs = (ushort)((mavinterface.packetsnotlost / (mavinterface.packetsnotlost + mavinterface.packetslost)) * 100.0); } if (DateTime.Now.Second != lastsecondcounter.Second) { lastsecondcounter = DateTime.Now; if (lastpos.Lat != 0 && lastpos.Lng != 0) { if (!MainV2.comPort.BaseStream.IsOpen && !MainV2.comPort.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(); } } if (mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] != null) // status text { var msg = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT].ByteArrayToStructure <MAVLink.mavlink_statustext_t>(6); /* * enum gcs_severity { * SEVERITY_LOW=1, * SEVERITY_MEDIUM, * SEVERITY_HIGH, * SEVERITY_CRITICAL * }; */ byte sev = msg.severity; string logdata = Encoding.ASCII.GetString(mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT], 6, mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT].Length - 6); int ind = logdata.IndexOf('\0'); if (ind != -1) { logdata = logdata.Substring(0, ind); } if (sev >= 3) { messageHigh = logdata; } try { while (messages.Count > 50) { messages.RemoveAt(0); } messages.Add(logdata + "\n"); } catch { } mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] = null; } byte[] bytearray = mavinterface.MAV.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; // Console.WriteLine("MAVLINK_MSG_ID_RC_CHANNELS_SCALED Packet"); mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] = null; } bytearray = mavinterface.MAV.packets[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"; } mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_FENCE_STATUS] = null; } bytearray = mavinterface.MAV.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.MAV.packets[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[MAVLink.MAVLINK_MSG_ID_HWSTATUS] = null; } bytearray = mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_RANGEFINDER]; if (bytearray != null) { var sonar = bytearray.ByteArrayToStructure <MAVLink.mavlink_rangefinder_t>(6); sonarrange = sonar.distance; sonarvoltage = sonar.voltage; } bytearray = mavinterface.MAV.packets[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; //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null; } bytearray = mavinterface.MAV.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; 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.getConfig("speechmodeenabled") == "True") { MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode"))); } } bytearray = mavinterface.MAV.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS]; if (bytearray != null) { var sysstatus = bytearray.ByteArrayToStructure <MAVLink.mavlink_sys_status_t>(6); battery_voltage = (float)sysstatus.voltage_battery / 1000.0f; battery_remaining = (float)sysstatus.battery_remaining; current = (float)sysstatus.current_battery / 100.0f; packetdropremote = sysstatus.drop_rate_comm; //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null; } bytearray = mavinterface.MAV.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.MAV.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.MAV.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; // Console.WriteLine(roll + " " + pitch + " " + yaw); //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE] = null; } bytearray = mavinterface.MAV.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 } altasl = gps.alt / 1000.0f; 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; } bytearray = mavinterface.MAV.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.MAV.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.MAV.packets[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.0f; lng = loc.lon / 10000000.0f; } } bytearray = mavinterface.MAV.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; if (oldwp != wpno && MainV2.speechEnable && MainV2.getConfig("speechwaypointenabled") == "True") { MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint"))); } //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] = null; } bytearray = mavinterface.MAV.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 / 100.0f; xtrack_error = nav.xtrack_error; //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT] = null; } bytearray = mavinterface.MAV.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; //percent rxrssi = (float)((rcin.rssi / 255.0) * 100.0); //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] = null; } bytearray = mavinterface.MAV.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.MAV.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.MAV.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.MAV.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; //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD] = null; } bytearray = mavinterface.MAV.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.Write(DateTime.Now.Millisecond + " start "); // update form try { if (bs != null) { //System.Diagnostics.Debug.WriteLine(DateTime.Now.Millisecond); // Console.Write(" "+DateTime.Now.Millisecond); bs.DataSource = this; // Console.Write(" " + DateTime.Now.Millisecond + " 1 " + updatenow + " " + System.Threading.Thread.CurrentThread.Name); bs.ResetBindings(false); //bs.ResetCurrentItem(); // mono workaround - this is alot faster //bs.Clear(); //bs.Add(this); // 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, MainV2.comPort); * } */ 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 (!MainV2.comPort.BaseStream.IsOpen && !MainV2.comPort.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[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; 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; } } } if (oldmode != mode && MainV2.speechEnable && MainV2.getConfig("speechmodeenabled") == "True") { MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode"))); } } 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; } #else bytearray = mavinterface.packets[MAVLink.MAVLINK_MSG_ID_SYS_STATUS]; if (bytearray != null) { var sysstatus = bytearray.ByteArrayToStructure <MAVLink.mavlink_sys_status_t>(6); armed = sysstatus.status; string oldmode = mode; switch (sysstatus.mode) { case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_UNINIT: switch (sysstatus.nav_mode) { case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_GROUNDED: mode = "Initialising"; break; } break; case (byte)(100 + Common.ac2modes.STABILIZE): mode = EnumTranslator.GetDisplayText(Common.ac2modes.STABILIZE); break; case (byte)(100 + Common.ac2modes.ACRO): mode = EnumTranslator.GetDisplayText(Common.ac2modes.ACRO); break; case (byte)(100 + Common.ac2modes.ALT_HOLD): mode = EnumTranslator.GetDisplayText(Common.ac2modes.ALT_HOLD); break; case (byte)(100 + Common.ac2modes.AUTO): mode = EnumTranslator.GetDisplayText(Common.ac2modes.AUTO); break; case (byte)(100 + Common.ac2modes.GUIDED): mode = EnumTranslator.GetDisplayText(Common.ac2modes.GUIDED); break; case (byte)(100 + Common.ac2modes.LOITER): mode = EnumTranslator.GetDisplayText(Common.ac2modes.LOITER); break; case (byte)(100 + Common.ac2modes.RTL): mode = EnumTranslator.GetDisplayText(Common.ac2modes.RTL); break; case (byte)(100 + Common.ac2modes.CIRCLE): mode = EnumTranslator.GetDisplayText(Common.ac2modes.CIRCLE); break; case (byte)(100 + Common.ac2modes.LAND): mode = EnumTranslator.GetDisplayText(Common.ac2modes.LAND); break; case (byte)(100 + Common.ac2modes.POSITION): mode = EnumTranslator.GetDisplayText(Common.ac2modes.POSITION); break; case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_MANUAL: mode = "Manual"; break; case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_GUIDED: mode = "Guided"; break; case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST1: mode = "Stabilize"; break; case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST2: mode = "FBW A"; // fall though old switch (sysstatus.nav_mode) { case (byte)1: mode = "FBW A"; break; case (byte)2: mode = "FBW B"; break; case (byte)3: mode = "FBW C"; break; } break; case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST3: mode = "Circle"; break; case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_AUTO: switch (sysstatus.nav_mode) { case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_WAYPOINT: mode = "Auto"; break; case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_RETURNING: mode = "RTL"; break; case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_HOLD: case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LOITER: mode = "Loiter"; break; case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LIFTOFF: mode = "Takeoff"; break; case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LANDING: mode = "Land"; break; } break; default: mode = "Unknown"; break; } battery_voltage = sysstatus.vbat; battery_remaining = sysstatus.battery_remaining; packetdropremote = sysstatus.packet_drop; if (oldmode != mode && MainV2.speechEnable && MainV2.speechEngine != null && MainV2.getConfig("speechmodeenabled") == "True") { MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode"))); } //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; // Console.WriteLine(roll + " " + pitch + " " + yaw); //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE] = null; } 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; } 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); // the new arhs deadreckoning may send 0 alt and 0 long. check for and undo useLocation = true; if (loc.lat == 0 && loc.lon == 0) { useLocation = false; } else { //alt = loc.alt / 1000.0f; lat = loc.lat / 10000000.0f; lng = loc.lon / 10000000.0f; } } 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; if (oldwp != wpno && MainV2.speechEnable && MainV2.getConfig("speechwaypointenabled") == "True") { MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint"))); } //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] = null; } 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 / 100.0f; 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; //percent rxrssi = (float)((rcin.rssi / 255.0) * 100.0); //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, MainV2.comPort); * } */ public void UpdateCurrentSettings(System.Windows.Forms.BindingSource bs, bool updatenow, MAVLink mavinterface) { if (DateTime.Now > lastupdate.AddMilliseconds(19) || updatenow) // 50 hz { lastupdate = DateTime.Now; if (DateTime.Now.Second != lastwindcalc.Second) { lastwindcalc = DateTime.Now; 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); } if (messages.Count > 5) { messages.RemoveAt(0); } messages.Add(logdata + "\n"); mavinterface.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] = null; } if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED] != null) // hil { var hil = new ArdupilotMega.MAVLink.__mavlink_rc_channels_scaled_t(); object temp = hil; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_SCALED], ref temp, 6); hil = (MAVLink.__mavlink_rc_channels_scaled_t)(temp); 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; } if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT] != null) { MAVLink.__mavlink_nav_controller_output_t nav = new MAVLink.__mavlink_nav_controller_output_t(); object temp = nav; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT], ref temp, 6); nav = (MAVLink.__mavlink_nav_controller_output_t)(temp); 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; } #if MAVLINK10 if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_HEARTBEAT] != null) { ArdupilotMega.MAVLink.__mavlink_heartbeat_t hb = new ArdupilotMega.MAVLink.__mavlink_heartbeat_t(); object temp = hb; MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_HEARTBEAT], ref temp, 6); hb = (ArdupilotMega.MAVLink.__mavlink_heartbeat_t)(temp); string oldmode = mode; mode = "Unknown"; if ((hb.base_mode & (byte)MAVLink.MAV_MODE_FLAG.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) != 0) { if (hb.type == (byte)MAVLink.MAV_TYPE.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; default: mode = "Unknown"; break; } } else if (hb.type == (byte)MAVLink.MAV_TYPE.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; default: mode = "Unknown"; break; } } } if (oldmode != mode && MainV2.speechenable && MainV2.getConfig("speechmodeenabled") == "True") { MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode"))); } } if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] != null) { ArdupilotMega.MAVLink.__mavlink_sys_status_t sysstatus = new ArdupilotMega.MAVLink.__mavlink_sys_status_t(); object temp = sysstatus; MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS], ref temp, 6); sysstatus = (ArdupilotMega.MAVLink.__mavlink_sys_status_t)(temp); battery_voltage = sysstatus.voltage_battery; battery_remaining = sysstatus.battery_remaining; packetdropremote = sysstatus.drop_rate_comm; //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null; } #else if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] != null) { ArdupilotMega.MAVLink.__mavlink_sys_status_t sysstatus = new ArdupilotMega.MAVLink.__mavlink_sys_status_t(); object temp = sysstatus; MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS], ref temp, 6); sysstatus = (ArdupilotMega.MAVLink.__mavlink_sys_status_t)(temp); string oldmode = mode; switch (sysstatus.mode) { case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_UNINIT: switch (sysstatus.nav_mode) { case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_GROUNDED: mode = "Initialising"; break; } break; case (byte)(100 + Common.ac2modes.STABILIZE): mode = "Stabilize"; break; case (byte)(100 + Common.ac2modes.ACRO): mode = "Acro"; break; case (byte)(100 + Common.ac2modes.ALT_HOLD): mode = "Alt Hold"; break; case (byte)(100 + Common.ac2modes.AUTO): mode = "Auto"; break; case (byte)(100 + Common.ac2modes.GUIDED): mode = "Guided"; break; case (byte)(100 + Common.ac2modes.LOITER): mode = "Loiter"; break; case (byte)(100 + Common.ac2modes.RTL): mode = "RTL"; break; case (byte)(100 + Common.ac2modes.CIRCLE): mode = "Circle"; break; case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_MANUAL: mode = "Manual"; break; case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_GUIDED: mode = "Guided"; break; case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST1: mode = "Stabilize"; break; case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST2: mode = "FBW A"; // fall though old switch (sysstatus.nav_mode) { case (byte)1: mode = "FBW A"; break; case (byte)2: mode = "FBW B"; break; case (byte)3: mode = "FBW C"; break; } break; case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_TEST3: mode = "Circle"; break; case (byte)ArdupilotMega.MAVLink.MAV_MODE.MAV_MODE_AUTO: switch (sysstatus.nav_mode) { case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_WAYPOINT: mode = "Auto"; break; case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_RETURNING: mode = "RTL"; break; case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_HOLD: case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LOITER: mode = "Loiter"; break; case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LIFTOFF: mode = "Takeoff"; break; case (byte)ArdupilotMega.MAVLink.MAV_NAV.MAV_NAV_LANDING: mode = "Land"; break; } break; default: mode = "Unknown"; break; } battery_voltage = sysstatus.vbat; battery_remaining = sysstatus.battery_remaining; packetdropremote = sysstatus.packet_drop; if (oldmode != mode && MainV2.speechenable && MainV2.getConfig("speechmodeenabled") == "True") { MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode"))); } //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null; } #endif if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE] != null) { var att = new ArdupilotMega.MAVLink.__mavlink_attitude_t(); object temp = att; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE], ref temp, 6); att = (MAVLink.__mavlink_attitude_t)(temp); roll = att.roll * rad2deg; pitch = att.pitch * rad2deg; yaw = att.yaw * rad2deg; // Console.WriteLine(roll + " " + pitch + " " + yaw); //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_ATTITUDE] = null; } #if MAVLINK10 if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT] != null) { var gps = new MAVLink.__mavlink_gps_raw_int_t(); object temp = gps; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW_INT], ref temp, 6); gps = (MAVLink.__mavlink_gps_raw_int_t)(temp); 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 = gps.eph; groundspeed = gps.vel * 1.0e-2f; groundcourse = gps.cog * 1.0e-2f; //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null; } #else if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] != null) { var gps = new MAVLink.__mavlink_gps_raw_t(); object temp = gps; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW], ref temp, 6); gps = (MAVLink.__mavlink_gps_raw_t)(temp); lat = gps.lat; lng = gps.lon; // alt = gps.alt; // using vfr as includes baro calc gpsstatus = gps.fix_type; // Console.WriteLine("gpsfix {0}",gpsstatus); gpshdop = gps.eph; groundspeed = gps.v; groundcourse = gps.hdg; //MAVLink.packets[MAVLink.MAVLINK_MSG_ID_GPS_RAW] = null; } #endif if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_STATUS] != null) { var gps = new MAVLink.__mavlink_gps_status_t(); object temp = gps; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GPS_STATUS], ref temp, 6); gps = (MAVLink.__mavlink_gps_status_t)(temp); satcount = gps.satellites_visible; } if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT] != null) { var loc = new MAVLink.__mavlink_global_position_int_t(); object temp = loc; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT], ref temp, 6); loc = (MAVLink.__mavlink_global_position_int_t)(temp); //alt = loc.alt / 1000.0f; lat = loc.lat / 10000000.0f; lng = loc.lon / 10000000.0f; } #if MAVLINK10 if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT] != null) { ArdupilotMega.MAVLink.__mavlink_mission_current_t wpcur = new ArdupilotMega.MAVLink.__mavlink_mission_current_t(); object temp = wpcur; MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_MISSION_CURRENT], ref temp, 6); wpcur = (ArdupilotMega.MAVLink.__mavlink_mission_current_t)(temp); int oldwp = (int)wpno; wpno = wpcur.seq; if (oldwp != wpno && MainV2.speechenable && MainV2.getConfig("speechwaypointenabled") == "True") { MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint"))); } //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] = null; } #else if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION] != null) { var loc = new MAVLink.__mavlink_global_position_t(); object temp = loc; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_GLOBAL_POSITION], ref temp, 6); loc = (MAVLink.__mavlink_global_position_t)(temp); alt = loc.alt; lat = loc.lat; lng = loc.lon; } if (mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] != null) { ArdupilotMega.MAVLink.__mavlink_waypoint_current_t wpcur = new ArdupilotMega.MAVLink.__mavlink_waypoint_current_t(); object temp = wpcur; MAVLink.ByteArrayToStructure(mavinterface.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT], ref temp, 6); wpcur = (ArdupilotMega.MAVLink.__mavlink_waypoint_current_t)(temp); int oldwp = (int)wpno; wpno = wpcur.seq; if (oldwp != wpno && MainV2.speechenable && MainV2.getConfig("speechwaypointenabled") == "True") { MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint"))); } //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] = null; } #endif if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW] != null) { var rcin = new MAVLink.__mavlink_rc_channels_raw_t(); object temp = rcin; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_RAW], ref temp, 6); rcin = (MAVLink.__mavlink_rc_channels_raw_t)(temp); 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; } if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW] != null) { var servoout = new MAVLink.__mavlink_servo_output_raw_t(); object temp = servoout; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_SERVO_OUTPUT_RAW], ref temp, 6); servoout = (MAVLink.__mavlink_servo_output_raw_t)(temp); 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; } if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU] != null) { var imu = new MAVLink.__mavlink_raw_imu_t(); object temp = imu; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_RAW_IMU], ref temp, 6); imu = (MAVLink.__mavlink_raw_imu_t)(temp); 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; } if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_SCALED_IMU] != null) { var imu = new MAVLink.__mavlink_scaled_imu_t(); object temp = imu; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_SCALED_IMU], ref temp, 6); imu = (MAVLink.__mavlink_scaled_imu_t)(temp); 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; } if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD] != null) { MAVLink.__mavlink_vfr_hud_t vfr = new MAVLink.__mavlink_vfr_hud_t(); object temp = vfr; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_VFR_HUD], ref temp, 6); vfr = (MAVLink.__mavlink_vfr_hud_t)(temp); groundspeed = vfr.groundspeed; airspeed = vfr.airspeed; alt = vfr.alt; // this might include baro //climbrate = vfr.climb; if ((DateTime.Now - lastalt).TotalSeconds >= 0.1 && 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; } if (mavinterface.packets[MAVLink.MAVLINK_MSG_ID_MEMINFO] != null) // hil { var mem = new ArdupilotMega.MAVLink.__mavlink_meminfo_t(); object temp = mem; MAVLink.ByteArrayToStructure(mavinterface.packets[MAVLink.MAVLINK_MSG_ID_MEMINFO], ref temp, 6); mem = (MAVLink.__mavlink_meminfo_t)(temp); freemem = mem.freemem; brklevel = mem.brkval; } } //Console.WriteLine(DateTime.Now.Millisecond + " start "); // update form try { if (bs != null) { //Console.WriteLine(DateTime.Now.Millisecond); bs.DataSource = this; //Console.WriteLine(DateTime.Now.Millisecond + " 1 " + updatenow); bs.ResetBindings(false); //Console.WriteLine(DateTime.Now.Millisecond + " done "); } } catch { Console.WriteLine("CurrentState Binding error"); } }