//void drawString(string str, double x, double y, double w, double h) //{ // using (QFont font = new QFont("Fonts/times.ttf", 20, new QFontBuilderConfiguration(true))) // { // font.Options.DropShadowActive = false; // QFont.Begin(); // GL.PushMatrix(); // GL.Translate(x + 30, h - y - 10, 0f); // font.Print(str, QFontAlignment.Left); // GL.PopMatrix(); // QFont.End(); // GL.Disable(EnableCap.Texture2D); // } //} /// <summary> /// This method triggers all the display functions to update graphs. /// </summary> private void UpdateDisplay(TJStatePacket state) { double Z = 255 - Math.Sqrt(Math.Abs(state.Accel.X * state.Accel.X + state.Accel.Y * state.Accel.Y)); roll_angle = Math.Atan2(state.Accel.X, Z); pitch_angle = Math.Atan2(state.Accel.Y, Z); glcAttitude.Invalidate(); //pieTemp.EndAngle = pieTemp.StartAngle + 30 + state.Temperature.Temperature * 0.3; //pieHumidity.EndAngle = pieHumidity.StartAngle + 30 + state.Humidity.Humidity; }
/// <summary> /// This function is called by the RF controller when data has been received. Here we identify what type of packet /// it is and instantiate an appropriate packet class to hold the data. The packet instance is then forwarded to /// all listeners of the PacketReceived event /// </summary> /// <param name="args"></param> private static void ProcessNewPacket(PacketReceivedEventArgs args) { int now = (int)sw.ElapsedTicks; TJPacket packet = args.Packet; switch (packet.PID) { case 0x00: packet = new TJStatePacket(packet.RawPacket); if (DragonflyStateUpdate != null) DragonflyStateUpdate(packet as TJStatePacket); break; case 0x01: packet = new TJState2Packet(packet.RawPacket); if (DragonflyState2Update != null) DragonflyState2Update(packet as TJState2Packet); break; case 0x02: // Scale factors packets break; case 0x3A: packet = new TJBootloaderResponsePacket(packet.RawPacket); if (DragonflyBootloaderUpdate != null) DragonflyBootloaderUpdate(packet as TJBootloaderResponsePacket); break; case 0x90: packet = new TJCameraPacket(packet.RawPacket); if (DragonflyCameraUpdate != null) DragonflyCameraUpdate(packet as TJCameraPacket); break; case 0x91: packet = new TJStartFramePacket(packet.RawPacket); if (DragonflyStartFrame != null) DragonflyStartFrame(packet as TJStartFramePacket); break; case 0x92: if (DragonflyEndFrame != null) DragonflyEndFrame(); break; default: break; } if (PacketReceived != null) PacketReceived(packet); // The following code is just to keep track of how many packets have been lost. int dif = 2; if (packet.Seq < lastSeqNum) { int seqdif = packet.Seq + 256 - lastSeqNum; if (seqdif != dif) { missed += seqdif / dif; } } else if (packet.Seq - lastSeqNum != dif) { missed += packet.Seq - lastSeqNum; } received++; if (received == 100) { Console.WriteLine("s/Packet: {0:0.00000}, missed: {1}", ((now - lastTime) / (double)received) / Stopwatch.Frequency, missed); lastTime = now; missed = 0; received = 0; } lastSeqNum = packet.Seq; }
/// <summary> /// This method keeps track of received packets in order to notify UpdateGraphs of when it should execute. /// </summary> void UpdateUIState(TJStatePacket state) { long now = stopwatch.ElapsedMilliseconds; this.Dispatcher.BeginInvoke((Action)delegate() { // Handle timers // In PID tuning tab, if the time exceeds the reference time, stop it TimeSpan ts = tuningTimer.Elapsed; // assign the elapsed time to TimeSpan class, to display the time properly string elapsedTime = String.Format("{0:00}:{1:00}:{2:00}", ts.Hours, ts.Minutes, ts.Seconds); labPidTimer.Content = elapsedTime; labDFTimer.Content = elapsedTime; if (tbxPidTimerRef.Text != "" || tbxDFTimerRef.Text != "") { try { int refTime = 0; if (tbxPidTimerRef.Text != "") refTime = Convert.ToInt16(tbxPidTimerRef.Text); else refTime = Convert.ToInt16(tbxDFTimerRef.Text); if (ts.TotalSeconds >= refTime && tuningTimer.IsRunning) { // Stop throttle byte[] throttlePacket = new byte[32]; throttlePacket[0] = 0x01; throttlePacket[2] = (byte)0; TJDragonfly.EnqueueTXPacket(throttlePacket); sldPidThrottle.Value = 0; labPidThrottleValue.Content = "0"; // Stop main motor byte[] DFPacket = new byte[32]; DFPacket[0] = (byte)TJCommandID.DFNomSatCmdID; DFPacket[2] = (byte)0; DFPacket[3] = (byte)sldDFServoNom.Value; DFPacket[4] = (byte)sldDFServoSat.Value; DFPacket[5] = (byte)sldDFPitchSat.Value; DFPacket[6] = (byte)sldDFYawSat.Value; TJDragonfly.EnqueueTXPacket(DFPacket); sldDFMainMotor.Value = 0; labDFMainMotorValue.Content = "0"; // Stop timer tuningTimer.Stop(); btnStart.IsEnabled = true; } } catch (Exception) { //throw; } } // Update Graphs if ((bool)cbxEnableGraphs.IsChecked) { packetIgnoreCount += 1; // See whether Aardvark is used if (TJDragonfly.getRFController() == 0) { bufferedStates.AddLast(new Tuple<TJStatePacket, long>(state, now)); packetIgnoreCount = 0; } else if (TJDragonfly.getRFController() == 1) { if (packetIgnoreCount >= packetRateScaler) { bufferedStates.AddLast(new Tuple<TJStatePacket, long>(state, now)); packetIgnoreCount = 0; } } if (now - lastGraphUpdate > graphUpdatePeriod) { lastGraphUpdate = now; LinkedList<Tuple<TJStatePacket, long>> statesToAdd = bufferedStates; this.Dispatcher.BeginInvoke((Action)delegate() { // Update global state 2 // Update graphic view globalState = state; UpdateDisplay(state); // Update engineering view UpdateGraphs(statesToAdd); }); bufferedStates = new LinkedList<Tuple<TJStatePacket, long>>(); } } // Update altitude information //tbAltPanic.Text = (state.Altitide.Panic == 0) ? "No" : "Yes"; tbAltPanic.Text = state.Altitide.Panic.ToString(); // to display target alt value if (cbbAltHold.SelectedIndex != 0) // display data in all three modes, auto take-off, landing, altitude hold tbAltReading.Text = state.Altitide.Altitude.ToString(); else // none of modes is selected { tbAltReading.Text = "0"; //tbAltPanic.Text = "No"; tbAltPanic.Text = "0"; } }); }