private void ReflashData(IMUData data)
{
labelData.Text = "";
labelData.Text = imuData.ToString();
if (imuData.SingleNode.Eul != null)
{
// attitudeIndicatorInstrumentControl1.SetAttitudeIndicatorParameters(-(double)imuData.SingleNode.Eul[1], (double)imuData.SingleNode.Eul[0]);
int aircraftHeading = 0;
try
{
aircraftHeading = Convert.ToInt16(imuData.SingleNode.Eul[2]);
}
catch
{
}
// dual to headingIndicatorInstrumentControl1 error, use the negnate number
aircraftHeading = -aircraftHeading;
if (aircraftHeading < 0)
{
aircraftHeading += 360;
}
// headingIndicatorInstrumentControl1.SetHeadingIndicatorParameters(aircraftHeading);
}
// altimeterInstrumentControl1.SetAlimeterParameters(Convert.ToInt32(Pa));
// airSpeedIndicatorInstrumentControl1.SetAirSpeedIndicatorParameters(SampleCounter.SampleRate);
label7.Text = "帧率: " + SampleCounter.SampleRate.ToString() + "Hz";
}
public void InitWithData(List <double> kinectSPAngles, List <double> kinectFlexAngles, IMUData imu, TrialTracker tt)
{
kinectSPAngleAt0T1 = kinectSPAngles;
kinectSPAngleAt0T1 = kinectFlexAngles;
imuData = imu;
trialTracker = tt;
}
void OnDecoded(object sender, byte[] data, int len)
{
if (hasStarted)
{
imuData = IMUData.Decode(data, len);
CSV_PutLine(imuData);
}
}
public static IMUData firstIntegral(IMUData data, IMUData origin)
{
DataPoint accelerometer = data.accelerometer.scale(data.time);
DataPoint gyroscope = data.gyroscope.scale(data.time);
DataPoint magnetometer = data.magnetometer.scale(data.time);
return(new IMUData(accelerometer, gyroscope, magnetometer, data.time) + origin);
}
public IMU(enums.IC_type type)
{
icType = type;
for (int i = 0; i < amountIMUAtributes; i++)
{
data[i] = new IMUData();
}
}
/* --- !! USES TIME OF FIRST DATA POINT !! --- */
/* --- !! FISRT IS THE VELOCITY SECOND IS POSITION !! --- */
public IMUData integrate_with(IMUData data)
{
DataPoint _accelerometer = accelerometer.integrate_with(data.accelerometer);
DataPoint _gyroscope = gyroscope.integrate_with(data.gyroscope);
DataPoint _magnetometer = magnetometer.integrate_with(magnetometer);
return(new IMUData(_accelerometer, _gyroscope, _magnetometer, time));
}
private void CSV_PutLine(IMUData data)
{
if (isInCSVTitle == true)
{
csvFileWriter.WriteCSVline(imuData.CSVHeaders.ToArray());
isInCSVTitle = false;
}
csvFileWriter.WriteCSVline(imuData.CSVData.ToArray());
}
protected override void OnIMUSensorUpdate(IMUData imu)
{
var q = imu.unity.quat;
if (baseRotarion == Quaternion.identity)
{
baseRotarion = q;
}
transform.rotation = Quaternion.Inverse(baseRotarion) * q;
}
//Acceleration and Angular Rate (0xC2)
void ProcessIMUc2(BinaryReader br, double ts)
{
IMUData imuData = new IMUData();
addsum(br);
float accelX = BitConverter.ToSingle(temp, 0);
addsum(br);
float accelY = BitConverter.ToSingle(temp, 0);
addsum(br);
float accelZ = BitConverter.ToSingle(temp, 0);
addsum(br);
float angRateX = BitConverter.ToSingle(temp, 0);
addsum(br);
float angRateY = BitConverter.ToSingle(temp, 0);
addsum(br);
float angRateZ = BitConverter.ToSingle(temp, 0);
addsum(br);
//float timer = BitConverter.ToSingle(temp, 0); //time since system power-up
uint timer = BitConverter.ToUInt32(temp, 0); //time since system power-up
br.ReadBytes(2);
UInt16 checksum = Endian.BigToLittle(br.ReadUInt16());
if (sum == checksum)
{
imuData.xAccel = accelX;
imuData.yAccel = accelY;
imuData.zAccel = accelZ;
imuData.xRate = angRateX;
imuData.yRate = angRateY;
imuData.zRate = angRateZ;
imuData.DataType = "IMU";
imuData.timer = timer;
}
else //if (showDebugMessages)
{
Console.Write("IMU Msg: @" + ts.ToString("F4") + ":::didn't pass checksum.");
Console.WriteLine();
}
sum = 0;
if (IMUUpdate != null)
{
IMUUpdate(this, new TimestampedEventArgs <IMUData>(ts, imuData));
}
if (showDebugMessages)
{
Console.WriteLine("Accel: ( " + accelX.ToString("F8") + ", " + accelY.ToString("F8") + ", " + accelZ.ToString("F8") + " )" + "AngRate: ( " + angRateX.ToString("F8") + ", " + angRateY.ToString("F8") + ", " + angRateZ.ToString("F8") + " ) Timer: " + timer);
}
}
IMUData[] integrate(IMUData[] data, IMUData origin)
{
IMUData[] integrals = new IMUData[data.Length];
integrals[0] = IMUData.firstIntegral(data[0], origin);
for (int i = 1; i < data.Length; i++)
{
integrals[i] = data[i].integrate_with(integrals[i - 1]);
}
return(integrals);
}
public void SetIMUData(IMUData data)
{
if (data.SingleNode.Eul != null)
{
this.SetPitchRollYaw(data.SingleNode.Eul[1], data.SingleNode.Eul[0], data.SingleNode.Eul[2]);
}
if (data.SingleNode.Quat != null)
{
this.SetQuaternion(data.SingleNode.Quat[0], data.SingleNode.Quat[1], data.SingleNode.Quat[2], data.SingleNode.Quat[3]);
}
}
private void ReceiveFrame()
{
// first the pose is sent
string pose = _client.ListenSync();
Pose p = JsonConvert.DeserializeObject <Pose>(pose);
_poseUpdater.setNewPose(p.pose.ToArray());
// then the IMU data is sent
string imu = _client.ListenSync();
_currentIMUData = JsonConvert.DeserializeObject <IMUData>(imu);
}
public IMUController(string[] csv, IMUData origin)
{
data = new IMUData[csv.Length - 1];
for (int i = 0; i < data.Length; i++)
{
data[i] = IMUData.fromCSVString(csv[i + 1]);
}
length = data.Length;
integrals = integrate(data, origin);
filteredOrientations = fuseAndFilter(data, integrals);
}
public override void Poll(HIDInterface.PacketData packet)
{
byte[] data = packet.rawData.Skip(12).Take(12).ToArray();
int[] ints = data.ToInt16();
this.data = new IMUData
{
xAcc = (int)(ints[0] * (16000d / 65535d / 1000d)),
yAcc = (int)(ints[1] * (16000d / 65535d / 1000d)),
zAcc = (int)(ints[2] * (16000d / 65535d / 1000d)),
xGyro = (int)(ints[3] * (4588d / 65535d)),
yGyro = (int)(ints[4] * (4588d / 65535d)),
zGyro = (int)(ints[5] * (4588d / 65535d))
};
}
private void ConnectionDataReceived(byte[] buffer, int index, int count)
{
fmConfig.PutRawData(buffer);
KbootDecoder.Input(buffer);
IMUData data;
data = SxDecode.Decode(buffer);
if (data != null)
{
SampleCounter.Increment(1);
imuData = data;
DoOnDataReceived(imuData);
}
}
private void DoOnDataReceived(IMUData data)
{
if (Form3DDisplay != null)
{
Form3DDisplay.SetIMUData(data);
}
fmConfig.PutPacket(this, data);
if (data.SingleNode.Gyr != null)
{
AddGraphData("Gyroscope", DateTime.Now, 0, data.SingleNode.Gyr[0]);
AddGraphData("Gyroscope", DateTime.Now, 1, data.SingleNode.Gyr[1]);
AddGraphData("Gyroscope", DateTime.Now, 2, data.SingleNode.Gyr[2]);
}
if (data.SingleNode.Acc != null)
{
AddGraphData("Accelerometer", DateTime.Now, 0, data.SingleNode.Acc[0]);
AddGraphData("Accelerometer", DateTime.Now, 1, data.SingleNode.Acc[1]);
AddGraphData("Accelerometer", DateTime.Now, 2, data.SingleNode.Acc[2]);
}
if (data.SingleNode.Mag != null)
{
AddGraphData("Magnetometer", DateTime.Now, 0, data.SingleNode.Mag[0]);
AddGraphData("Magnetometer", DateTime.Now, 1, data.SingleNode.Mag[1]);
AddGraphData("Magnetometer", DateTime.Now, 2, data.SingleNode.Mag[2]);
}
if (data.SingleNode.Eul != null)
{
AddGraphData("Euler Angles", DateTime.Now, 0, data.SingleNode.Eul[0]);
AddGraphData("Euler Angles", DateTime.Now, 1, data.SingleNode.Eul[1]);
AddGraphData("Euler Angles", DateTime.Now, 2, data.SingleNode.Eul[2]);
}
}
public Rotater(string path, Transform _body)
{
string[] strings = File.ReadAllLines(path);
List <string> stringFinal = new List <string>();
foreach (string str in strings)
{
if (str.Contains(","))
{
stringFinal.Add(str);
}
}
body = _body;
// if (body.name == "braco.R")
// {
// origin = new IMUData(DataPoint.zero(), DataPoint.zero(), DataPoint.zero(), 0);
// } else
// {
origin = new IMUData(DataPoint.zero(), new DataPoint(body.eulerAngles, 0), DataPoint.zero(), 0);
// }
data = new CSVData(stringFinal.ToArray(), origin);
}
private void OnKbootDecoderDataReceived(object sender, byte[] buf, int len)
{
device_data = IMUData.Decode(buf, len);
fmTermial.Input(device_data.ToString());
}
public void PutPacket(object sender, IMUData imuData)
{
this.imuData = imuData;
}
private void DataReceivedHandler(IMUData d)
{
_state.Data = d;
SendNotification(_subMgrPort, new IMUNotification(d));
}
public void InitWithData(List <float> kinectSPAngles, List <float> kinectFlexAngles, IMUData imu)
{
kinectSPAngleAt0 = kinectSPAngles;
kinectFlexAngleAt0 = kinectFlexAngles;
imuData = imu;
}
public static IMUData Decode(byte[] buf)
{
IMUData imu = null;
foreach (byte data in buf)
{
switch (state)
{
case status.kStatus_Idle:
if (data == 0xA6 || data == 0xAA)
{
if (data == 0xA6)
{
isR6082 = false;
}
if (data == 0xAA)
{
isR6082 = true;
}
state = status.kStatus_Hdr;
}
break;
case status.kStatus_Hdr:
if (data == 0xA6 && isR6082 == false)
{
DATA_LEN = 24;
state = status.kStatus_Data;
}
else if (data == 0x00 && isR6082 == true)
{
DATA_LEN = 13;
state = status.kStatus_Data;
}
else
{
state = status.kStatus_Idle;
}
list.Clear();
break;
case status.kStatus_Data:
list.Add(data);
if (list.Count == DATA_LEN)
{
state = status.kStatus_Idle;
byte[] ctx = list.ToArray();
byte checkSumRecv = ctx[DATA_LEN - 1];
byte checkSumCal = 0;
for (int i = 0; i < DATA_LEN - 1; i++)
{
checkSumCal += ctx[i];
}
if (checkSumCal == checkSumRecv)
{
imu = new IMUData();
if (isR6082 == false)
{
imu.SingleNode.Eul = new float[3];
imu.SingleNode.Eul[0] = (float)(Int16)(ctx[1] + (ctx[2] << 8));
imu.SingleNode.Eul[1] = (float)(Int16)(ctx[3] + (ctx[4] << 8));
imu.SingleNode.Eul[2] = (float)(Int16)(ctx[5] + (ctx[6] << 8));
imu.SingleNode.Eul[0] /= 100;
imu.SingleNode.Eul[1] /= 100;
imu.SingleNode.Eul[2] /= 100;
imu.SingleNode.Gyr = new float[3];
imu.SingleNode.Gyr[0] = (float)BitConverter.ToInt16(ctx, 7) / 100;
imu.SingleNode.Gyr[1] = (float)BitConverter.ToInt16(ctx, 9) / 100;
imu.SingleNode.Gyr[2] = (float)BitConverter.ToInt16(ctx, 11) / 100;
imu.SingleNode.Acc = new float[3];
imu.SingleNode.Acc[0] = (float)BitConverter.ToInt16(ctx, 13);
imu.SingleNode.Acc[1] = (float)BitConverter.ToInt16(ctx, 15);
imu.SingleNode.Acc[2] = (float)BitConverter.ToInt16(ctx, 17);
imu.ToStringData = string.Format("Angles(PRY):").PadRight(14) + imu.SingleNode.Eul[0].ToString("f2").PadLeft(5, ' ') + " " + imu.SingleNode.Eul[1].ToString("f2").PadLeft(5, ' ') + " " + imu.SingleNode.Eul[2].ToString("f2").PadLeft(5, ' ') + "\r\n";
imu.ToStringData += string.Format("加速度:").PadRight(11) + imu.SingleNode.Acc[0].ToString("0").PadLeft(5, ' ') + " " + imu.SingleNode.Acc[1].ToString("0").PadLeft(5, ' ') + " " + imu.SingleNode.Acc[2].ToString("0").PadLeft(5, ' ') + "\r\n";
imu.ToStringData += string.Format("角速度:").PadRight(11) + imu.SingleNode.Gyr[0].ToString("0").PadLeft(5, ' ') + " " + imu.SingleNode.Gyr[1].ToString("0").PadLeft(5, ' ') + " " + imu.SingleNode.Gyr[2].ToString("0").PadLeft(5, ' ') + "\r\n";
}
else
{
imu.SingleNode.Eul = new float[3];
imu.SingleNode.Eul[0] = 0;
imu.SingleNode.Eul[1] = 0;
imu.SingleNode.Eul[2] = (float)(Int16)(ctx[1] + (ctx[2] << 8));
imu.SingleNode.Eul[2] /= 100;
imu.SingleNode.Gyr = new float[3];
imu.SingleNode.Gyr[0] = 0;
imu.SingleNode.Gyr[1] = 0;
imu.SingleNode.Gyr[2] = (float)BitConverter.ToInt16(ctx, 3) / 100;
imu.SingleNode.Acc = new float[3];
imu.SingleNode.Acc[0] = (float)BitConverter.ToInt16(ctx, 5);
imu.SingleNode.Acc[1] = (float)BitConverter.ToInt16(ctx, 7);
imu.SingleNode.Acc[2] = (float)BitConverter.ToInt16(ctx, 9);
imu.ToStringData = string.Format("Angles(PRY):").PadRight(14) + imu.SingleNode.Eul[0].ToString("f2").PadLeft(5, ' ') + " " + imu.SingleNode.Eul[1].ToString("f2").PadLeft(5, ' ') + " " + imu.SingleNode.Eul[2].ToString("f2").PadLeft(5, ' ') + "\r\n";
imu.ToStringData += string.Format("加速度:").PadRight(11) + imu.SingleNode.Acc[0].ToString("0").PadLeft(5, ' ') + " " + imu.SingleNode.Acc[1].ToString("0").PadLeft(5, ' ') + " " + imu.SingleNode.Acc[2].ToString("0").PadLeft(5, ' ') + "\r\n";
imu.ToStringData += string.Format("角速度:").PadRight(11) + imu.SingleNode.Gyr[0].ToString("0").PadLeft(5, ' ') + " " + imu.SingleNode.Gyr[1].ToString("0").PadLeft(5, ' ') + " " + imu.SingleNode.Gyr[2].ToString("0").PadLeft(5, ' ') + "\r\n";
}
}
}
break;
default:
break;
}
}
return(imu);
}
public IMUDataMessage(int robotID, IMUData data)
{
this.robotID = robotID; this.data = data;
}
public static Quaternion rotationFunction(float time, IMUData origin)
{
return(Quaternion.Euler(new Vector3(0, function(time), 0) + origin.gyroscope.value));
}
public CSVData(string[] csv, IMUData origin)
{
name = csv[1].Split(',')[7];
controller = new IMUController(csv, origin);
times = getTimes(controller);
}
private void KootPacketReceived(object sender, byte[] buf, int len)
{
SampleCounter.Increment(1);
imuData = IMUData.Decode(buf, len);
DoOnDataReceived(imuData);
}
