//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";
}
});
}
