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++;
}
}
private static void RunPwm()
{
StreamWriter swCal = new StreamWriter("Logs/Calib");
SerialInput si = new SerialInput(path + pwmIn, b57600);
SerialInput so = new SerialInput(path + pwmOut, b57600);
Thread.Sleep(1000);
si.WriteNBytes(new byte[] { 1, 1, 1 });
si.CheckStartOfMessage();
so.WriteNBytes(new byte[] { 1 });
byte[] b = new byte[4];
byte[] ans = new byte[4];
PIDManager pid = PIDManager.GetInstance();
NavManager nav = NavManager.GetInstance();
GlobalArea ga = GlobalArea.GetInstance();
while (true)
{
switch (nav.GetCurrentMode())
{
case NavManager.Mode.MANUAL:
b = si.ReadNBytes(4);
so.WriteNBytes(b);
Console.WriteLine("entrada: " + b[0] + " " + b[1] + " " + b[2] + " " + b[3]);
int[] aux = new int[4];
aux[0] = (int)Math.Round(b[0] * pid.GetSpanValue(PIDManager.Ctrl.THROTTLE) + pid.GetOffsetValue(PIDManager.Ctrl.THROTTLE));
aux[1] = (int)Math.Round(b[1] * pid.GetSpanValue(PIDManager.Ctrl.ROLL) + pid.GetOffsetValue(PIDManager.Ctrl.ROLL));
aux[2] = (int)Math.Round(b[2] * pid.GetSpanValue(PIDManager.Ctrl.PITCH) + pid.GetOffsetValue(PIDManager.Ctrl.PITCH));
aux[3] = (int)Math.Round(b[3] * pid.GetSpanValue(PIDManager.Ctrl.YAW) + pid.GetOffsetValue(PIDManager.Ctrl.YAW));
Console.WriteLine("entrada: " + aux[0] + " " + aux[1] + " " + aux[2] + " " + aux[3]);
pid.SetCh(1, b[0]);
pid.SetCh(2, b[1]);
pid.SetCh(3, b[2]);
pid.SetCh(4, b[3]);
op.WritePwm(time, b);
op.WritePwm(time, b);
break;
case NavManager.Mode.CALIBRATION_THROTTLE:
swCal.WriteLine(time + " " + "throt");
b = si.ReadNBytes(4);
Console.WriteLine("entrada:" + b[0] + " " + b[1] + " " + b[2] + " " + b[3]);
pid.SetCh(1, b[0]);
pid.SetCh(2, b[1]);
pid.SetCh(3, b[2]);
pid.SetCh(4, b[3]);
ans[0] = ThrottleCalib(pid, ga.Adc);
ans[1] = b[1];
ans[2] = b[2];
ans[3] = b[3];
so.WriteNBytes(ans);
Console.WriteLine("sortida:" + ans[0] + " " + ans[1] + " " + ans[2] + " " + ans[3]);
op.WritePwm(time, b);
op.WritePwm(time, ans);
break;
case NavManager.Mode.CALIBRATION_ROLL:
swCal.WriteLine(time + " " + "roll");
b = si.ReadNBytes(4);
Console.WriteLine("entrada:" + b[0] + " " + b[1] + " " + b[2] + " " + b[3]);
pid.SetCh(1, b[0]);
pid.SetCh(2, b[1]);
pid.SetCh(3, b[2]);
pid.SetCh(4, b[3]);
ans[0] = b[0];
ans[1] = RollCalib(pid, ga.Imu);
ans[2] = b[2];
ans[3] = b[3];
so.WriteNBytes(ans);
Console.WriteLine("sortida:" + ans[0] + " " + ans[1] + " " + ans[2] + " " + ans[3]);
op.WritePwm(time, b);
op.WritePwm(time, ans);
break;
case NavManager.Mode.CALIBRATION_PITCH:
swCal.WriteLine(time + " " + "pitch");
b = si.ReadNBytes(4);
Console.WriteLine("entrada:" + b[0] + " " + b[1] + " " + b[2] + " " + b[3]);
pid.SetCh(1, b[0]);
pid.SetCh(2, b[1]);
pid.SetCh(3, b[2]);
pid.SetCh(4, b[3]);
ans[0] = b[0];
ans[1] = b[1];
ans[2] = PitchCalib(pid, ga.Imu);
ans[3] = b[3];
so.WriteNBytes(ans);
Console.WriteLine("sortida:" + ans[0] + " " + ans[1] + " " + ans[2] + " " + ans[3]);
op.WritePwm(time, b);
op.WritePwm(time, ans);
break;
case NavManager.Mode.CALIBRATION_YAW:
swCal.WriteLine(time + " " + "yaw");
b = si.ReadNBytes(4);
Console.WriteLine("entrada:" + b[0] + " " + b[1] + " " + b[2] + " " + b[3]);
pid.SetCh(1, b[0]);
pid.SetCh(2, b[1]);
pid.SetCh(3, b[2]);
pid.SetCh(4, b[3]);
ans[0] = b[0];
ans[1] = b[1];
ans[2] = b[2];
ans[3] = YawCalib(pid, ga.Imu);
Console.WriteLine("sortida:" + ans[0] + " " + ans[1] + " " + ans[2] + " " + ans[3]);
op.WritePwm(time, b);
op.WritePwm(time, ans);
so.WriteNBytes(ans);
break;
default:
b = si.ReadNBytes(4);
Console.WriteLine("entrada:" + b[0] + " " + b[1] + " " + b[2] + " " + b[3]);
pid.SetCh(1, b[0]);
pid.SetCh(2, b[1]);
pid.SetCh(3, b[2]);
pid.SetCh(4, b[3]);
Console.WriteLine("sortida:" + ans[0] + " " + ans[1] + " " + ans[2] + " " + ans[3]);
Console.WriteLine();
ans[0] = pid.GetCh(1);
ans[1] = pid.GetCh(2);
ans[2] = pid.GetCh(3);
ans[3] = pid.GetCh(4);
so.WriteNBytes(ans);
op.WritePwm(time, b);
op.WritePwm(time, ans);
break;
}
}
}