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; }