public void ReadAGPSStatus(ref SkyTraqConfig config)
{
config.agps_hours_left = 0;
config.agps_enabled = 0;
var now = DateTime.UtcNow;
var request = new SkyTraqPackage(
SkyTraqCommand.SKYTRAQ_COMMAND_READ_AGPS_STATUS,
(byte)(now.Year >> 8),
(byte)now.Year,
(byte)now.Month,
(byte)now.Day,
(byte)now.Hour,
(byte)now.Minute,
(byte)now.Second
);
if (ACK == WritePackageWithResponse(request))
{
var response = ReadNextPackage();
if (response != null)
{
config.agps_hours_left = BitConverter.ToUInt16(response.data, 1);
config.agps_enabled = response.data[3];
}
}
}
public void ReadAGPSStatus(ref SkyTraqConfig config)
{
config.agps_hours_left = 0;
config.agps_enabled = 0;
var now = DateTime.UtcNow;
var request = new SkyTraqPackage(
SkyTraqCommand.SKYTRAQ_COMMAND_READ_AGPS_STATUS,
(byte)(now.Year >> 8),
(byte)now.Year,
(byte)now.Month,
(byte)now.Day,
(byte)now.Hour,
(byte)now.Minute,
(byte)now.Second
);
if (ACK == WritePackageWithResponse(request))
{
var response = ReadNextPackage();
if (response != null)
{
config.agps_hours_left = BitConverter.ToUInt16(response.data, 1);
config.agps_enabled = response.data[3];
}
}
}
private int SetOutput(byte output_type, bool permanent)
{
int success;
var request = new SkyTraqPackage(
SkyTraqCommand.SKYTRAQ_COMMAND_CONFIGURE_MESSAGE_TYPE,
output_type,
(byte)(permanent ? 1 : 0)
);
success = WritePackageWithResponse(request);
return(success);
}
private SkyTraqPackage ReadNextPackage()
{
byte c, prevByte = 0;
var start = DateTime.Now;
serialPort.ReadTimeout = (int)(TIMEOUT * 1000);
try
{
c = ReadByte();
while ((DateTime.Now - start).TotalSeconds < TIMEOUT)
{
if ((prevByte == 0xa0) && c == 0xa1)
{
int dataRead;
byte end1, end2;
var pkg = new SkyTraqPackage(ReadUShort());
dataRead = 0;
while (dataRead < pkg.length)
{
pkg.data[dataRead++] = ReadByte();
}
pkg.checksum = ReadByte();
end1 = ReadByte();
end2 = ReadByte();
if (end1 == 0x0d && end2 == 0x0a)
{
if (pkg.CheckChecksum())
{
return(pkg);
}
}
}
prevByte = c;
c = ReadByte();
}
}
catch (System.ServiceProcess.TimeoutException)
{
return(null);
}
return(null);
}
private SkyTraqPackage ReadNextPackage()
{
byte c, prevByte = 0;
var start = DateTime.Now;
serialPort.ReadTimeout = (int)(TIMEOUT * 1000);
try
{
c = ReadByte();
while ((DateTime.Now - start).TotalSeconds < TIMEOUT)
{
if ((prevByte == 0xa0) && c == 0xa1)
{
int dataRead;
byte end1, end2;
var pkg = new SkyTraqPackage(ReadUShort());
dataRead = 0;
while (dataRead < pkg.length)
{
pkg.data[dataRead++] = ReadByte();
}
pkg.checksum = ReadByte();
end1 = ReadByte();
end2 = ReadByte();
if (end1 == 0x0d && end2 == 0x0a)
{
if (pkg.CheckChecksum())
{
return pkg;
}
}
}
prevByte = c;
c = ReadByte();
}
}
catch (System.ServiceProcess.TimeoutException)
{
return null;
}
return null;
}
private int WritePackageWithResponse(SkyTraqPackage p)
{
int retries_left = 3;
byte request_message_id;
request_message_id = p.data[0];
WritePackage(p);
Debug.WriteLine("Waiting for ACK with msg id 0x{0:x2}", request_message_id);
while (retries_left > 0)
{
var response = ReadNextPackage();
if (response != null)
{
if (response.data[0] == SkyTraqCommand.SKYTRAQ_RESPONSE_ACK)
{
Debug.WriteLine("got ACK for msg id: 0x{0:x2}", response.data[1]);
if (response.data[1] == request_message_id)
{
return(ACK);
}
}
else if (response.data[0] == SkyTraqCommand.SKYTRAQ_RESPONSE_NACK)
{
Debug.WriteLine("got NACK for msg id: 0x{0:x2}", response.data[1]);
if (response.data[1] == request_message_id)
{
return(NACK);
}
}
}
else
{
Debug.WriteLine("Got invalid package");
}
retries_left--;
}
Debug.WriteLine("Ran out of retries waiting for response of msg id 0x{0:x2}", request_message_id);
return(ERROR);
}
public int ReadDataloggerConfig(out SkyTraqConfig config)
{
int result = ERROR;
config = null;
var request = new SkyTraqPackage(SkyTraqCommand.SKYTRAQ_COMMAND_GET_CONFIG);
if (ACK == WritePackageWithResponse(request))
{
var response = ReadNextPackage();
if (response != null)
{
config = new SkyTraqConfig(response.data);
result = SUCCESS;
}
}
return(result);
}
private void SendAGPSData(byte[] data)
{
uint checkSumA, checkSumB;
CalculateAGPSChecksum(data, out checkSumA, out checkSumB);
var request = new SkyTraqPackage(1);
request.data[0] = SkyTraqCommand.SKYTRAQ_COMMAND_SEND_AGPS_DATA;
if (ACK == WritePackageWithResponse(request))
{
System.Threading.Thread.Sleep(500);
var info_string = String.Format("BINSIZE = {0} Checksum = {1} Checksumb = {2} \0", data.Length, checkSumA, checkSumB);
serialPort.Write(info_string);
var buff = new byte[50];
int c = serialPort.Read(buff, 0, buff.Length);
var str = Encoding.ASCII.GetString(buff, 0, 2);
}
}
public void SetSpeed(int baudrate, bool permanent)
{
byte speed = byte.MaxValue;
switch (baudrate)
{
case 4800: speed = 0; break;
case 9600: speed = 1; break;
case 19200: speed = 2; break;
case 38400: speed = 3; break;
case 57600: speed = 4; break;
case 115200: speed = 5; break;
default: Debug.WriteLine("Baudrate {0} is not supported by device!", baudrate); return;
}
var request = new SkyTraqPackage(SkyTraqCommand.SKYTRAQ_COMMAND_CONFIGURE_SERIAL_PORT, 0, speed, (byte)(permanent ? 1 : 0));
if (ACK == WritePackageWithResponse(request))
{
serialPort.BaudRate = baudrate;
}
else
{
Debug.WriteLine("setting line speed FAILED");
}
}
public int ReadSoftwareVersion()
{
int result = ERROR;
var request = new SkyTraqPackage(SkyTraqCommand.SKYTRAQ_COMMAND_QUERY_SOFTWARE_VERSION, 1);
if (ACK == WritePackageWithResponse(request))
{
var response = ReadNextPackage();
if (response != null)
{
var printf = String.Format("Kernel version: {0}.{1}.{2} -- ODM version: {3}.{4}.{5} -- revision: 20{6:00}-{7:00}-{8:00}",
response.data[3], response.data[4], response.data[5],
response.data[7], response.data[8], response.data[9],
response.data[11], response.data[12], response.data[13]
);
Console.WriteLine(printf);
result = SUCCESS;
}
}
return result;
}
public int ReadSoftwareVersion()
{
int result = ERROR;
var request = new SkyTraqPackage(SkyTraqCommand.SKYTRAQ_COMMAND_QUERY_SOFTWARE_VERSION, 1);
if (ACK == WritePackageWithResponse(request))
{
var response = ReadNextPackage();
if (response != null)
{
var printf = String.Format("Kernel version: {0}.{1}.{2} -- ODM version: {3}.{4}.{5} -- revision: 20{6:00}-{7:00}-{8:00}",
response.data[3], response.data[4], response.data[5],
response.data[7], response.data[8], response.data[9],
response.data[11], response.data[12], response.data[13]
);
Console.WriteLine(printf);
result = SUCCESS;
}
}
return result;
}
public void ClearDatalog()
{
var request = new SkyTraqPackage(SkyTraqCommand.SKYTRAQ_COMMAND_ERASE);
WritePackageWithResponse(request);
}
public void ClearDatalog()
{
var request = new SkyTraqPackage(SkyTraqCommand.SKYTRAQ_COMMAND_ERASE);
WritePackageWithResponse(request);
}
public int ReadDataLogSector(byte sector, ref byte[] buffer)
{
if (buffer != null)
{
var request = new SkyTraqPackage(
SkyTraqCommand.SKYTRAQ_COMMAND_READ_SECTOR,
sector
);
if (ACK == WritePackageWithResponse(request))
{
int i, len, count = 0;
byte c, lastByte1 = 0, lastByte2 = 0, cs, checksum;
Debug.WriteLine("START READING DATA");
cs = 0;
len = 1;
c = ReadByte();
while (len > 0)
{
if ((lastByte2 == (byte)'E') && lastByte1 == (byte)'N' && c == (byte)'D')
{
cs = (byte)(cs ^ (byte)'N');
cs = (byte)(cs ^ (byte)'E');
Debug.WriteLine("DONE");
break;
}
if (count >= 2)
{
buffer[count - 2] = lastByte2;
}
cs = (byte)(cs ^ c);
lastByte2 = lastByte1;
lastByte1 = c;
count++;
len = 1;
c = ReadByte();
}
/* remaining characters after data block */
for (i = 0; i < 10; ++i)
{
ReadByte();
}
checksum = ReadByte();
for (i = 0; i < 5; ++i)
{
ReadByte();
}
if (cs == checksum) return count - 2;
else
{
Console.WriteLine("Wrong checksum for sector {0}", sector);
return ERROR;
}
}
}
return ERROR;
}
public int ReadDataloggerConfig(out SkyTraqConfig config)
{
int result = ERROR;
config = null;
var request = new SkyTraqPackage(SkyTraqCommand.SKYTRAQ_COMMAND_GET_CONFIG);
if (ACK == WritePackageWithResponse(request))
{
var response = ReadNextPackage();
if (response != null)
{
config = new SkyTraqConfig(response.data);
result = SUCCESS;
}
}
return result;
}
private void WritePackage(SkyTraqPackage p)
{
var data = p.GetRawPackage();
serialPort.Write(data, 0, data.Length);
}
public long ReadDataLogSectorBatch(byte start_sector, byte end_sector, out byte[] buffer)
{
if (end_sector <= start_sector)
{
Debug.WriteLine("Incorrect parameters");
buffer = null;
return ERROR;
}
buffer = new byte[4096 * (end_sector - start_sector + 1)];
var request = new SkyTraqPackage(
SkyTraqCommand.SKYTRAQ_COMMAND_READ_BATCH,
0, start_sector,
0, end_sector
);
if (ACK == WritePackageWithResponse(request))
{
long i, len, count = 0;
byte c, lastByte1 = 0, lastByte2 = 0, cs, checksum;
Debug.WriteLine("START READING DATA");
cs = 0;
len = 1;
c = ReadByte();
while (len > 0)
{
if ((lastByte2 == (byte)'E') && lastByte1 == (byte)'N' && c == (byte)'D')
{
cs = (byte)(cs ^ (byte)'N');
cs = (byte)(cs ^ (byte)'E');
Debug.WriteLine("DONE");
break;
}
if (count >= 2)
{
buffer[count - 2] = lastByte2;
}
cs = (byte)(cs ^ c);
lastByte2 = lastByte1;
lastByte1 = c;
count++;
len = 1;
c = ReadByte();
}
/* remaining characters after data block */
for (i = 0; i < 10; ++i)
{
ReadByte();
}
checksum = ReadByte();
for (i = 0; i < 5; ++i)
{
ReadByte();
}
if (cs == checksum) return count - 2;
else
{
Console.WriteLine("Wrong checksum for sectors {0}-{1}", start_sector, end_sector);
return ERROR;
}
}
return ERROR;
}
private void SendAGPSData(byte[] data)
{
uint checkSumA, checkSumB;
CalculateAGPSChecksum(data, out checkSumA, out checkSumB);
var request = new SkyTraqPackage(1);
request.data[0] = SkyTraqCommand.SKYTRAQ_COMMAND_SEND_AGPS_DATA;
if (ACK == WritePackageWithResponse(request))
{
System.Threading.Thread.Sleep(500);
var info_string = String.Format("BINSIZE = {0} Checksum = {1} Checksumb = {2} \0", data.Length, checkSumA, checkSumB);
serialPort.Write(info_string);
var buff = new byte[50];
int c = serialPort.Read(buff, 0, buff.Length);
var str = Encoding.ASCII.GetString(buff, 0, 2);
}
}
private int SetOutput(byte output_type, bool permanent)
{
int success;
var request = new SkyTraqPackage(
SkyTraqCommand.SKYTRAQ_COMMAND_CONFIGURE_MESSAGE_TYPE,
output_type,
(byte)(permanent ? 1 : 0)
);
success = WritePackageWithResponse(request);
return success;
}
public int ReadDataLogSector(byte sector, ref byte[] buffer)
{
if (buffer != null)
{
var request = new SkyTraqPackage(
SkyTraqCommand.SKYTRAQ_COMMAND_READ_SECTOR,
sector
);
if (ACK == WritePackageWithResponse(request))
{
int i, len, count = 0;
byte c, lastByte1 = 0, lastByte2 = 0, cs, checksum;
Debug.WriteLine("START READING DATA");
cs = 0;
len = 1;
c = ReadByte();
while (len > 0)
{
if ((lastByte2 == (byte)'E') && lastByte1 == (byte)'N' && c == (byte)'D')
{
cs = (byte)(cs ^ (byte)'N');
cs = (byte)(cs ^ (byte)'E');
Debug.WriteLine("DONE");
break;
}
if (count >= 2)
{
buffer[count - 2] = lastByte2;
}
cs = (byte)(cs ^ c);
lastByte2 = lastByte1;
lastByte1 = c;
count++;
len = 1;
c = ReadByte();
}
/* remaining characters after data block */
for (i = 0; i < 10; ++i)
{
ReadByte();
}
checksum = ReadByte();
for (i = 0; i < 5; ++i)
{
ReadByte();
}
if (cs == checksum)
{
return(count - 2);
}
else
{
Console.WriteLine("Wrong checksum for sector {0}", sector);
return(ERROR);
}
}
}
return(ERROR);
}
public void SetSpeed(int baudrate, bool permanent)
{
byte speed = byte.MaxValue;
switch (baudrate)
{
case 4800: speed = 0; break;
case 9600: speed = 1; break;
case 19200: speed = 2; break;
case 38400: speed = 3; break;
case 57600: speed = 4; break;
case 115200: speed = 5; break;
default: Debug.WriteLine("Baudrate {0} is not supported by device!", baudrate); return;
}
var request = new SkyTraqPackage(SkyTraqCommand.SKYTRAQ_COMMAND_CONFIGURE_SERIAL_PORT, 0, speed, (byte)(permanent ? 1 : 0));
if (ACK == WritePackageWithResponse(request))
{
serialPort.BaudRate = baudrate;
}
else
{
Debug.WriteLine("setting line speed FAILED");
}
}
private void WritePackage(SkyTraqPackage p)
{
var data = p.GetRawPackage();
serialPort.Write(data, 0, data.Length);
}
private int WritePackageWithResponse(SkyTraqPackage p)
{
int retries_left = 3;
byte request_message_id;
request_message_id = p.data[0];
WritePackage(p);
Debug.WriteLine("Waiting for ACK with msg id 0x{0:x2}", request_message_id);
while (retries_left > 0)
{
var response = ReadNextPackage();
if (response != null)
{
if (response.data[0] == SkyTraqCommand.SKYTRAQ_RESPONSE_ACK)
{
Debug.WriteLine("got ACK for msg id: 0x{0:x2}", response.data[1]);
if (response.data[1] == request_message_id) return ACK;
}
else if (response.data[0] == SkyTraqCommand.SKYTRAQ_RESPONSE_NACK)
{
Debug.WriteLine("got NACK for msg id: 0x{0:x2}", response.data[1]);
if (response.data[1] == request_message_id) return NACK;
}
}
else
{
Debug.WriteLine("Got invalid package");
}
retries_left--;
}
Debug.WriteLine("Ran out of retries waiting for response of msg id 0x{0:x2}", request_message_id);
return ERROR;
}
public long ReadDataLogSectorBatch(byte start_sector, byte end_sector, out byte[] buffer)
{
if (end_sector <= start_sector)
{
Debug.WriteLine("Incorrect parameters");
buffer = null;
return(ERROR);
}
buffer = new byte[4096 * (end_sector - start_sector + 1)];
var request = new SkyTraqPackage(
SkyTraqCommand.SKYTRAQ_COMMAND_READ_BATCH,
0, start_sector,
0, end_sector
);
if (ACK == WritePackageWithResponse(request))
{
long i, len, count = 0;
byte c, lastByte1 = 0, lastByte2 = 0, cs, checksum;
Debug.WriteLine("START READING DATA");
cs = 0;
len = 1;
c = ReadByte();
while (len > 0)
{
if ((lastByte2 == (byte)'E') && lastByte1 == (byte)'N' && c == (byte)'D')
{
cs = (byte)(cs ^ (byte)'N');
cs = (byte)(cs ^ (byte)'E');
Debug.WriteLine("DONE");
break;
}
if (count >= 2)
{
buffer[count - 2] = lastByte2;
}
cs = (byte)(cs ^ c);
lastByte2 = lastByte1;
lastByte1 = c;
count++;
len = 1;
c = ReadByte();
}
/* remaining characters after data block */
for (i = 0; i < 10; ++i)
{
ReadByte();
}
checksum = ReadByte();
for (i = 0; i < 5; ++i)
{
ReadByte();
}
if (cs == checksum)
{
return(count - 2);
}
else
{
Console.WriteLine("Wrong checksum for sectors {0}-{1}", start_sector, end_sector);
return(ERROR);
}
}
return(ERROR);
}
