public void SetChYaw(ImuEulerMessage eulerMessage)
{
double yaw = eulerMessage.yaw.V;
lock (this.pidMutex)
{
if (this.yawPid != null)
{
this.yawPid.SetValue(yaw);
}
}
}
public void SetChPitch(ImuEulerMessage eulerMessage)
{
double pitch = eulerMessage.pitch.V;
lock (this.pidMutex)
{
if (this.pitchPid != null)
{
this.pitchPid.SetValue(pitch);
}
}
}
public void SetChRoll(ImuEulerMessage eulerMessage)
{
double roll = eulerMessage.roll.V;
roll = -roll;
lock (this.pidMutex)
{
if (this.rollPid != null)
{
this.rollPid.SetValue(roll);
}
}
}
/// <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();
}
/// <summary>
/// Makes an exact copy of imu
/// </summary>
/// <returns>
/// The copy.
/// </returns>
/// <param name='imu'>
/// The imu message to be copied
/// </param>
public static ImuEulerMessage DeepCopy(ImuEulerMessage imu)
{
ImuEulerMessage ans = new ImuEulerMessage();
ans.time = imu.time;
ans.accelX = Field2.DeepCopy(imu.accelX);
ans.accelY = Field2.DeepCopy(imu.accelY);
ans.accelZ = Field2.DeepCopy(imu.accelZ);
ans.roll = Field2.DeepCopy(imu.roll);
ans.pitch = Field2.DeepCopy(imu.pitch);
ans.yaw = Field2.DeepCopy(imu.yaw);
return(ans);
}
private static byte PitchCalib(PIDManager pid, ImuEulerMessage m)
{
byte ans;
if (m.pitch.V > 2)
{
int meanVal = pid.GetMeanValue(PIDManager.Ctrl.PITCH);
double span = pid.GetSpanValue(PIDManager.Ctrl.PITCH);
int offset = pid.GetOffsetValue(PIDManager.Ctrl.PITCH);
Console.WriteLine("MeanValue: " + meanVal);
Console.WriteLine("Span: " + span);
Console.WriteLine("Offset: " + offset);
ans = (byte)(((meanVal + calValuePitch) - offset) / span);
statePitch = false;
}
else if (m.pitch.V < -2)
{
int meanVal = pid.GetMeanValue(PIDManager.Ctrl.PITCH);
double span = pid.GetSpanValue(PIDManager.Ctrl.PITCH);
int offset = pid.GetOffsetValue(PIDManager.Ctrl.PITCH);
Console.WriteLine("MeanValue: " + meanVal);
Console.WriteLine("Span: " + span);
Console.WriteLine("Offset: " + offset);
ans = (byte)(((meanVal - calValuePitch) - offset) / span);
statePitch = true;
}
else if (!statePitch)
{
int meanVal = pid.GetMeanValue(PIDManager.Ctrl.PITCH);
double span = pid.GetSpanValue(PIDManager.Ctrl.PITCH);
int offset = pid.GetOffsetValue(PIDManager.Ctrl.PITCH);
Console.WriteLine("MeanValue: " + meanVal);
Console.WriteLine("Span: " + span);
Console.WriteLine("Offset: " + offset);
ans = (byte)(((meanVal + calValuePitch) - offset) / span);
}
else
{
int meanVal = pid.GetMeanValue(PIDManager.Ctrl.PITCH);
double span = pid.GetSpanValue(PIDManager.Ctrl.PITCH);
int offset = pid.GetOffsetValue(PIDManager.Ctrl.PITCH);
Console.WriteLine("MeanValue: " + meanVal);
Console.WriteLine("Span: " + span);
Console.WriteLine("Offset: " + offset);
ans = (byte)(((meanVal - calValuePitch) - offset) / span);
}
return(ans);
}
public void Flush(Database db)
{
foreach (Message m in db.gpsList)
{
GpsMessage gpsMess = m as GpsMessage;
//this.gpsUtm.WriteLine(gpsMess.time + "\t" + gpsMess.pos.getUtmX() + "\t" + gpsMess.pos.getUtmY() + "\t" + gpsMess.gndSpeed + "\t" + gpsMess.trackAngle);
//this.gpsGeo.WriteLine(gpsMess.time + "\t" + gpsMess.pos.getLatitude() + "\t" + gpsMess.pos.getLongitude() + "\t" + gpsMess.gndSpeed + "\t" + gpsMess.trackAngle);
}
foreach (Message m in db.imuEulerList)
{
ImuEulerMessage imuMess = m as ImuEulerMessage;
this.imuEuler.WriteLine(imuMess.time + "\t" + imuMess.roll.V + "\t" + imuMess.pitch.V + "\t" + imuMess.yaw.V + "\t" + imuMess.accelX.V + "\t" + imuMess.accelY.V + "\t" + imuMess.accelZ.V);
}
foreach (Message m in db.imuRawList)
{
//ImuRawMessage imuMess = m as ImuRawMessage;
//this.imuRaw.WriteLine(imuMess.time + "\t" + imuMess.accelX + "\t" + imuMess.accelY + "\t" + imuMess.accelZ + "\t" + imuMess.magnetomX + "\t" + imuMess.magnetomY + "\t" + imuMess.magnetomZ);
}
foreach (Message m in db.adcList)
{
AdcMessage adcMess = m as AdcMessage;
this.adc.WriteLine(adcMess.time + "\t" + adcMess.barometer.V + "\t" + adcMess.thermometer.V + "\t" + adcMess.pitot.V + "\t" + adcMess.tas + "\t" + adcMess.altitude);
}
this.imuEuler.Flush();
//this.imuRaw.Flush();
//this.gpsUtm.Flush();
//this.gpsGeo.Flush();
this.adc.Flush();
this.pwm.Flush();
db.Initialize();
}
private static byte YawCalib(PIDManager pid, ImuEulerMessage m)
{
byte ans;
Console.WriteLine("INITIAL VAL: " + pid.GetParam(PIDManager.Ctrl.YAW, PID.Param.INITIAL_VAL));
if ((m.yaw.V - pid.GetParam(PIDManager.Ctrl.YAW, PID.Param.INITIAL_VAL)) > 2)
{
int meanVal = pid.GetMeanValue(PIDManager.Ctrl.YAW);
double span = pid.GetSpanValue(PIDManager.Ctrl.YAW);
int offset = pid.GetOffsetValue(PIDManager.Ctrl.YAW);
ans = (byte)(((meanVal - calValueYaw) - offset) / span);
stateYaw = true;
}
else if (m.yaw.V - pid.GetParam(PIDManager.Ctrl.YAW, PID.Param.INITIAL_VAL) < -2)
{
int meanVal = pid.GetMeanValue(PIDManager.Ctrl.YAW);
double span = pid.GetSpanValue(PIDManager.Ctrl.YAW);
int offset = pid.GetOffsetValue(PIDManager.Ctrl.YAW);
ans = (byte)(((meanVal + calValueYaw) - offset) / span);
stateYaw = false;
}
else if (!stateYaw)
{
int meanVal = pid.GetMeanValue(PIDManager.Ctrl.YAW);
double span = pid.GetSpanValue(PIDManager.Ctrl.YAW);
int offset = pid.GetOffsetValue(PIDManager.Ctrl.YAW);
ans = (byte)(((meanVal - calValueYaw) - offset) / span);
}
else
{
int meanVal = pid.GetMeanValue(PIDManager.Ctrl.YAW);
double span = pid.GetSpanValue(PIDManager.Ctrl.YAW);
int offset = pid.GetOffsetValue(PIDManager.Ctrl.YAW);
ans = (byte)(((meanVal - calValueYaw) - offset) / span);
}
return(ans);
}
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++;
}
}
public void SetChPitch(ImuEulerMessage eulerMessage)
{
double pitch = eulerMessage.pitch.V;
lock(this.pidMutex)
{
if(this.pitchPid != null)
this.pitchPid.SetValue(pitch);
}
}
private static byte YawCalib(PIDManager pid, ImuEulerMessage m)
{
byte ans;
Console.WriteLine("INITIAL VAL: " + pid.GetParam(PIDManager.Ctrl.YAW, PID.Param.INITIAL_VAL));
if((m.yaw.V - pid.GetParam(PIDManager.Ctrl.YAW, PID.Param.INITIAL_VAL)) > 2)
{
int meanVal = pid.GetMeanValue(PIDManager.Ctrl.YAW);
double span = pid.GetSpanValue(PIDManager.Ctrl.YAW);
int offset = pid.GetOffsetValue(PIDManager.Ctrl.YAW);
ans = (byte)(((meanVal - calValueYaw)-offset)/span);
stateYaw = true;
}
else if(m.yaw.V - pid.GetParam(PIDManager.Ctrl.YAW, PID.Param.INITIAL_VAL) < -2)
{
int meanVal = pid.GetMeanValue(PIDManager.Ctrl.YAW);
double span = pid.GetSpanValue(PIDManager.Ctrl.YAW);
int offset = pid.GetOffsetValue(PIDManager.Ctrl.YAW);
ans = (byte)(((meanVal + calValueYaw)-offset)/span);
stateYaw = false;
}
else if(!stateYaw)
{
int meanVal = pid.GetMeanValue(PIDManager.Ctrl.YAW);
double span = pid.GetSpanValue(PIDManager.Ctrl.YAW);
int offset = pid.GetOffsetValue(PIDManager.Ctrl.YAW);
ans = (byte)(((meanVal - calValueYaw)-offset)/span);
}
else
{
int meanVal = pid.GetMeanValue(PIDManager.Ctrl.YAW);
double span = pid.GetSpanValue(PIDManager.Ctrl.YAW);
int offset = pid.GetOffsetValue(PIDManager.Ctrl.YAW);
ans = (byte)(((meanVal - calValueYaw)-offset)/span);
}
return ans;
}
private static byte PitchCalib(PIDManager pid, ImuEulerMessage m)
{
byte ans;
if(m.pitch.V > 2)
{
int meanVal = pid.GetMeanValue(PIDManager.Ctrl.PITCH);
double span = pid.GetSpanValue(PIDManager.Ctrl.PITCH);
int offset = pid.GetOffsetValue(PIDManager.Ctrl.PITCH);
Console.WriteLine("MeanValue: " + meanVal);
Console.WriteLine("Span: " + span);
Console.WriteLine("Offset: " + offset);
ans = (byte)(((meanVal + calValuePitch)-offset)/span);
statePitch = false;
}
else if(m.pitch.V < -2)
{
int meanVal = pid.GetMeanValue(PIDManager.Ctrl.PITCH);
double span = pid.GetSpanValue(PIDManager.Ctrl.PITCH);
int offset = pid.GetOffsetValue(PIDManager.Ctrl.PITCH);
Console.WriteLine("MeanValue: " + meanVal);
Console.WriteLine("Span: " + span);
Console.WriteLine("Offset: " + offset);
ans = (byte)(((meanVal - calValuePitch)-offset)/span);
statePitch = true;
}
else if(!statePitch)
{
int meanVal = pid.GetMeanValue(PIDManager.Ctrl.PITCH);
double span = pid.GetSpanValue(PIDManager.Ctrl.PITCH);
int offset = pid.GetOffsetValue(PIDManager.Ctrl.PITCH);
Console.WriteLine("MeanValue: " + meanVal);
Console.WriteLine("Span: " + span);
Console.WriteLine("Offset: " + offset);
ans = (byte)(((meanVal + calValuePitch)-offset)/span);
}
else
{
int meanVal = pid.GetMeanValue(PIDManager.Ctrl.PITCH);
double span = pid.GetSpanValue(PIDManager.Ctrl.PITCH);
int offset = pid.GetOffsetValue(PIDManager.Ctrl.PITCH);
Console.WriteLine("MeanValue: " + meanVal);
Console.WriteLine("Span: " + span);
Console.WriteLine("Offset: " + offset);
ans = (byte)(((meanVal - calValuePitch)-offset)/span);
}
return ans;
}
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++;
}
}
/// <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++;
}
}
public void SetChRoll(ImuEulerMessage eulerMessage)
{
double roll = eulerMessage.roll.V;
roll = -roll;
lock(this.pidMutex)
{
if(this.rollPid != null)
this.rollPid.SetValue(roll);
}
}
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++;
}
}
public void SetChYaw(ImuEulerMessage eulerMessage)
{
double yaw = eulerMessage.yaw.V;
lock(this.pidMutex)
{
if(this.yawPid != null)
this.yawPid.SetValue(yaw);
}
}
/// <summary>
/// Makes an exact copy of imu
/// </summary>
/// <returns>
/// The copy.
/// </returns>
/// <param name='imu'>
/// The imu message to be copied
/// </param>
public static ImuEulerMessage DeepCopy(ImuEulerMessage imu)
{
ImuEulerMessage ans = new ImuEulerMessage();
ans.time = imu.time;
ans.accelX = Field2.DeepCopy(imu.accelX);
ans.accelY = Field2.DeepCopy(imu.accelY);
ans.accelZ = Field2.DeepCopy(imu.accelZ);
ans.roll = Field2.DeepCopy(imu.roll);
ans.pitch = Field2.DeepCopy(imu.pitch);
ans.yaw = Field2.DeepCopy(imu.yaw);
return ans;
}
