public void IMUSensorUpdatedHandler(IMU imuSensor)
{
if (PropertyChanged != null)
{
PropertyChanged(this, new PropertyChangedEventArgs("IMUSensor"));
}
}
// Use this for initialization
private void Start()
{
//runs the pidLoop at 2khz
InvokeRepeating("pidLoop", 0, loopTime);
//gets access to all other scripts attached to this game obejct
receiver = gameObject.GetComponent <RX>();
getIMU = gameObject.GetComponent <IMU>();
motorControl = gameObject.GetComponent <Motor>();
}
/// <summary>
/// Handles receiving a payload from ROCKS representing IMU data.
/// </summary>
/// <param name="opcode">Opcode for packet received from ROCKS.</param>
/// <param name="payload">BCL packets received from ROCKS representing IMU data.</param>
/// <returns>bool - Success of receiving and parsing payload into a IMU object.</returns>
public bool HandlePacket(byte opcode, byte[] payload)
{
// Is opcode, payload valid (able to be made into IMU object). If no, return false
// Form payload into IMU object, update current data
IMU parsedIMU = BclPayloadToIMU(payload);
this.imu = parsedIMU;
// TODO return value
return(true);
}
/// <summary>
/// Transform IMU data in the form of a BCL payload into the IMU object resulting from this payload.
/// </summary>
/// <param name="payload">The payload representing the IMU data.</param>
/// <returns>GPS - The IMU data formed from the BCL payload.</returns>
private IMU BclPayloadToIMU(byte[] payload)
{
// TODO validity checking
// BCL packet -> IMU object
short xAccel = (short)((payload[0] << 8) | payload[1]);
short yAccel = (short)((payload[2] << 8) | payload[3]);
short zAccel = (short)((payload[4] << 8) | payload[5]);
short xOrient = (short)((payload[6] << 8) | payload[7]);
short yOrient = (short)((payload[8] << 8) | payload[9]);
short zOrient = (short)((payload[10] << 8) | payload[11]);
IMU parsedIMU = new IMU(xAccel, yAccel, zAccel, xOrient, yOrient, zOrient);
return(parsedIMU);
}
// Delete all Test Objects and files before starting
public void Start()
{
dock = GameObject.FindGameObjectWithTag("dockDevice").GetComponent <dockProperties>();
device = GameObject.FindGameObjectWithTag("virtualDevice");
imu = device.GetComponent <IMU>();
dockPresets = GameObject.FindGameObjectsWithTag("dockPresets").ToList();
checkObjects = GameObject.FindGameObjectsWithTag("clipCheck").ToList();
// sort the dock presets
dockPresets = dockPresets.OrderBy(preset => preset.transform.position.x).ToList();
ClearTests();
fileEditor.ClearDir(filePath);
// Creates a new test before starting
NewTest(deviceReset);
}
/// <summary>
/// 如果不用简单工厂
/// </summary>
/// <param name="str1"></param>
private void button2_NoEncapsulation(string className)
{
string str1 = "";
if (className == "gps")
{
Gps e1 = new Gps();
str1 = e1.GetName();
Invoke(new show(showTextbox), new object[] { str1 });
}
else
{
IMU e2 = new IMU();
str1 = e2.GetName();
Invoke(new show(showTextbox), new object[] { str1 });
}
}
public static IMUNotifyData ToIMUNotifyData(IMU data)
{
var ret = IMUNotifyData.Empty;
ret.deviceId = data.DeviceId;
ret.acc[0] = data.Acc.x;
ret.acc[1] = data.Acc.y;
ret.acc[2] = data.Acc.z;
ret.gyro[0] = data.Gyro.x;
ret.gyro[1] = data.Gyro.y;
ret.gyro[2] = data.Gyro.z;
ret.mag[0] = data.Mag.x;
ret.mag[1] = data.Mag.y;
ret.mag[2] = data.Mag.z;
ret.quat[0] = data.Quat.x;
ret.quat[1] = data.Quat.y;
ret.quat[2] = data.Quat.z;
ret.quat[3] = data.Quat.w;
return(ret);
}
private static EulerIMU ToEuler(IMU imu)
{
if (imu == null)
{
return new EulerIMU()
{
X_Pitch = 0,
Y_Roll = 0,
Z_Yaw = 0
}
}
;
//C# portation
double ySqr = imu.QY * imu.QY;
var euler = new EulerIMU();
// roll (x-axis rotation)
double t0 = +2.0 * (imu.QW * imu.QX + imu.QY * imu.QZ);
double t1 = +1.0 - 2.0 * (imu.QX * imu.QX + ySqr);
euler.Y_Roll = Math.Atan2(t0, t1);
euler.Y_Roll *= 180 / Math.PI;
// pitch (y-axis rotation)
double t2 = +2.0 * (imu.QW * imu.QY - imu.QZ * imu.QX);
t2 = ((t2 > 1.0) ? 1.0 : t2);
t2 = ((t2 < -1.0) ? -1.0 : t2);
euler.X_Pitch = Math.Asin(t2);
euler.X_Pitch *= 180 / Math.PI;
// yaw (z-axis rotation)
double t3 = +2.0 * (imu.QW * imu.QZ + imu.QX * imu.QY);
double t4 = +1.0 - 2.0 * (ySqr + imu.QZ * imu.QZ);
euler.Z_Yaw = Math.Atan2(t3, t4);
euler.Z_Yaw *= 180 / Math.PI;
return(euler);
}
}
public static string ToIMUNotifyJson(string deviceId, float[] acc, float[] gyro, float[] mag, float[] quat)
{
var imuData = new IMU {
DeviceId = deviceId,
Acc = new Vector3 {
x = acc[0], y = acc[1], z = acc[2]
},
Gyro = new Vector3 {
x = gyro[0], y = gyro[1], z = gyro[2]
},
Mag = new Vector3 {
x = mag[0], y = mag[1], z = mag[2]
},
Quat = new Quaternion {
x = quat[0], y = quat[1], z = quat[2], w = quat[3]
}
};
var data = new GrayBlueJson <IMU> {
Type = JsonType.NotifyIMU.ToString(),
Content = imuData
};
return(JsonConvert.SerializeObject(data));
}
public void BuildImu()
{
IEquip e = new IMU();
_usv.Add(e);
}
private DriveCommand DriveNoBallDetected(TennisBall ball)
{
// Sanity check
if (ball != null)
{
return(null);
}
Console.Write("Ball not detected | ");
// Clear verification queue if it has values
this.verificationQueue.Clear();
GPS ascent = AscentPacketHandler.GPSData;
double distanceToGate = ascent.GetDistanceTo(this.gate);
// Kick back to GPS
if (distanceToGate > DISTANCE_SWITCH_TO_GPS) // 6 meters
{
Console.WriteLine("Distance: " + distanceToGate + ". Switch to GPS");
switchToGPS = true;
return(DriveCommand.Straight(Speed.HALT));
}
// Turn to face heading, drive toward it
if (distanceToGate > DISTANCE_USE_HEADING) // 3 meters
{
Console.WriteLine("Distance: " + distanceToGate + ". Turning toward heading to drive towrad it");
short ascentHeading = AscentPacketHandler.Compass;
double headingToGate = ascent.GetHeadingTo(this.gate);
Console.Write("currCompass: "******" | headingToGoal: " + headingToGate + " | distance: " + distanceToGate + " | ");
// Aligned with heading. Start going straight
if (IMU.IsHeadingWithinThreshold(ascentHeading, headingToGate, Scan.HEADING_THRESHOLD))
{
return(DriveCommand.Straight(Speed.VISION));
}
// Turn toward gate heading angle
if (IMU.IsHeadingWithinThreshold(ascentHeading, (headingToGate + 90) % 360, 90))
{
return(DriveCommand.LeftTurn(Speed.VISION_SCAN));
}
else
{
return(DriveCommand.RightTurn(Speed.VISION_SCAN));
}
// Probably would work, kept as reference
/*
* double lowBound = headingToGate;
* double highBound = (headingToGate + 180) % 360;
*
* if (lowBound < highBound)
* {
* if (lowBound < ascentHeading && ascentHeading < highBound)
* {
* return DriveCommand.LeftTurn(DriveCommand.SPEED_VISION_SCAN);
* }
* else
* {
* return DriveCommand.RightTurn(DriveCommand.SPEED_VISION_SCAN);
* }
* }
* else
* {
* if (!(highBound < ascentHeading && ascentHeading < lowBound))
* {
* return DriveCommand.LeftTurn(DriveCommand.SPEED_VISION_SCAN);
* }
* else
* {
* return DriveCommand.RightTurn(DriveCommand.SPEED_VISION_SCAN);
* }
* }
*/
}
// If scanning, complete scan
if (this.scan != null)
{
Console.WriteLine("Scanning... Distance: " + distanceToGate);
if (!this.scan.IsComplete())
{
return(scan.FindNextDriveCommand());
}
else
{
this.completedScans++;
// Clear scan, will rescan below
this.scan = null;
}
}
switch (this.completedScans)
{
case 0:
{
// Initialize the first scan
if (distanceToGate > DISTANCE_CLOSE_RANGE) // 2 meters
{
// Turn toward heading. Scan, use heading as reference
StatusHandler.SendDebugAIPacket(Status.AIS_BEGIN_SCAN, "Scan: Using heading as reference. Distance: " + Math.Round(distanceToGate, 2));
Console.WriteLine("Scan: Using heading as reference. Distance: " + Math.Round(distanceToGate, 2));
this.scan = new Scan(this.gate, true);
}
else
{
// Scan
StatusHandler.SendDebugAIPacket(Status.AIS_BEGIN_SCAN, "Scan: Not using heading as reference. Distance: " + Math.Round(distanceToGate, 2));
Console.WriteLine("Scan: Not using heading as reference. Distance: " + Math.Round(distanceToGate, 2));
this.scan = new Scan(this.gate, false);
}
break;
}
case 1:
{
// Align toward heading, drive for 10ish seconds,
StatusHandler.SendDebugAIPacket(Status.AIS_BEGIN_SCAN, "Scan: 1st 10s scan toward heading. Distance: " + Math.Round(distanceToGate, 2));
Console.WriteLine("Scan: Distance: " + Math.Round(distanceToGate, 2) + ". Scanning (using heading). Driving 5m away...");
this.scan = new Scan(this.gate, SCAN_DRIVE_STRAIGHT_DURATION_MILLIS);
break;
}
/*
* case 2:
* {
* // Broaden HSV values, scan
* StatusHandler.SendDebugAIPacket(Status.AIS_BEGIN_SCAN, "Scan: Broadening HSV values. Distance: " + Math.Round(distanceToGate, 2));
* Console.WriteLine("Scan: Broadening HSV values. Distance: " + Math.Round(distanceToGate, 2));
*
* this.camera.BroadenHsvValues();
*
* this.scan = new Scan(this.gate, false);
*
* break;
* }
*/
case 2:
{
// Align toward heading (again), drive for 5ish seconds,
StatusHandler.SendDebugAIPacket(Status.AIS_BEGIN_SCAN, "Scan: 2nd 5m scan toward heading. Distance: " + Math.Round(distanceToGate, 2));
Console.WriteLine("Scan: 2nd 5m scan toward heading. Distance: " + Math.Round(distanceToGate, 2));
this.scan = new Scan(this.gate, SCAN_DRIVE_STRAIGHT_DURATION_MILLIS);
break;
}
case 3:
{
// We've already run 1 scan, 1 5m scan, 1 broaden HSV, 1 more 5m scan, so drive straight to kick back to GPS and do it over again
return(DriveCommand.Straight(Speed.VISION));
}
default:
{
break;
}
}
return(scan.FindNextDriveCommand());
}
public IMUViewModel()
{
IMUSensor = StatusUpdater.Instance.RoverStatus.IMUSensor;
StatusUpdater.Instance.IMUUpdated += new StatusUpdater.IMUUpdatedDelegate(this.IMUSensorUpdatedHandler);
}
/// <summary>
/// Find the next DriveCommand to be issued to ROCKS.
/// </summary>
/// <returns>DriveCommand - the next drive command for ROCKS to execute.</returns>
public DriveCommand FindNextDriveCommand()
{
GPS currGPS = AscentPacketHandler.GPSData;
short currCompass = AscentPacketHandler.Compass;
double idealDirection = currGPS.GetHeadingTo(gate);
double distance = AscentPacketHandler.GPSData.GetDistanceTo(gate);
// Add distance to averaging queue
while (this.averagingQueue.Count >= AVERAGING_QUEUE_CAPACITY)
{
double value;
this.averagingQueue.TryDequeue(out value);
}
this.averagingQueue.Enqueue(distance);
// Debugging - delete
Console.Write("currCompass: "******" | headingToGoal: " + idealDirection + " | distance: " + distance + " | ");
// Stop when within proximity to see if average distance of last 5 distances is within proximity.
// If so, wait to switch to Vision, otherwise this acts as a buffer.
if (distance <= GATE_PROXIMITY)
{
return(DriveCommand.Straight(Speed.HALT));
}
// If current heading within threshold, go straight
if (IMU.IsHeadingWithinThreshold(currCompass, idealDirection, THRESHOLD_HEADING_ANGLE))
{
return(DriveCommand.Straight(Speed.SLOW_OPERATION));
}
// not aligned with endGPS point, need to turn
// the math here is strange due to compass vs unit circle stuff
// the first case takes care of all time when the ideal direction is in some way east of us,
// and the second case takes care of all time when the ideal direction is in some way west of us
double opposite = (idealDirection + 180) % 360;
if (idealDirection < opposite)
{
// Modulo not necessary - ideal direction < 180
if (currCompass > idealDirection && currCompass < opposite)
{
// Turn left
return(DriveCommand.LeftTurn(Speed.SLOW_TURN));
}
else
{
// Turn right
return(DriveCommand.RightTurn(Speed.SLOW_TURN));
}
}
else
{
// Modulo necessary
if ((currCompass > idealDirection && currCompass < 360) || (currCompass >= 0 && currCompass < opposite))
{
// Turn left
return(DriveCommand.LeftTurn(Speed.SLOW_TURN));
}
else
{
// Turn right
return(DriveCommand.RightTurn(Speed.SLOW_TURN));
}
}
}
public void Init()
{
XmlNode node = XmlParser.GetModelNode(AppDomain.CurrentDomain.BaseDirectory + "\\..\\..\\..\\IMUTests.xml");
testIMU = new IMU(node.FirstChild.ChildNodes[1], program.assetList[0]);
}
public IMUHandler()
{
// TODO better way of doing this to avoid null pointers?
this.imu = new IMU(0, 0, 0, 0, 0, 0);
}
private void IMUTest_Load(object sender, EventArgs e)
{
imu = new IMU();
}
private void Start()
{
imu = GetComponent <IMU>();
}
public IMUAdapter(IMU target)
{
_target = target;
}
public static void ReceiveCallback(IAsyncResult ar)
{
String Gimbal_content = String.Empty;
String content = String.Empty;
List <byte> data = new List <byte>();
double[] data_f = new double[9];
double[] data_9axis = new double[9];
double[] acc = new double[3];
double[] gyr = new double[3];
double[] mag = new double[3];
int deviceID, SN;
// Retrieve the state object and the handler socket
// from the asynchronous state object.
StateObject state = (StateObject)ar.AsyncState;
Socket handler = state.workSocket;
try
{
// Read data from the client socket.
int bytesRead = handler.EndReceive(ar);
if (bytesRead > 0)
{
for (int i = 0; i <= bytesRead - 27; i++)
{
if (state.buffer[i] == 35 && state.buffer[i + 1] == 222 && state.buffer[i + 2] == 233 &&
state.buffer[i + 24] == 233 && state.buffer[i + 25] == 222 && state.buffer[i + 26] == 36)
{
//Get the data from packet
for (int index = i + 3; index <= i + 23; index++)
{
data.Add(state.buffer[index]);
}
if (!state.IDchecked)
{
deviceID = data[0];
SID_List.Add(deviceID);
sensor_num_ID.Add(deviceID);
state.IDchecked = true;
Sensors[deviceID] = new IMU();
//System.Diagnostics.Debug.Print("idchecked");
}
else
{
count++;
//myForm.Invoke(new UpdateLableHandler(myForm.printRemsg), count.ToString());
//System.Diagnostics.Debug.Print("Thread ID: ");
//System.Diagnostics.Debug.Print(Thread.CurrentThread.ManagedThreadId.ToString());
deviceID = data[0]; //packetData[3]
SN = (data[1] << 8 | data[2]);
//Gyro
gyr[0] = (short)(data[3] << 8 | data[4]);
gyr[1] = (short)(data[5] << 8 | data[6]);
gyr[2] = (short)(data[7] << 8 | data[8]);
//Acc
acc[0] = (short)(data[9] << 8 | data[10]);
acc[1] = (short)(data[11] << 8 | data[12]);
acc[2] = (short)(data[13] << 8 | data[14]);
//Mag
mag[0] = (short)(data[15] << 8 | data[16]);
mag[1] = (short)(data[17] << 8 | data[18]);
mag[2] = (short)(data[19] << 8 | data[20]);
// (""+ gyr[0]+" " +gyr[1]+" "+ gyr[2]);
// TCP_Handler.pitch = acc[0];
// TCP_Handler.roll = acc[1];
// TCP_Handler.yaw = acc[2];
double degree = 0;
double temp = 0;
int range = 0;
if (count == 1)
{
Gimbaloffset_flag_Y = true;
Gimbaloffset_flag_R = true;
Gimbaloffset_flag_P = true;
}
switch (deviceID) //看哪個sensor
{
case 11: //yaw
temp = acc[0]; //磁角值(原本是加速度值，但韌體封包改成是磁角值，因為磁角的值只有一個)
range = 1006; //0`1006(理論上0~3.3V，值為0~1023，但可變電阻是人工的，無法精準，所以可能調到0~3.2V，值為0~1006，每顆都不一樣)
count_yaw++;
yaw_status = count_yaw;
if (Gimbaloffset_flag_Y)
{
yawoffset = temp / range * 360; //初始值
Gimbaloffset_flag_Y = false; //設立flag使他只讀一次
}
degree = temp / range * 360 - yawoffset;
if (degree >= 180) //限制在-180~180度
{
degree -= 360;
}
else if (degree < -180)
{
degree += 360;
}
TCP_Handler.yaw = Math.Abs(degree) > 1 ? degree : 0;
break;
case 12: //roll
temp = acc[0];
range = 1013;
count_roll++;
roll_status = count_roll;
if (Gimbaloffset_flag_R)
{
rolloffset = temp / range * 360;
Gimbaloffset_flag_R = false;
}
degree = temp / range * 360 - rolloffset;
if (degree >= 180)
{
degree -= 360;
}
else if (degree < -180)
{
degree += 360;
}
TCP_Handler.roll = Math.Abs(degree) > 1 ? degree : 0;
break;
case 13: //pitch
temp = acc[0];
range = 1005;
count_pitch++;
pitch_status = count_pitch;
if (Gimbaloffset_flag_P)
{
pitchoffset = temp / range * 360;
Gimbaloffset_flag_P = false;
}
degree = temp / range * 360 - pitchoffset;
if (degree >= 180)
{
degree -= 360;
}
else if (degree < -180)
{
degree += 360;
}
TCP_Handler.pitch = Math.Abs(degree) > 1 ? degree : 0;
break;
}
//System.Diagnostics.Debug.Print(gyr[0].ToString() + " " + gyr[1].ToString() + " " + gyr[2].ToString());
//System.Diagnostics.Debug.Print(acc[0].ToString() + " " + acc[1].ToString() + " " + acc[2].ToString());
//System.Diagnostics.Debug.Print(mag[0].ToString() + " " + mag[1].ToString() + " " + mag[2].ToString());
//System.Diagnostics.Debug.Print("========");
//Raw data tranformation (calculate the ture value)
/*******************/
// n = mag.Length
// for (int i = 0 ;i< n;i++)
/*******************/
Sensors[deviceID].SN = SN;
//System.Diagnostics.Debug.Print("SN: " + SN.ToString());
// Write the data to file
//data_f = Sensors[deviceID].data_f;
//content += SN + "\t";
//content += deviceID + "\t";
Gimbal_content += SN + "\t";
Gimbal_content += deviceID + "\t";
Gimbal_content += degree + "\t";
//Debug.Print("my parameter G:{0},Q:{1},A:{2}", IMU.gyr_gf[0, 0], IMU.Q[0, 0], IMU.acc_gf[0, 0]);
string filename = foldername + "Sensor" + deviceID.ToString() + ".txt";
StreamWriter sw = new StreamWriter(filename, true);
sw.WriteLine(Gimbal_content);
sw.Close();
}
}
else
{
content = "";
Gimbal_content = "";
//WritetoFile(content, 1);
data.Clear();
}
}
#region original method to get the data
//for (int i = bytesRead - 1; i >= 26; i--)
// {
// //Decode the packet received from client
// if (state.buffer[i] == 36 && state.buffer[i - 1] == 222 && state.buffer[i - 2] == 233 &&
// state.buffer[i - 24] == 233 && state.buffer[i - 25] == 222 && state.buffer[i - 26] == 35)
// {
// //Get the data from packet
// for (int index = i - 23; index <= i - 3; index++)
// data.Add(state.buffer[index]);
// if (!state.IDchecked)
// {
// deviceID = data[0];
// MR_Term.SID_List.Add(deviceID);
// state.IDchecked = true;
// MR_Term.Sensors[deviceID] = new IMU();
// System.Diagnostics.Debug.Print("idchecked");
// }
// else
// {
// count++;
// //myForm.Invoke(new UpdateLableHandler(myForm.printRemsg), count.ToString());
// //System.Diagnostics.Debug.Print("Thread ID: ");
// //System.Diagnostics.Debug.Print(Thread.CurrentThread.ManagedThreadId.ToString());
// deviceID = data[0];
// SN = (data[1] << 8 | data[2]);
// //Gyro
// gyr[0] = (short)(data[3] << 8 | data[4]);
// gyr[1] = (short)(data[5] << 8 | data[6]);
// gyr[2] = (short)(data[7] << 8 | data[8]);
// //Acc
// acc[0] = (short)(data[9] << 8 | data[10]);
// acc[1] = (short)(data[11] << 8 | data[12]);
// acc[2] = (short)(data[13] << 8 | data[14]);
// //Mag
// mag[0] = (short)(data[15] << 8 | data[16]);
// mag[1] = (short)(data[17] << 8 | data[18]);
// mag[2] = (short)(data[19] << 8 | data[20]);
// //Raw data tranformation (calculate the ture value)
// for (int n = 0; n < 3; n++)
// {
// acc[n] = MR_Term.Sensors[deviceID].calcAcc(acc[n]);
// gyr[n] = MR_Term.Sensors[deviceID].calcGyro(gyr[n]);
// mag[n] = MR_Term.Sensors[deviceID].calcMag(mag[n]);
// }
// for (int n = 0; n < 3; n++)
// {
// MR_Term.Sensors[deviceID].data_9axis[n] = acc[n];
// MR_Term.Sensors[deviceID].data_9axis[n + 3] = gyr[n];
// MR_Term.Sensors[deviceID].data_9axis[n + 6] = mag[n];
// }
// ++MR_Term.Sensors[deviceID].indexInChart;
// MR_Term.Sensors[deviceID].SN = SN;
// MR_Term.Sensors[deviceID].data_f = MR_Term.Sensors[deviceID].MovingAverage_life(acc, gyr, mag);
// switch (MR_Term.Mode)
// {
// case MR_Term.Operation_Mode.M_Demo:
// //====================Demo Mode==========================
// #region Demo mode
// if (MR_Term.Sensors[deviceID].indexInChart < 10)
// {
// for (int t = 0; t < 9; t++)
// MR_Term.Sensors[deviceID].data4IniAtt[t].Add(MR_Term.Sensors[deviceID].data_f[t]);
// System.Diagnostics.Debug.Print("Att1");
// }
// else if (MR_Term.Sensors[deviceID].indexInChart == 10)
// {
// for (int t = 0; t < 9; t++)
// MR_Term.Sensors[deviceID].DataAve[t] = IMU.mean(MR_Term.Sensors[deviceID].data4IniAtt[t]);
// MR_Term.Sensors[deviceID].q = MR_Term.Sensors[deviceID].initial_attitude(MR_Term.Sensors[deviceID].DataAve);
// MR_Term.Sensors[deviceID].mag_ref[0] = MR_Term.Sensors[deviceID].DataAve[6];
// MR_Term.Sensors[deviceID].mag_ref[1] = MR_Term.Sensors[deviceID].DataAve[7];
// MR_Term.Sensors[deviceID].mag_ref[2] = MR_Term.Sensors[deviceID].DataAve[8];
// //System.Diagnostics.Debug.Print(MR_Term.Sensors[deviceID].q[0].ToString() + " " + MR_Term.Sensors[deviceID].q[1].ToString() + " " + MR_Term.Sensors[deviceID].q[2].ToString() + " " + MR_Term.Sensors[deviceID].q[3].ToString());
// //System.Diagnostics.Debug.Print("Att2");
// }
// else
// {
// MR_Term.Sensors[deviceID].q = MR_Term.Sensors[deviceID].EKF_cal(MR_Term.Sensors[deviceID].data_f, 0.02);
// MR_Term.Sensors[deviceID].eul = MR_Term.Sensors[deviceID].getEular(MR_Term.Sensors[deviceID].q);
// MR_Term.Sensors[deviceID].q_out = MR_Term.Sensors[deviceID].getQout(MR_Term.Sensors[deviceID].eul);
// //MR_Term.Sensors[deviceID].eul = MR_Term.Sensors[deviceID].qua2eul(MR_Term.Sensors[deviceID].q);
// //System.Diagnostics.Debug.Print(deviceID.ToString() + ": " + MR_Term.Sensors[deviceID].eul[0].ToString() + " " + MR_Term.Sensors[deviceID].eul[1].ToString() + " " + MR_Term.Sensors[deviceID].eul[2].ToString());
// //System.Diagnostics.Debug.Print("Att3");
// }
// #endregion
// break;
// case MR_Term.Operation_Mode.M_Record:
// //====================Record Mode==========================
// #region Record Mode
// #endregion
// break;
// case MR_Term.Operation_Mode.M_Calibartion:
// //====================Calibration Mode==========================
// #region
// #endregion
// break;
// }
// //StopTime = DateTime.Now;
// //ts = StopTime - StartTime;
// //myForm.Invoke(new UpdateLableHandler(myForm.printTime), ts.ToString());
// if (deviceID == MR_Term.n_SID)
// MR_Term.isGet = true;
// // Write the data to file
// data_f = MR_Term.Sensors[deviceID].data_f;
// content += SN + "\t";
// content += deviceID + "\t";
// foreach (double d in data_f)
// {
// content += Math.Round(d, 4) + "\t";
// }
// foreach (double e in MR_Term.Sensors[deviceID].eul)
// {
// content += Math.Round(e, 2) + "\t";
// }
// string filename = foldername + "tdata" + deviceID.ToString() + ".txt";
// StreamWriter sw = new StreamWriter(filename, true);
// sw.WriteLine(content);
// sw.Close();
// }
// }
// else
// {
// content = "";
// //WritetoFile(content, 1);
// data.Clear();
// }
// }
#endregion
#region another receive method
//for (int i = 0; i < bytesRead; i++)
//{
// switch(state.buffer[i])
// {
// case 35: //'#'
// start_flag = true;
// break;
// case 36: //'$'
// count++;
// if (start_flag == true && data.Count == 21)
// {
// deviceID = data[0];
// SN = (short)(data[1] << 8 | data[2]);
// Acc_x = (short)(data[3] << 8 | data[4]);
// Acc_y = (short)(data[5] << 8 | data[6]);
// Acc_z = (short)(data[7] << 8 | data[8]);
// Gyro_x = (short)(data[9] << 8 | data[10]);
// Gyro_y = (short)(data[11] << 8 | data[12]);
// Gyro_z = (short)(data[13] << 8 | data[14]);
// Mag_x = (short)(data[15] << 8 | data[16]);
// Mag_y = (short)(data[17] << 8 | data[18]);
// Mag_z = (short)(data[19] << 8 | data[20]);
// content = deviceID + "\t" + SN + "\t" + Acc_x + "\t" + Acc_y + "\t" + Acc_z + "\t" + Gyro_x + "\t" +
// Gyro_y + "\t" + Gyro_z + "\t" + Mag_x + "\t" + Mag_y + "\t" + Mag_z;
// //WritetoFile(content, deviceID);
// string filename = foldername + "tdata" + deviceID.ToString() + ".txt";
// StreamWriter sww = new StreamWriter(filename, true);
// sww.WriteLine(content);
// //sww.Flush();
// sww.Close();
// }
// else
// {
// start_flag = false;
// content = "";
// //WritetoFile(content);
// data.Clear();
// }
// myForm.Invoke( new UpdateLableHandler(myForm.printRemsg), count.ToString() );
// break;
// default:
// if (start_flag == true)
// data.Add(state.buffer[i]);
// break;
// }
//}
#endregion
}
//continue to receive next data
if (SocketApp.isRun)
{
handler.BeginReceive(state.buffer, 0, StateObject.BufferSize, 0,
out state.errorcode, new AsyncCallback(ReceiveCallback), state);
}
}
catch (ObjectDisposedException e)
{
//MessageBox.Show(e.ToString());
//do nothing
}
catch (SocketException e)
{
if (SocketApp.isRun)
{
SocketApp.removeClient(handler.RemoteEndPoint);
showClientName();
}
handler.Disconnect(true);
//handler.Shutdown(SocketShutdown.Both);
//handler.Close();
}
finally
{
}
}
