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;
}
}
}
