static void dataReady_OnInterrupt(uint data1, uint data2, DateTime time)
{
ulong ticksOut = (ulong)time.Ticks;
led.Write(!led.Read());
acc.getValues(ref x, ref y, ref z);
acc.clearInterrupt();
Debug.Print(ticksOut.ToString() + " " + x.ToString());
}
static void dataReady_OnInterrupt(uint data1, uint data2, DateTime time)
{
acc.getValues(ref GlobalVariables.x, ref GlobalVariables.y, ref GlobalVariables.z);
acc.clearInterrupt();
Int16 zShort = (Int16)GlobalVariables.z;
//
// Conver to bytes and write out.
// Optimal synch word from http://www2.l-3com.com/tw/telemetry_tutorial/r_frame_synchronization_pattern.html
// Stuff it in the beginning. For now just use two Synch bytes
//
// In an effort to save bytes change the synch word to FF, use only
// one and store the tymers into 24 bits.
// Also write every second time only.
// Make the minimum on setting 0x3C to save the motor
//
if (GlobalVariables.writeNow)
{
GlobalVariables.writeNow = false;
UInt32 ticksOut = (UInt32)time.Ticks;
byte[] junk = new byte[6] {
// Synch word, stick with 2
// (byte)((ticksOut >> 8) & 0xFF),
// (byte)((ticksOut >> 16) & 0xFF), // Only three bytes
// (byte)((ticksOut >> 24) & 0xFF), // Only three bytes
(byte)(GlobalVariables.zKeep & 0xFF),
(byte)((GlobalVariables.zKeep >> 8) & 0xFF), // Only 2 bytes
(byte)(zShort & 0xFF),
(byte)((zShort >> 8) & 0xFF), // Only 2 bytes
(byte)(GlobalVariables.hertz & 0xFF),
(byte)((GlobalVariables.hertz >> 8) & 0xFF)
}; // Only 2 bytes
//(byte)((GlobalVariables.halTime >> 8) & 0xFF),
//(byte)((GlobalVariables.halTime >> 16) & 0xFF), // Only 3 bytes
//(byte)((GlobalVariables.halTime >> 24) & 0xFF)}; // Only 3 bytes
sock.SendTo(junk, sendingEndPoint);
// Debug.Print(zShort.ToString());
}
else
{
GlobalVariables.writeNow = true;
GlobalVariables.zKeep = zShort;
}
}
static void accelInt_OnInterrupt(uint data1, uint data2, DateTime time)
{
// Debug.Print("Interrupting Sheep");
acc.getValues(ref GlobalVariables.x, ref GlobalVariables.y, ref GlobalVariables.z);
acc.clearInterrupt();
UInt16 xDig = (UInt16)(-GlobalVariables.x + 2048);
UInt16 zDig = (UInt16)(-GlobalVariables.z + 2048);
//
// Convert to bytes and write out.
// Optimal sync word from http://www2.l-3com.com/tw/telemetry_tutorial/r_frame_synchronization_pattern.html
// Stuff it in the beginning. For now just use two Sync bytes
//
// In an effort to save bytes change the sync word to FF, use only
// one and store the timers into 24 bits.
// Also write every second time only.
// Make the minimum on setting 0x3C to save the motor
//
byte[] junk = new byte[8] {
(byte)(zDig & 0xFF),
(byte)((zDig >> 8) & 0xFF), // Only 2 bytes
(byte)(xDig & 0xFF),
(byte)((xDig >> 8) & 0xFF),
(byte)(GlobalVariables.halTimeShifted & 0xFF),
(byte)((GlobalVariables.halTimeShifted >> 8) & 0xFF), // Only 2 bytes
(byte)(GlobalVariables.hertz & 0xFF),
(byte)((GlobalVariables.hertz >> 8) & 0xFF)
}; // Only 2 bytes
byte[] byteOut = System.Text.Encoding.UTF8.GetBytes("E" +
zDig.ToString() + "," +
xDig.ToString() + "," +
GlobalVariables.hertz.ToString() + "," +
GlobalVariables.receivedMessage[0].ToString() + "," +
GlobalVariables.receivedMessage[1].ToString() + "," +
GlobalVariables.receivedMessage[3].ToString() +
"\n");
phone.Print(byteOut);
// Debug.Print(GlobalVariables.throttleByte[0].ToString());
motorDrive.Duration = (UInt32)((double)GlobalVariables.throttleByte[0] * 980d / 512d + 1000);
// Do some mixing. Servos 1 & 3 are +, 2 is -. For collective.
// We take everything relative to 127. So it is 127 + collective. No cyclic for now.
UInt32 servo1out = (UInt32)((double)(127 + (GlobalVariables.receivedMessage[3] - 127)) * 1000d / 256d + GlobalVariables.servo1offset);
UInt32 servo2out = (UInt32)((double)(127 - (GlobalVariables.receivedMessage[3] - 127) + (GlobalVariables.receivedMessage[1] - 127)) * 1000d / 256d + GlobalVariables.servo2offset);
UInt32 servo3out = (UInt32)((double)(127 + (GlobalVariables.receivedMessage[3] - 127) + (GlobalVariables.receivedMessage[1] - 127)) * 1000d / 256d + GlobalVariables.servo3offset);
// Check limits.
// 1.
if (servo1out > GlobalVariables.servo1u)
{
servo1out = GlobalVariables.servo1u;
}
if (servo1out < GlobalVariables.servo1l)
{
servo1out = GlobalVariables.servo1l;
}
// 2.
if (servo2out > GlobalVariables.servo2u)
{
servo2out = GlobalVariables.servo2u;
}
if (servo2out < GlobalVariables.servo2l)
{
servo2out = GlobalVariables.servo2l;
}
// 3.
if (servo3out > GlobalVariables.servo3u)
{
servo3out = GlobalVariables.servo3u;
}
if (servo3out < GlobalVariables.servo3l)
{
servo3out = GlobalVariables.servo3l;
}
// Set PWMs.
servo1.Duration = servo1out;
servo2.Duration = servo2out;
servo3.Duration = servo3out;
//}
GlobalVariables.throttleOld = GlobalVariables.throttleByte[0];
}