public static Library.SerialNumber CopySerialNumber(Library.SerialNumber serialNb)
{
Library.SerialNumber sn = new Library.SerialNumber();
sn.Part = new byte[8];
for (int i = 0; i < 8; i++)
{
sn.Part[i] = serialNb.Part[i];
}
return sn;
}
public AccelerometerRecording(String line)
{
var s = line.Split(':');
SerialNumber = Z.SerialNumberFromString(s[0]);
DateTime = DateTime.FromFileTimeUtc(long.Parse(s[1]));
Id = int.Parse(s[2]);
Filtered0 = int.Parse(s[3]);
Filtered1 = int.Parse(s[4]);
Filtered2 = int.Parse(s[5]);
Filtered3 = int.Parse(s[6]);
Filtered4 = int.Parse(s[7]);
Filtered5 = int.Parse(s[8]);
}
private void OnLoaded(object sender, RoutedEventArgs e)
{
Instance = this;
Library.SerialNumber serialNb = new Library.SerialNumber();
StringBuilder sn = new StringBuilder(16);
Library.GetSerialNumber(Z.Handle, ref serialNb);
for (int i = 0; i < 8; i++)
{
sn.AppendFormat("{0:x2}", serialNb.Part[i]);
}
Header.Content = String.Format("Results for {0}", sn.ToString());
LibZano.Version version = new LibZano.Version();
Library.GetFirmwareVersion(Z.Handle, ref version);
VersionHardware.Text = version.Hardware.ToString();
VersionMajor.Text = version.Major.ToString();
VersionMinor.Text = version.Minor.ToString();
VersionRevision.Text = version.Revision.ToString();
XValue.Text = Library.GetAccelOffsetConfiguration_X(Z.Handle).ToString();
YValue.Text = Library.GetAccelOffsetConfiguration_Y(Z.Handle).ToString();
ZValue.Text = Library.GetAccelOffsetConfiguration_Z(Z.Handle).ToString();
XMinValue.Text = Library.GetAccelOffsetConfiguration_MinX(Z.Handle).ToString();
YMinValue.Text = Library.GetAccelOffsetConfiguration_MinY(Z.Handle).ToString();
ZMinValue.Text = Library.GetAccelOffsetConfiguration_MinZ(Z.Handle).ToString();
XMaxValue.Text = Library.GetAccelOffsetConfiguration_MaxX(Z.Handle).ToString();
YMaxValue.Text = Library.GetAccelOffsetConfiguration_MaxY(Z.Handle).ToString();
ZMaxValue.Text = Library.GetAccelOffsetConfiguration_MaxZ(Z.Handle).ToString();
}
static void Init()
{
c = new FSM();
c.OnTrigger("NULL.connect", args =>
{
if (args.Length != 0 && args[0] is Library.SerialNumber)
{
Target = Z.CopySerialNumber((Library.SerialNumber) args[0]);
}
else
{
Target = new Library.SerialNumber();
Target.Part = new byte[8];
}
V.Navigate("Connect");
V.ConnectTrigger("connecting", Target);
Z.Connect();
c.Move("connect");
});
c.OnTrigger("connect.connect", args =>
{
V.ConnectTrigger("connecting", Target);
Z.Connect();
});
c.OnTrigger("connect.connected", args =>
{
if (Target.Part != null && Z.IsNullSerialNumber(Target) == false && Z.IsSerialNumber(Target) == false)
{
V.ConnectTrigger("unexpected_serial", Target);
Library.Disconnect(Z.Handle);
return;
}
V.ConnectTrigger("connected");
T.Trigger("connected");
});
c.OnTrigger("connect.booted", args =>
{
V.ConnectTrigger("booted");
});
c.OnTrigger("connect.not_connected", args =>
{
V.ConnectTrigger("not_connected");
T.Trigger("not_connected");
});
c.OnTrigger("connect.not_still", args =>
{
V.ConnectTrigger("not_still");
T.Trigger("not_still");
});
c.OnTrigger("connect.need_to_update", args =>
{
V.ConnectTrigger("need_to_update");
T.Trigger("need_to_update");
});
c.OnTrigger("connect.drone_not_associated", args =>
{
V.ConnectTrigger("drone_not_associated");
T.Trigger("drone_not_associated");
});
c.OnTrigger("connect.bad_update", args =>
{
V.ConnectTrigger("bad_update");
T.Trigger("bad_update");
});
c.OnTrigger("connect.firmware_upgrade_start", args =>
{
if (Z.IgnoreConnecting == false)
V.ConnectTrigger("firmware_upgrade_start");
T.Trigger("firmware_upgrade_start");
});
c.OnTrigger("connect.firmware_upgrade_in_progress", args =>
{
int percentage = LibZano.Library.GetFirmwareUploadProgress(Z.Handle);
if (Z.IgnoreConnecting == false)
V.ConnectTrigger("firmware_upgrade_in_progress", percentage);
T.Trigger("firmware_upgrade_in_progress", percentage);
});
c.OnTrigger("connect.firmware_upgrade_start_complete", args =>
{
if (Z.IgnoreConnecting == false)
V.ConnectTrigger("firmware_upgrade_start_complete");
T.Trigger("firmware_upgrade_start_complete");
});
c.OnTrigger("connect.firmware_upgrade_start_failed", args =>
{
if (Z.IgnoreConnecting == false)
V.ConnectTrigger("firmware_upgrade_start_failed");
T.Trigger("firmware_upgrade_start_failed");
});
}
public static Library.SerialNumber SerialNumberFromString(String str)
{
Library.SerialNumber sn = new Library.SerialNumber();
sn.Part = new byte[8];
for (int i = 0; i < 8; i++)
{
sn.Part[i] = byte.Parse(str.Substring(i*2, 2), NumberStyles.HexNumber);
}
return sn;
}
private Accelerometer()
{
OnTrigger("NULL.started", args =>
{
U.LoadRecordings();
V.Navigate("$Start");
/*
if (Z.IsConnected)
{
Move("connect");
Move("start");
}
else
{
Trigger("connect");
}*/
});
OnTrigger("NULL.start_new", args =>
{
for (int i = 0; i < 6; i++)
{
G.FilteredReadings[i] = 0;
G.FilteredComplete[i] = false;
G.FilteredValue[i] = 0;
}
});
OnTrigger("NULL.apply_new", args =>
{
G.SaveRecording = true;
Trigger("connect");
});
OnTrigger("NULL.apply_recording", args =>
{
if (args.Length != 0 && args[0] is AccelerometerRecording)
{
var recording = (AccelerometerRecording) args[0];
G.FilteredReadings[0] = K.MinReadings;
G.FilteredValue[0] = recording.Filtered0;
G.FilteredComplete[0] = true;
G.FilteredReadings[1] = K.MinReadings;
G.FilteredValue[1] = recording.Filtered1;
G.FilteredComplete[1] = true;
G.FilteredReadings[2] = K.MinReadings;
G.FilteredValue[2] = recording.Filtered2;
G.FilteredComplete[2] = true;
G.FilteredReadings[3] = K.MinReadings;
G.FilteredValue[3] = recording.Filtered3;
G.FilteredComplete[3] = true;
G.FilteredReadings[4] = K.MinReadings;
G.FilteredValue[4] = recording.Filtered4;
G.FilteredComplete[4] = true;
G.FilteredReadings[5] = K.MinReadings;
G.FilteredValue[5] = recording.Filtered5;
G.FilteredComplete[5] = true;
G.SaveRecording = false;
Trigger("connect", recording.SerialNumber);
}
});
OnTrigger("NULL.connect", args =>
{
if (Z.IsConnected == false)
{
A.Trigger("connect", args);
Move("connect");
}
else
{
Move("connect");
Trigger("connected");
}
});
OnTrigger("connect.connected", args =>
{
bool completeSet = true;
for (int i = 0; i < 6; i++)
{
if (G.FilteredComplete[i] == false)
{
completeSet = false;
}
}
if (completeSet)
{
Move("axis");
Trigger("save");
}
else
{
Move("wait_start");
}
});
OnMove("connect>wait_start", args =>
{
// Clear
G.DidSave = false;
for (int i = 0; i < 6; i++)
{
G.FilteredReadings[i] = 0;
G.FilteredComplete[i] = false;
G.FilteredValue[i] = 0;
}
V.Navigate("$Accelerometer");
});
OnTrigger("wait_start.start", args =>
{
Move("start");
});
OnMove("wait_start>start", args =>
{
// Get ZanoHandler to send gyros and raw acceleration frames this way.
Z.Listen(Symbols.kSensorsGyroGet, "receive_gyro");
Z.Listen(Symbols.kSensorsRawAccelerationGet, "receive_accelerometer");
Views.Accelerometer.Instance.Begin();
Move("axis");
});
OnTrigger("axis.retry", args =>
{
G.DidSave = false;
for (int i = 0; i < 6; i++)
{
G.FilteredReadings[i] = 0;
G.FilteredComplete[i] = false;
G.FilteredValue[i] = 0;
}
});
OnTrigger("axis.save, save.not_sent, connect.save", args =>
{
// Compile the readings into maximum and minimum readings
G.ResultsXMin = U.MinToShort(G.FilteredValue[U.AxisToIndex(1, 0, 0)], G.FilteredValue[U.AxisToIndex(-1, 0, 0)]);
G.ResultsXMax = U.MaxToShort(G.FilteredValue[U.AxisToIndex(1, 0, 0)], G.FilteredValue[U.AxisToIndex(-1, 0, 0)]);
G.ResultsYMin = U.MinToShort(G.FilteredValue[U.AxisToIndex(0, 1, 0)], G.FilteredValue[U.AxisToIndex(0, -1, 0)]);
G.ResultsYMax = U.MaxToShort(G.FilteredValue[U.AxisToIndex(0, 1, 0)], G.FilteredValue[U.AxisToIndex(0, -1, 0)]);
G.ResultsZMin = U.MinToShort(G.FilteredValue[U.AxisToIndex(0, 0, 1)], G.FilteredValue[U.AxisToIndex(0, 0, -1)]);
G.ResultsZMax = U.MaxToShort(G.FilteredValue[U.AxisToIndex(0, 0, 1)], G.FilteredValue[U.AxisToIndex(0, 0, -1)]);
G.ResultsXAvg = (short)(-1 * ((G.ResultsXMin + G.ResultsXMax) / 2));
G.ResultsYAvg = (short)(-1 * ((G.ResultsYMin + G.ResultsYMax) / 2));
G.ResultsZAvg = (short)(-1 * ((G.ResultsZMin + G.ResultsZMax) / 2));
// Copied from ConfigAccelOffsetSet
Frame sendFrame = new Frame(Z.Handle);
sendFrame.Reference = Symbols.kConfigAccelOffsetSet;
sendFrame.Type = Frame.kSendFrame;
unchecked
{
sendFrame.Names[0] = Symbols.kX;
sendFrame.Values[0] = (int)G.ResultsXAvg;
sendFrame.Names[1] = Symbols.kY;
sendFrame.Values[1] = (int)G.ResultsYAvg;
sendFrame.Names[2] = Symbols.kZ;
sendFrame.Values[2] = (int)G.ResultsZAvg;
sendFrame.Names[3] = Symbols.kXmin;
sendFrame.Values[3] = (int)G.ResultsXMin;
sendFrame.Names[4] = Symbols.kYmin;
sendFrame.Values[4] = (int)G.ResultsYMin;
sendFrame.Names[5] = Symbols.kZmin;
sendFrame.Values[5] = (int)G.ResultsZMin;
sendFrame.Names[6] = Symbols.kXmax;
sendFrame.Values[6] = (int)G.ResultsXMax;
sendFrame.Names[7] = Symbols.kYmax;
sendFrame.Values[7] = (int)G.ResultsYMax;
sendFrame.Names[8] = Symbols.kZmax;
sendFrame.Values[8] = (int)G.ResultsZMax;
}
LibZano.Calibration.Set(ref Z.SerialNumber, ref sendFrame);
Library.Send(ref sendFrame);
Frames.ConfigSave(Z.Handle);
StringBuilder sn = new StringBuilder(16);
Library.SerialNumber serialNb = new Library.SerialNumber();
Library.GetSerialNumber(Z.Handle, ref serialNb);
for (int i = 0; i < 8; i++)
{
sn.AppendFormat("{0:x2}", serialNb.Part[i]);
}
G.ResultsSerial = sn.ToString();
StringBuilder sb = new StringBuilder();
sb.Append(G.ResultsSerial);
sb.AppendFormat(" {0:X8} 09", Symbols.kConfigAccelOffsetSet);
sb.AppendFormat(" {0:X8} {1}", Symbols.kX, U.FormatShort(G.ResultsXAvg));
sb.AppendFormat(" {0:X8} {1}", Symbols.kY, U.FormatShort(G.ResultsYAvg));
sb.AppendFormat(" {0:X8} {1}", Symbols.kZ, U.FormatShort(G.ResultsZAvg));
sb.AppendFormat(" {0:X8} {1}", Symbols.kXmin, U.FormatShort(G.ResultsXMin));
sb.AppendFormat(" {0:X8} {1}", Symbols.kYmin, U.FormatShort(G.ResultsYMin));
sb.AppendFormat(" {0:X8} {1}", Symbols.kZmin, U.FormatShort(G.ResultsZMin));
sb.AppendFormat(" {0:X8} {1}", Symbols.kXmax, U.FormatShort(G.ResultsXMax));
sb.AppendFormat(" {0:X8} {1}", Symbols.kYmax, U.FormatShort(G.ResultsYMax));
sb.AppendFormat(" {0:X8} {1}", Symbols.kZmax, U.FormatShort(G.ResultsZMax));
Console.WriteLine(sb.ToString());
File.WriteAllText(Core.U.GetApplicationDataPath() + "last_accel_results.txt", sb.ToString());
if (G.SaveRecording)
{
AccelerometerRecording reading = new AccelerometerRecording();
reading.SerialNumber = Z.CopySerialNumber(serialNb);
G.HighestRecordingId++;
reading.Id = G.HighestRecordingId;
reading.Filtered0 = (int) G.FilteredValue[0];
reading.Filtered1 = (int) G.FilteredValue[1];
reading.Filtered2 = (int) G.FilteredValue[2];
reading.Filtered3 = (int) G.FilteredValue[3];
reading.Filtered4 = (int) G.FilteredValue[4];
reading.Filtered5 = (int) G.FilteredValue[5];
G.Recordings.Add(reading);
U.SaveRecordings();
}
G.ResultsValues = sb.ToString();
//StringBuilder fc = new StringBuilder();
//var now = DateTime.Now;
//fc.AppendFormat("{0} {1}{2}", now.ToLongDateString(), now.ToLongTimeString(), Environment.NewLine);
// fc.AppendFormat("{0}", sb.)
//fc.AppendLine(DateTime.Now.)
// The Zano will automatically reboot, so we should switch state and wait for the connected event which
// is handled by libZano.
Move("save_to_zano");
V.Navigate("$SaveToZano");
});
OnTrigger("save_to_zano.disconnected", args =>
{
Console.WriteLine("Probably Saved");
Move("save_to_server");
V.Navigate("$SaveToServer");
LibZano.ServerConnectivity.ForceRefresh();
});
OnTrigger("save_to_server.no_internet", args =>
{
Console.WriteLine("Checking for Internet");
LibZano.ServerConnectivity.ForceRefresh();
});
OnTrigger("save_to_server.configuration_downloaded", args =>
{
Console.WriteLine("Got for Internet");
if (G.DidSave == false)
{
G.DidSave = true;
Console.WriteLine("Saving to server...");
LibZano.Calibration.ApplyToServer(ref Z.SerialNumber, Symbols.kConfigAccelOffsetSet);
}
});
OnTrigger("save_to_server.config_server_save_complete", args =>
{
Console.WriteLine("Saved Config");
V.Navigate("$Complete");
});
OnTrigger("save_to_server.config_server_save_failed", args =>
{
Console.WriteLine("Not Saved Config");
});
OnTrigger("axis.tick", args =>
{
Frames.SensorsGyroGet(Z.Handle);
if (G.Recording)
{
Frames.SensorsRawAccelerationGet(Z.Handle);
}
});
OnTrigger("axis.receive_gyro", args =>
{
Frame frame = (Frame)args[0];
// Get the gyro vector and calculate the magnitude
double x = frame.Get(Symbols.kX);
double y = frame.Get(Symbols.kY);
double z = frame.Get(Symbols.kZ);
double mag = Math.Sqrt(x * x + y * y + z * z);
G.GyroMagnitude = mag;
Console.WriteLine("Gyro Mag = {0}", mag);
if (mag >= K.GyroMaxThreshold)
{
// If the magnitude is over the threshold stop asking for accelerometer
G.Recording = false;
G.RecordingTime = 0;
int lastAxis = U.AxisToIndex(G.LastAxisX, G.LastAxisY, G.LastAxisZ);
if (G.FilteredReadings[lastAxis] < K.MinReadings)
{
// Was the number of readings not finished? Then reset them
G.FilteredComplete[lastAxis] = false;
G.FilteredReadings[lastAxis] = 0;
G.FilteredValue[lastAxis] = 0;
}
}
else
{
// Underneath the current threshold
if (G.Recording == false)
{
// Not recording?
// Turn it on!
G.Recording = true;
G.RecordingTime = Library.Time();
G.RecordingStarted = true;
}
}
// Refresh the view
if (Views.Accelerometer.Instance != null)
{
Views.Accelerometer.Instance.Refresh();
}
});
OnTrigger("axis.receive_accelerometer", args =>
{
Frame frame = (Frame)args[0];
// Get raw accelerometer values
double x = frame.Get(Symbols.kX);
double y = frame.Get(Symbols.kY);
double z = frame.Get(Symbols.kZ);
int xDir = 0;
int yDir = 0;
int zDir = 0;
U.GetAxisGravityVector(x, y, z, ref xDir, ref yDir, ref zDir);
/*
// Console.WriteLine("{0} {1} {2}", x, y, z);
U.Normalise(ref x, ref y, ref z);
// Calculate the best axis that the Zano is pointing up as using
// three dot products against each of the unit vectors, from that
// you have a ~unit vector that is the best direction.
int xDir = U.DotToInt(x, y, z, 1, 0, 0);
int yDir = U.DotToInt(x, y, z, 0, 1, 0);
int zDir = U.DotToInt(x, y, z, 0, 0, 1);
*/
// Store old axis and copy new one over.
G.LastAxisX = G.AxisX;
G.LastAxisY = G.AxisY;
G.LastAxisZ = G.AxisZ;
G.AxisX = xDir;
G.AxisY = yDir;
G.AxisZ = zDir;
// Now on a different axis?
if (G.AxisX != G.LastAxisX || G.AxisY != G.LastAxisY || G.AxisZ != G.LastAxisZ)
{
int lastAxis = U.AxisToIndex(G.LastAxisX, G.LastAxisY, G.LastAxisZ);
if (G.FilteredReadings[lastAxis] >= K.MinReadings)
{
// Did all readings?
// Mark old axis as complete
G.FilteredComplete[lastAxis] = true;
}
else
{
// Didn't complete.?
// Reset readings and value, so they can be done again.
G.FilteredReadings[lastAxis] = 0;
G.FilteredComplete[lastAxis] = false;
G.FilteredValue[lastAxis] = 0;
}
// Say something
Console.WriteLine("Changed Axis from {0} to {1}", U.AxisToString(G.LastAxisX, G.LastAxisY, G.LastAxisZ), U.AxisToString(G.AxisX, G.AxisY, G.AxisZ));
}
int axisIndex = U.AxisToIndex(G.AxisX, G.AxisY, G.AxisZ);
// Read and perform a low pass if the number of readings of the current axis
// has not been met yet.
if (G.FilteredReadings[axisIndex] < K.MinReadings)
{
// Set Initial Value, which is +-4096 (based on axis) where 4096 is 1G.
// Ideally the accelerometer would be +-4096 but due to manufacturing this
// can vary, hence the need for calibration!
if (G.RecordingStarted)
{
G.RecordingStarted = false;
G.FilteredValue[axisIndex] = K.LowPassDefaultFilter * U.AxisSign(G.AxisX, G.AxisY, G.AxisZ);
}
// Perform a low pass filter on the value and store.
double filtered = G.FilteredValue[axisIndex];
double now = frame.Get(U.AxisToSymbol(G.AxisX, G.AxisY, G.AxisZ));
G.FilteredValue[axisIndex] = U.LowPass(filtered, now, K.LowPassD);
G.FilteredReadings[axisIndex]++;
}
// Exceeded reading count? Then it's complete
if (G.FilteredReadings[axisIndex] >= K.MinReadings)
{
G.FilteredComplete[axisIndex] = true;
}
// Update the view
// V.RefreshAccelerometerDisplay();
if (Views.Accelerometer.Instance != null)
{
Views.Accelerometer.Instance.Refresh();
}
});
OnTrigger("save_to_server.menu", args =>
{
Move("NULL");
T.Set("NULL");
});
}
