-
Notifications
You must be signed in to change notification settings - Fork 0
/
esfins.cs
78 lines (62 loc) · 2.45 KB
/
esfins.cs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
using System;
using System.Runtime.InteropServices;
using System.IO;
namespace csNMEA
{
[StructLayout(LayoutKind.Sequential, Pack = 1)]
struct tEFSINS {
[MarshalAs(UnmanagedType.U4)]
public UInt32 bitfield;
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)]
public byte[] reserved;
[MarshalAs(UnmanagedType.U4)]
public UInt32 iTOW;
[MarshalAs(UnmanagedType.I4)]
public Int32 xAngRate;
[MarshalAs(UnmanagedType.I4)]
public Int32 yAngRate;
[MarshalAs(UnmanagedType.I4)]
public Int32 zAngRate;
[MarshalAs(UnmanagedType.I4)]
public Int32 xAccel;
[MarshalAs(UnmanagedType.I4)]
public Int32 yAccel;
[MarshalAs(UnmanagedType.I4)]
public Int32 zAccel;
}
public class ESFINS
{
private StreamWriter sw;
private bool writeFile = false;
public ESFINS(bool writefile) {
var fpath = $"{Program.CSVFilePath}/ESF-INS.csv";
var exists = File.Exists(fpath);
writeFile = writefile;
if (writefile) {
sw = new System.IO.StreamWriter(new System.IO.FileStream(fpath, FileMode.OpenOrCreate));
if (!exists) {
sw.WriteLine("timestamp,xAccel,yAccel,zAccel,xAngRate,yAngRate,zAngRate");
}
}
}
public async void WriteAsync(byte[] fielddata) {
var handle = GCHandle.Alloc(fielddata, GCHandleType.Pinned);
tEFSINS tins = (tEFSINS)Marshal.PtrToStructure(handle.AddrOfPinnedObject(), typeof(tEFSINS));
var ts = Program.GetTimestamp();
var bf = Convert.ToString(tins.bitfield, 2);
var xa = (tins.xAccel * .0001).ToString("0.###");
var ya = (tins.yAccel * .0001).ToString("0.###");
var za = (tins.zAccel * .0001).ToString("0.###");
var xr = (tins.xAngRate * .001).ToString("0.##");
var yr = (tins.yAngRate * .001).ToString("0.##");
var zr = (tins.zAngRate * .001).ToString("0.##");
if (writeFile) {
await sw.WriteLineAsync($"{ts},{bf},{xa},{ya},{za},{xr},{yr},{zr}");
await sw.FlushAsync();
}
else {
Console.WriteLine($"EFS-INS {ts} - Bitfield: {bf}, X AngRate|Accel: {xa}|{xr}, Y AngRate|Accel: {ya}|{yr}, Z AngRate|Accel: {za}|{zr}");
}
}
}
}