void Update() { if (Input.GetKeyDown(KeyCode.R)) { serial.Stop(); Restart(resetCountdownLength); } else if (Input.GetKeyDown(KeyCode.S)) { serial.Stop(); } else if (Input.GetKeyDown(KeyCode.V)) { cameraRoll = !cameraRoll; } string gitsha1 = serial.gitsha1; if (gitsha1 == null) { gitsha1 = "null"; } pose = serial.PopBicyclePose(); if (pose != null) { q.SetState(pose); // There is no operand to add between ushort in C# const int CH_CFG_ST_RESOLUTION = 10000; int dt = pose.timestamp - timestamp; // system clock @ 10 kHz if (dt < 0) { dt += 0xffff; } SetBicycleTransform(q); stateInfo.text = System.String.Format( "x: {0:F3}\ny: {1:F3}\npitch: {2:F3}\nyaw: {3:F3}\nroll: {4:F3}\nsteer: {5:F3}\nwheel: {6:F3}\nv: {7:F3}", q.x, q.y, Mathf.Rad2Deg * ((q.pitch + Math.PI) % (2 * Math.PI) - Math.PI), Mathf.Rad2Deg * ((q.yaw + Math.PI) % (2 * Math.PI) - Math.PI), Mathf.Rad2Deg * ((q.lean + Math.PI) % (2 * Math.PI) - Math.PI), Mathf.Rad2Deg * ((q.steer + Math.PI) % (2 * Math.PI) - Math.PI), Mathf.Rad2Deg * ((q.wheelAngle + Math.PI) % (2 * Math.PI) - Math.PI), pose.v); sensorInfo.text = System.String.Format( "firmware {0}\npose dt:\t\t{1} us\nunity dt:\t\t{2} us\nupdate dt:\t{3} us\ncamera roll: {4}", gitsha1, (dt * 1000 * 1000 / CH_CFG_ST_RESOLUTION).ToString("D6"), (stopwatch.ElapsedTicks * 1000 * 1000 / System.Diagnostics.Stopwatch.Frequency).ToString("D6"), pose.computation_time.ToString("D6"), cameraRoll); timestamp = pose.timestamp; stopwatch.Reset(); // .NET 2.0 doesn't have Stopwatch.Restart() stopwatch.Start(); } }
private void PushReceivedPacket() { const int EMPTY_PACKET_SIZE = 0; const int GITSHA1_PACKET_SIZE = 7; const int POSE_PACKET_SIZE = 36; switch (_packet_size) { case EMPTY_PACKET_SIZE: return; case GITSHA1_PACKET_SIZE: _gitsha1 = _ascii.GetString(_inactive_buffer, 0, GITSHA1_PACKET_SIZE); return; case POSE_PACKET_SIZE: BicyclePose pose = new BicyclePose(); pose.SetFromByteArray(_inactive_buffer); lock (_pose_lock) { _pose = pose; } return; default: Debug.Log(String.Format("Invalid packet size: {0}", ++_packet_error)); break; } }
public BicyclePose PopBicyclePose() { BicyclePose pose = null; lock (_pose_lock) { pose = _pose; _pose = null; } return(pose); }
public SerialThread(string portname, Int32 baudrate) { _portname = portname; _baudrate = baudrate; _port = null; _thread = null; _should_terminate = false; _buffer_one = new byte[_buffer_size]; _buffer_two = new byte[_buffer_size]; _buffer_offset = 0; _active_buffer = _buffer_one; _inactive_buffer = _buffer_two; _packet_start = 0; _packet_size = 0; _gitsha1 = null; _ascii = new System.Text.ASCIIEncoding(); _pose_lock = new object(); _pose = null; _packet_error = 0; int log_number = 0; _file = null; while (_file == null) { try { _file = new System.IO.FileStream( String.Format("pose{0}.log", log_number), System.IO.FileMode.CreateNew, System.IO.FileAccess.Write, System.IO.FileShare.Read); Debug.Log(String.Format("writing log to file: {0}", _file.Name)); } catch (System.IO.IOException e) { ++log_number; } } }