/// <summary>
/// Makes and exact copy of the adc message
/// </summary>
/// <returns>
/// The copy.
/// </returns>
/// <param name='adc'>
/// The adc message to copy
/// </param>
public static AdcMessage DeepCopy(AdcMessage adc)
{
AdcMessage ans = new AdcMessage();
ans.time = adc.time;
ans.altitude = adc.altitude;
ans.tas = adc.tas - 29; //Offset degut a error en la tensio.
ans.barometer = Field2.DeepCopy(adc.barometer);
ans.thermometer = Field2.DeepCopy(adc.thermometer);
ans.pitot = Field2.DeepCopy(adc.pitot);
return ans;
}
public void SetChThrottle(AdcMessage adcMessage)
{
double vel = adcMessage.tas;
lock(this.pidMutex)
{
if(this.throttlePid != null)
this.throttlePid.SetValue(vel);
}
}
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 int calValueThrottle = 180; //20% span
private static byte ThrottleCalib(PIDManager pid, AdcMessage m)
{
byte ans;
if(m.tas > 12)
{
int meanVal = pid.GetMeanValue(PIDManager.Ctrl.THROTTLE);
double span = pid.GetSpanValue(PIDManager.Ctrl.THROTTLE);
int offset = pid.GetOffsetValue(PIDManager.Ctrl.THROTTLE);
ans = (byte)(((meanVal - calValueThrottle)-offset)/span);
stateThrottle = false;
}
else if(m.tas < 8)
{
int meanVal = pid.GetMeanValue(PIDManager.Ctrl.THROTTLE);
double span = pid.GetSpanValue(PIDManager.Ctrl.THROTTLE);
int offset = pid.GetOffsetValue(PIDManager.Ctrl.THROTTLE);
ans = (byte)(((meanVal + calValueThrottle)-offset)/span);
stateThrottle = true;
}
else if(!stateThrottle)
{
int meanVal = pid.GetMeanValue(PIDManager.Ctrl.THROTTLE);
double span = pid.GetSpanValue(PIDManager.Ctrl.THROTTLE);
int offset = pid.GetOffsetValue(PIDManager.Ctrl.THROTTLE);
ans = (byte)(((meanVal - calValueThrottle)-offset)/span);
}
else
{
int meanVal = pid.GetMeanValue(PIDManager.Ctrl.THROTTLE);
double span = pid.GetSpanValue(PIDManager.Ctrl.THROTTLE);
int offset = pid.GetOffsetValue(PIDManager.Ctrl.THROTTLE);
ans = (byte)(((meanVal + calValueThrottle)-offset)/span);
}
return ans;
}
/// <summary>
/// Initializes a new instance of the <see cref="GroundStation.Database"/> class.
/// </summary>
private Database()
{
this.gpsList = new List<GpsMessage>();
this.adcList = new List<AdcMessage>();
this.imuRawList = new List<ImuRawMessage>();
this.imuEulerList = new List<ImuEulerMessage>();
this.pwmList = new List<PwmMessage>();
this.gps = new GpsMessage();
this.imuEuler = new ImuEulerMessage();
this.imuRaw = new ImuRawMessage();
this.pwm = new PwmMessage();
this.adc = new AdcMessage();
}
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++;
}
}
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++;
}
}
