static void Main() { Application.EnableVisualStyles(); XmlConfigurator.Configure(); log.Info("******************* Logging Configured *******************"); Application.SetCompatibleTextRenderingDefault(false); Application.ThreadException += Application_ThreadException; Application.Idle += Application_Idle; int wt = 0, ct = 0; ThreadPool.GetMaxThreads(out wt, out ct); log.Info("Max Threads: " + wt); //MagCalib.ProcessLog(); //MessageBox.Show("NOTE: This version may break advanced mission scripting"); //Common.linearRegression(); //Console.WriteLine(srtm.getAltitude(-35.115676879882812, 117.94178754638671,20)); PointLatLngAlt plla = new PointLatLngAlt(54.0359, 5.4253, 0, ""); PointLatLngAlt plla2 = new PointLatLngAlt(54.3838, 3.0412, 0, ""); Console.WriteLine(plla.GetDistance(plla2)); Console.WriteLine(plla.GetDistance2(plla2)); if (System.Diagnostics.Debugger.IsAttached) { // testing // Utilities.ParameterMetaDataParser.GetParameterInformation(); } try { Thread.CurrentThread.Name = "Base Thread"; Application.Run(new MainV2()); } catch (Exception ex) { log.Fatal("Fatal app exception",ex); Console.WriteLine(ex.ToString()); Console.ReadLine(); } }
void mainloop() { DateTime nextsend = DateTime.Now; threadrun = true; while (threadrun) { try { string line = comPort.ReadLine(); //string line = string.Format("$GP{0},{1:HHmmss},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},", "GGA", DateTime.Now.ToUniversalTime(), Math.Abs(lat * 100), MainV2.comPort.MAV.cs.lat < 0 ? "S" : "N", Math.Abs(lng * 100), MainV2.comPort.MAV.cs.lng < 0 ? "W" : "E", MainV2.comPort.MAV.cs.gpsstatus, MainV2.comPort.MAV.cs.satcount, MainV2.comPort.MAV.cs.gpshdop, MainV2.comPort.MAV.cs.alt, "M", 0, "M", ""); if (line.StartsWith("$GPGGA")) // { string[] items = line.Trim().Split(',','*'); if (items[15] != GetChecksum(line.Trim())) { Console.WriteLine("Bad Nmea line " + items[15] + " vs " + GetChecksum(line.Trim())); continue; } if (items[6] == "0") { Console.WriteLine("No Fix"); continue; } gotolocation.Lat = double.Parse(items[2], CultureInfo.InvariantCulture) / 100.0; gotolocation.Lat = (int)gotolocation.Lat + ((gotolocation.Lat - (int)gotolocation.Lat) / 0.60); if (items[3] == "S") gotolocation.Lat *= -1; gotolocation.Lng = double.Parse(items[4], CultureInfo.InvariantCulture) / 100.0; gotolocation.Lng = (int)gotolocation.Lng + ((gotolocation.Lng - (int)gotolocation.Lng) / 0.60); if (items[5] == "W") gotolocation.Lng *= -1; gotolocation.Alt = intalt; // double.Parse(line.Substring(c9, c10 - c9 - 1)) + gotolocation.Tag = "Sats "+ items[7] + " hdop " + items[8] ; } if (DateTime.Now > nextsend && gotolocation.Lat != 0 && gotolocation.Lng != 0 && gotolocation.Alt != 0) // 200 * 10 = 2 sec /// lastgotolocation != gotolocation && { nextsend = DateTime.Now.AddSeconds(2); Console.WriteLine("Sending follow wp " +DateTime.Now.ToString("h:MM:ss")+" "+ gotolocation.Lat + " " + gotolocation.Lng + " " +gotolocation.Alt); lastgotolocation = new PointLatLngAlt(gotolocation); Locationwp gotohere = new Locationwp(); gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT; gotohere.alt = (float)(gotolocation.Alt); gotohere.lat = (gotolocation.Lat); gotohere.lng = (gotolocation.Lng); try { updateLocationLabel(gotohere); } catch { } if (MainV2.comPort.BaseStream.IsOpen && MainV2.comPort.giveComport == false) { try { MainV2.comPort.giveComport = true; MainV2.comPort.setGuidedModeWP(gotohere); MainV2.comPort.giveComport = false; } catch { MainV2.comPort.giveComport = false; } } } } catch { System.Threading.Thread.Sleep(2000); } } }
/// <summary> /// Calc Distance in M /// </summary> /// <param name="p2"></param> /// <returns>Distance in M</returns> public double GetDistance(PointLatLngAlt p2) { double d = Lat * 0.017453292519943295; double num2 = Lng * 0.017453292519943295; double num3 = p2.Lat * 0.017453292519943295; double num4 = p2.Lng * 0.017453292519943295; double num5 = num4 - num2; double num6 = num3 - d; double num7 = Math.Pow(Math.Sin(num6 / 2.0), 2.0) + ((Math.Cos(d) * Math.Cos(num3)) * Math.Pow(Math.Sin(num5 / 2.0), 2.0)); double num8 = 2.0 * Math.Atan2(Math.Sqrt(num7), Math.Sqrt(1.0 - num7)); return (6371 * num8) * 1000.0; // M }
public double GetDistance2(PointLatLngAlt p2) { //http://www.movable-type.co.uk/scripts/latlong.html var R = 6371.0; // 6371 km var dLat = (p2.Lat - Lat) * deg2rad; var dLon = (p2.Lng - Lng) * deg2rad; var lat1 = Lat * deg2rad; var lat2 = p2.Lat * deg2rad; var a = Math.Sin(dLat / 2.0) * Math.Sin(dLat / 2.0) + Math.Sin(dLon / 2.0) * Math.Sin(dLon / 2.0) * Math.Cos(lat1) * Math.Cos(lat2); var c = 2.0 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1.0 - a)); var d = R * c * 1000.0; // M return d; }
public PointLatLngAlt getFencePoint(int no, ref int total) { byte[] buffer; giveComport = true; PointLatLngAlt plla = new PointLatLngAlt(); mavlink_fence_fetch_point_t req = new mavlink_fence_fetch_point_t(); req.idx = (byte)no; req.target_component = compid; req.target_system = sysid; // request point generatePacket(MAVLINK_MSG_ID_FENCE_FETCH_POINT, req); DateTime start = DateTime.Now; int retrys = 3; while (true) { if (!(start.AddMilliseconds(500) > DateTime.Now)) { if (retrys > 0) { log.Info("getFencePoint Retry " + retrys + " - giv com " + giveComport); generatePacket(MAVLINK_MSG_ID_FENCE_FETCH_POINT, req); start = DateTime.Now; retrys--; continue; } giveComport = false; throw new Exception("Timeout on read - getFencePoint"); } buffer = readPacket(); if (buffer.Length > 5) { if (buffer[5] == MAVLINK_MSG_ID_FENCE_POINT) { giveComport = false; mavlink_fence_point_t fp = buffer.ByteArrayToStructure<mavlink_fence_point_t>(6); plla.Lat = fp.lat; plla.Lng = fp.lng; plla.Tag = fp.idx.ToString(); total = fp.count; return plla; } } } }
public PointLatLngAlt(PointLatLngAlt plla) { this.Lat = plla.Lat; this.Lng = plla.Lng; this.Alt = plla.Alt; this.color = plla.color; this.Tag = plla.Tag; }
/* 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, ""); } // cant use gs, cant use alt, if (ch3percent > 12) timeInAir++; if (!gotwind) dowindcalc(); } if (mavinterface.MAV.packets[MAVLink.MAVLINK_MSG_ID_STATUSTEXT] != null) // status text { 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); 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_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_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; } #if MAVLINK10 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; 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.TRAINING): mode = "Training"; 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 (byte)(Common.apmmodes.TAKEOFF): mode = "Takeoff"; 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; case (byte)Common.ac2modes.OF_LOITER: mode = "OF Loiter"; break; default: mode = "Unknown"; break; } } } 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 = 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.MAV.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.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 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 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.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; // check update rate, and ensure time hasnt gone backwards if ((datetime - lastalt).TotalSeconds >= 0.2 && oldalt != alt || lastalt > datetime) { climbrate = (alt - oldalt) / (float)(datetime - lastalt).TotalSeconds; verticalspeed = (alt - oldalt) / (float)(datetime - lastalt).TotalSeconds; if (float.IsInfinity(_verticalspeed)) _verticalspeed = 0; lastalt = datetime; oldalt = alt; } //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.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(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"); } } }
List <PointLatLngAlt> getGEAltPath(List <PointLatLngAlt> list) { double alt = 0; double lat = 0; double lng = 0; int pos = 0; List <PointLatLngAlt> answer = new List <PointLatLngAlt>(); //http://code.google.com/apis/maps/documentation/elevation/ //http://maps.google.com/maps/api/elevation/xml string coords = ""; foreach (PointLatLngAlt loc in list) { coords = coords + loc.Lat.ToString(new System.Globalization.CultureInfo("en-US")) + "," + loc.Lng.ToString(new System.Globalization.CultureInfo("en-US")) + "|"; } coords = coords.Remove(coords.Length - 1); if (list.Count <= 2 || coords.Length > (2048 - 256) || distance > 50000) { CustomMessageBox.Show("To many/few WP's or to Big a Distance " + (distance / 1000) + "km"); return(answer); } try { using (XmlTextReader xmlreader = new XmlTextReader("http://maps.google.com/maps/api/elevation/xml?path=" + coords + "&samples=" + (distance / 100).ToString(new System.Globalization.CultureInfo("en-US")) + "&sensor=false")) { while (xmlreader.Read()) { xmlreader.MoveToElement(); switch (xmlreader.Name) { case "elevation": alt = double.Parse(xmlreader.ReadString(), new System.Globalization.CultureInfo("en-US")); Console.WriteLine("DO it " + lat + " " + lng + " " + alt); PointLatLngAlt loc = new PointLatLngAlt(lat, lng, alt, ""); answer.Add(loc); pos++; break; case "lat": lat = double.Parse(xmlreader.ReadString(), new System.Globalization.CultureInfo("en-US")); break; case "lng": lng = double.Parse(xmlreader.ReadString(), new System.Globalization.CultureInfo("en-US")); break; default: break; } } } } catch { CustomMessageBox.Show("Error getting GE data"); } return(answer); }
List<PointLatLngAlt> getGEAltPath(List<PointLatLngAlt> list) { double alt = 0; double lat = 0; double lng = 0; int pos = 0; List<PointLatLngAlt> answer = new List<PointLatLngAlt>(); //http://code.google.com/apis/maps/documentation/elevation/ //http://maps.google.com/maps/api/elevation/xml string coords = ""; foreach (PointLatLngAlt loc in list) { coords = coords + loc.Lat.ToString(new System.Globalization.CultureInfo("en-US")) + "," + loc.Lng.ToString(new System.Globalization.CultureInfo("en-US")) + "|"; } coords = coords.Remove(coords.Length - 1); if (list.Count <= 2 || coords.Length > (2048 - 256) || distance > 50000) { MessageBox.Show("To many/few WP's or to Big a Distance " + (distance/1000) + "km"); return answer; } try { using (XmlTextReader xmlreader = new XmlTextReader("http://maps.google.com/maps/api/elevation/xml?path=" + coords + "&samples=" + (distance / 100).ToString(new System.Globalization.CultureInfo("en-US")) + "&sensor=false")) { while (xmlreader.Read()) { xmlreader.MoveToElement(); switch (xmlreader.Name) { case "elevation": alt = double.Parse(xmlreader.ReadString(), new System.Globalization.CultureInfo("en-US")); Console.WriteLine("DO it " + lat + " " + lng + " " + alt); PointLatLngAlt loc = new PointLatLngAlt(lat,lng,alt,""); answer.Add(loc); pos++; break; case "lat": lat = double.Parse(xmlreader.ReadString(), new System.Globalization.CultureInfo("en-US")); break; case "lng": lng = double.Parse(xmlreader.ReadString(), new System.Globalization.CultureInfo("en-US")); break; default: break; } } } } catch { MessageBox.Show("Error getting GE data"); } return answer; }
private void processLine(string line) { try { Application.DoEvents(); line = line.Replace(", ", ","); line = line.Replace(": ", ":"); string[] items = line.Split(',', ':'); if (items[0].Contains("CMD")) { if (flightdata.Count == 0) { if (int.Parse(items[2]) <= (int)MAVLink.MAV_CMD.LAST) // wps { PointLatLngAlt temp = new PointLatLngAlt(double.Parse(items[5], new System.Globalization.CultureInfo("en-US")) / 10000000, double.Parse(items[6], new System.Globalization.CultureInfo("en-US")) / 10000000, double.Parse(items[4], new System.Globalization.CultureInfo("en-US")) / 100, items[1].ToString()); cmd.Add(temp); } } } if (items[0].Contains("MOD")) { positionindex++; modelist.Add(""); // i cant be bothered doing this properly modelist.Add(""); modelist[positionindex] = (items[1]); } if (items[0].Contains("GPS") && items[2] == "1" && items[4] != "0" && items[4] != "-1" && lastline != line) // check gps line and fixed status { if (position[positionindex] == null) { position[positionindex] = new List <Point3D>(); } double alt = double.Parse(items[6], new System.Globalization.CultureInfo("en-US")); if (items.Length == 11 && items[6] == "0.0000") { alt = double.Parse(items[7], new System.Globalization.CultureInfo("en-US")); } position[positionindex].Add(new Point3D(double.Parse(items[5], new System.Globalization.CultureInfo("en-US")), double.Parse(items[4], new System.Globalization.CultureInfo("en-US")), alt)); oldlastpos = lastpos; lastpos = (position[positionindex][position[positionindex].Count - 1]); lastline = line; } if (items[0].Contains("GPS") && items[4] != "0" && items[4] != "-1" && items.Length <= 9) { if (position[positionindex] == null) { position[positionindex] = new List <Point3D>(); } MainV2.cs.firmware = MainV2.Firmwares.ArduCopter2; double alt = double.Parse(items[5], new System.Globalization.CultureInfo("en-US")); position[positionindex].Add(new Point3D(double.Parse(items[4], new System.Globalization.CultureInfo("en-US")), double.Parse(items[3], new System.Globalization.CultureInfo("en-US")), alt)); oldlastpos = lastpos; lastpos = (position[positionindex][position[positionindex].Count - 1]); lastline = line; } if (items[0].Contains("CTUN")) { ctunlast = items; } if (items[0].Contains("NTUN")) { ntunlast = items; line = "ATT:" + double.Parse(ctunlast[3], new System.Globalization.CultureInfo("en-US")) * 100 + "," + double.Parse(ctunlast[6], new System.Globalization.CultureInfo("en-US")) * 100 + "," + double.Parse(items[1], new System.Globalization.CultureInfo("en-US")) * 100; items = line.Split(',', ':'); } if (items[0].Contains("ATT")) { try { if (lastpos.X != 0 && oldlastpos.X != lastpos.X && oldlastpos.Y != lastpos.Y) { Data dat = new Data(); try { dat.datetime = int.Parse(lastline.Split(',', ':')[1]); } catch { } runmodel = new Model(); runmodel.Location.longitude = lastpos.X; runmodel.Location.latitude = lastpos.Y; runmodel.Location.altitude = lastpos.Z; oldlastpos = lastpos; runmodel.Orientation.roll = double.Parse(items[1], new System.Globalization.CultureInfo("en-US")) / -100; runmodel.Orientation.tilt = double.Parse(items[2], new System.Globalization.CultureInfo("en-US")) / -100; runmodel.Orientation.heading = double.Parse(items[3], new System.Globalization.CultureInfo("en-US")) / 100; dat.model = runmodel; dat.ctun = ctunlast; dat.ntun = ntunlast; flightdata.Add(dat); } } catch { } } } catch (Exception) { // if items is to short or parse fails.. ignore } }
/// <summary> /// Used to extract mission from log file /// </summary> /// <param name="buffer">packet</param> void getWPsfromstream(ref byte[] buffer) { #if MAVLINK10 if (buffer[5] == MAVLINK_MSG_ID_MISSION_COUNT) { // clear old wps = new PointLatLngAlt[wps.Length]; } if (buffer[5] == MAVLink.MAVLINK_MSG_ID_MISSION_ITEM) { mavlink_mission_item_t wp = buffer.ByteArrayToStructure<mavlink_mission_item_t>(6); #else if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_COUNT) { // clear old wps = new PointLatLngAlt[wps.Length]; } if (buffer[5] == MAVLink.MAVLINK_MSG_ID_WAYPOINT) { mavlink_waypoint_t wp = buffer.ByteArrayToStructure<mavlink_waypoint_t>(6); #endif wps[wp.seq] = new PointLatLngAlt(wp.x, wp.y, wp.z, wp.seq.ToString()); Console.WriteLine("WP # {7} cmd {8} p1 {0} p2 {1} p3 {2} p4 {3} x {4} y {5} z {6}",wp.param1,wp.param2,wp.param3,wp.param4,wp.x,wp.y,wp.z,wp.seq,wp.command); } } public PointLatLngAlt getFencePoint(int no, ref int total) { byte[] buffer; MainV2.giveComport = true; PointLatLngAlt plla = new PointLatLngAlt(); mavlink_fence_fetch_point_t req = new mavlink_fence_fetch_point_t(); req.idx = (byte)no; req.target_component = compid; req.target_system = sysid; // request point generatePacket(MAVLINK_MSG_ID_FENCE_FETCH_POINT, req); DateTime start = DateTime.Now; int retrys = 3; while (true) { if (!(start.AddMilliseconds(500) > DateTime.Now)) { if (retrys > 0) { log.Info("getFencePoint Retry " + retrys + " - giv com " + MainV2.giveComport); generatePacket(MAVLINK_MSG_ID_FENCE_FETCH_POINT, req); start = DateTime.Now; retrys--; continue; } MainV2.giveComport = false; throw new Exception("Timeout on read - getFencePoint"); } buffer = readPacket(); if (buffer.Length > 5) { if (buffer[5] == MAVLINK_MSG_ID_FENCE_POINT) { MainV2.giveComport = false; mavlink_fence_point_t fp = buffer.ByteArrayToStructure<mavlink_fence_point_t>(6); plla.Lat = fp.lat; plla.Lng = fp.lng; plla.Tag = fp.idx.ToString(); total = fp.count; return plla; } } } }
/* * 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"); } } }
private void processLine(string line) { try { Application.DoEvents(); line = line.Replace(", ", ","); line = line.Replace(": ", ":"); string[] items = line.Split(',', ':'); if (items[0].Contains("CMD")) { if (flightdata.Count == 0) { if (int.Parse(items[2]) <= (int)MAVLink.MAV_CMD.LAST) // wps { PointLatLngAlt temp = new PointLatLngAlt(double.Parse(items[5], new System.Globalization.CultureInfo("en-US")) / 10000000, double.Parse(items[6], new System.Globalization.CultureInfo("en-US")) / 10000000, double.Parse(items[4], new System.Globalization.CultureInfo("en-US")) / 100, items[1].ToString()); cmd.Add(temp); } } } if (items[0].Contains("MOD")) { positionindex++; modelist.Add(""); // i cant be bothered doing this properly modelist.Add(""); modelist[positionindex] = (items[1]); } if (items[0].Contains("GPS") && items[2] == "1" && items[4] != "0" && items[4] != "-1" && lastline != line) // check gps line and fixed status { if (position[positionindex] == null) position[positionindex] = new List<Point3D>(); double alt = double.Parse(items[6], new System.Globalization.CultureInfo("en-US")); if (items.Length == 11 && items[6] == "0.0000") alt = double.Parse(items[7], new System.Globalization.CultureInfo("en-US")); if (items.Length == 11 && items[6] == "0") alt = double.Parse(items[7], new System.Globalization.CultureInfo("en-US")); position[positionindex].Add(new Point3D(double.Parse(items[5], new System.Globalization.CultureInfo("en-US")), double.Parse(items[4], new System.Globalization.CultureInfo("en-US")), alt)); oldlastpos = lastpos; lastpos = (position[positionindex][position[positionindex].Count - 1]); lastline = line; } if (items[0].Contains("GPS") && items[4] != "0" && items[4] != "-1" && items.Length <= 9) { if (position[positionindex] == null) position[positionindex] = new List<Point3D>(); double alt = double.Parse(items[5], new System.Globalization.CultureInfo("en-US")); position[positionindex].Add(new Point3D(double.Parse(items[4], new System.Globalization.CultureInfo("en-US")), double.Parse(items[3], new System.Globalization.CultureInfo("en-US")), alt)); oldlastpos = lastpos; lastpos = (position[positionindex][position[positionindex].Count - 1]); lastline = line; } if (items[0].Contains("CTUN")) { ctunlast = items; } if (items[0].Contains("NTUN")) { ntunlast = items; line = "ATT:" + double.Parse(ctunlast[3], new System.Globalization.CultureInfo("en-US")) * 100 + "," + double.Parse(ctunlast[6], new System.Globalization.CultureInfo("en-US")) * 100 + "," + double.Parse(items[1], new System.Globalization.CultureInfo("en-US")) * 100; items = line.Split(',', ':'); } if (items[0].Contains("ATT")) { try { if (lastpos.X != 0 && oldlastpos.X != lastpos.X && oldlastpos.Y != lastpos.Y) { Data dat = new Data(); try { dat.datetime = int.Parse(lastline.Split(',', ':')[1]); } catch { } runmodel = new Model(); runmodel.Location.longitude = lastpos.X; runmodel.Location.latitude = lastpos.Y; runmodel.Location.altitude = lastpos.Z; oldlastpos = lastpos; runmodel.Orientation.roll = double.Parse(items[1], new System.Globalization.CultureInfo("en-US")) / -100; runmodel.Orientation.tilt = double.Parse(items[2], new System.Globalization.CultureInfo("en-US")) / -100; runmodel.Orientation.heading = double.Parse(items[3], new System.Globalization.CultureInfo("en-US")) / 100; dat.model = runmodel; dat.ctun = ctunlast; dat.ntun = ntunlast; flightdata.Add(dat); } } catch { } } } catch (Exception) { // if items is to short or parse fails.. ignore } }
public bool setFencePoint(byte index, PointLatLngAlt plla, byte fencepointcount) { mavlink_fence_point_t fp = new mavlink_fence_point_t(); fp.idx = index; fp.count = fencepointcount; fp.lat = (float)plla.Lat; fp.lng = (float)plla.Lng; fp.target_component = compid; fp.target_system = sysid; int retry = 3; while (retry > 0) { generatePacket(MAVLINK_MSG_ID_FENCE_POINT, fp); int counttemp = 0; PointLatLngAlt newfp = getFencePoint(fp.idx, ref counttemp); if (newfp.Lat == plla.Lat && newfp.Lng == fp.lng) return true; retry--; } return false; }
/* 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 } 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(); 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); } } } 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 } 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.Add(this); // Console.WriteLine(" " + DateTime.Now.Millisecond + " done "); } } catch { log.InfoFormat("CurrentState Binding error"); } } }
void mainloop() { DateTime nextsend = DateTime.Now; StreamWriter sw = new StreamWriter(File.OpenWrite("followmeraw.txt")); threadrun = true; while (threadrun) { try { string line = comPort.ReadLine(); sw.WriteLine(line); //string line = string.Format("$GP{0},{1:HHmmss},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},", "GGA", DateTime.Now.ToUniversalTime(), Math.Abs(lat * 100), MainV2.comPort.MAV.cs.lat < 0 ? "S" : "N", Math.Abs(lng * 100), MainV2.comPort.MAV.cs.lng < 0 ? "W" : "E", MainV2.comPort.MAV.cs.gpsstatus, MainV2.comPort.MAV.cs.satcount, MainV2.comPort.MAV.cs.gpshdop, MainV2.comPort.MAV.cs.alt, "M", 0, "M", ""); if (line.StartsWith("$GPGGA")) // { string[] items = line.Trim().Split(',', '*'); if (items[15] != GetChecksum(line.Trim())) { Console.WriteLine("Bad Nmea line " + items[15] + " vs " + GetChecksum(line.Trim())); continue; } if (items[6] == "0") { Console.WriteLine("No Fix"); continue; } gotolocation.Lat = double.Parse(items[2], CultureInfo.InvariantCulture) / 100.0; gotolocation.Lat = (int)gotolocation.Lat + ((gotolocation.Lat - (int)gotolocation.Lat) / 0.60); if (items[3] == "S") { gotolocation.Lat *= -1; } gotolocation.Lng = double.Parse(items[4], CultureInfo.InvariantCulture) / 100.0; gotolocation.Lng = (int)gotolocation.Lng + ((gotolocation.Lng - (int)gotolocation.Lng) / 0.60); if (items[5] == "W") { gotolocation.Lng *= -1; } gotolocation.Alt = intalt; // double.Parse(line.Substring(c9, c10 - c9 - 1)) + gotolocation.Tag = "Sats " + items[7] + " hdop " + items[8]; } if (DateTime.Now > nextsend && gotolocation.Lat != 0 && gotolocation.Lng != 0 && gotolocation.Alt != 0) // 200 * 10 = 2 sec /// lastgotolocation != gotolocation && { nextsend = DateTime.Now.AddMilliseconds(1000 / updaterate); Console.WriteLine("Sending follow wp " + DateTime.Now.ToString("h:MM:ss") + " " + gotolocation.Lat + " " + gotolocation.Lng + " " + gotolocation.Alt); lastgotolocation = new PointLatLngAlt(gotolocation); Locationwp gotohere = new Locationwp(); gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT; gotohere.alt = (float)(gotolocation.Alt); gotohere.lat = (gotolocation.Lat); gotohere.lng = (gotolocation.Lng); try { updateLocationLabel(gotohere); } catch { } if (MainV2.comPort.BaseStream.IsOpen && MainV2.comPort.giveComport == false) { try { MainV2.comPort.giveComport = true; MainV2.comPort.setGuidedModeWP(gotohere); MainV2.comPort.giveComport = false; } catch { MainV2.comPort.giveComport = false; } } } } catch { System.Threading.Thread.Sleep((int)(1000 / updaterate)); } } sw.Close(); }