private static void RunTelemetry()
{
GlobalArea ga = GlobalArea.GetInstance();
Database db = Database.GetInstance();
PIDManager pid = PIDManager.GetInstance();
NavManager nav = NavManager.GetInstance();
AdcMessage adc = new AdcMessage();
ImuEulerMessage imu = new ImuEulerMessage();
PwmMessage pwm = new PwmMessage();
GpsMessage gps = new GpsMessage();
nav.Initialize(0, 0, 0);
Input p;
p = new SerialInput(path + telIn, b19200);
int i = 0;
while (true)
{
p.CheckStartOfMessage();
//Console.WriteLine("Message Received");
byte[] header = p.ReadNBytes(1);
byte[] m;
switch (header[0])
{
case (byte)0: //IMU-Euler Angles
m = p.ReadNBytes(25);
time += m[0];
imu.CreateMessage(time, m);
ga.Imu = imu;
db.Add(ga.Imu);
pid.SetChPitch(ga.Imu);
pid.SetChRoll(ga.Imu);
pid.SetChYaw(ga.Imu);
if (ga.IsReady())
{
nav.UpdateAltRef();
nav.UpdateHeadRef();
}
break;
case (byte)3: //Adc
m = p.ReadNBytes(7);
time += m[0];
adc.CreateMessage(time, m);
ga.Adc = adc;
db.Add(ga.Adc);
pid.SetChThrottle(adc);
if (ga.IsReady())
{
nav.UpdateAltRef();
nav.UpdateHeadRef();
}
break;
case (byte)4: //Pwm
m = p.ReadNBytes(9);
time += m[0];
pwm.CreateMessage(time, m);
ga.Pwm = pwm;
db.Add(ga.Pwm);
break;
case (byte)5: //Gps
m = p.ReadNBytes(13);
time += m[0];
gps.CreateMessage(time, m);
ga.Gps = gps;
db.Add(ga.Gps);
break;
}
if (i == 10)
{
op.Flush(db);
//op.Close();
//op = new Output();
i = 0;
}
i++;
}
}
static void Main(string[] args)
{
Input p;
Database db = Database.GetInstance();
Output op = new Output("Logs");
//if (Settings.Default.Mode == 0)
p = new SerialInput("COM19");
// else if (Settings.Default.Mode == 1)
//p = new FileInput("LOG06.TXT");
//else
// return;
ulong time = 0;
int i = 0;
while (true)
{
p.CheckStartOfMessage();
byte[] header = p.ReadNBytes(1);
byte[] m;
switch (header[0])
{
case (byte)0: //IMU-Euler Angles
m = p.ReadNBytes(7);
time += m[0];
ImuEulerMessage imuEuler = new ImuEulerMessage(time, m);
db.Add(imuEuler);
break;
case (byte)2: //Imu-Raw data
m = p.ReadNBytes(19);
time += m[0];
ImuRawMessage imuRaw = new ImuRawMessage(time, m);
db.Add(imuRaw);
break;
case (byte)3: //Adc
m = p.ReadNBytes(7);
time += m[0];
AdcMessage adc = new AdcMessage(time, m);
db.Add(adc);
break;
case (byte)4: //Pwm
m = p.ReadNBytes(11);
time += m[0];
PwmMessage pwm = new PwmMessage(time, m);
db.Add(pwm);
break;
case (byte)5: //Gps
m = p.ReadNBytes(13);
time += m[0];
GpsMessage gps = new GpsMessage(time, m);
db.Add(gps);
break;
}
if (i == 10)
{
op.Flush(db);
op.Close();
op = new Output("Logs");
i = 0;
}
i++;
}
}