commit 38e94e2d80d6e1e95a025c7a0fa17986eece6206 Author: Nonannet Date: Wed Dec 5 10:50:00 2018 +0100 Add files via upload diff --git a/Hardware/BMP085_Barometer.cs b/Hardware/BMP085_Barometer.cs new file mode 100644 index 0000000..d2b38b7 --- /dev/null +++ b/Hardware/BMP085_Barometer.cs @@ -0,0 +1,138 @@ +using System; +using Microsoft.SPOT; +using Microsoft.SPOT.Hardware; +using GHIElectronics.NETMF.System; + +namespace PlaneOnBoardSoftware +{ + /// + /// Absolute Pressure + /// + public class BMP085_Barometer : I2CBase + { + private Int16 AC1, AC2, AC3, B1, B2, MB, MC, MD; + private UInt16 AC4, AC5, AC6; + + private byte OS = 3; + private Int32 UT = 0; + + public float Temperature = 0; // in Celcius + public float Pressure = 0; // in Pa + public float Altitude = 0; //in m + public float OffSetAltitude = 0; //in m + + private I2CDevice.Configuration _config; + + private int stCount = 0; + + public BMP085_Barometer() + { + _config = new I2CDevice.Configuration(0x77, 400); //68 + + readCalibrationData(); + } + + public void RequestSensorData() + { + stCount++; + + if (stCount == 1) + { + Write(0xF4, 0x2E); //Start Temperature Measurement + } + else if (stCount == 2) + { + ReadUtMeasurement(); //Read UT Temperature + } + else if (stCount == 3) + { + Write(0xF4, 0x34 + 0xC0); //Start Pressure Measurement with oversampling = 3 (0xC0 = 3*2^6) + } + else + { + ReadMeasurement(); + stCount = 0; + } + } + + private void ReadUtMeasurement() + { + //Temperatur + byte[] rawData = new byte[2]; + Read(0xF6, rawData); + UT = (Int32)((Int32)(rawData[0] << 8) + (Int32)(rawData[1])); + } + + private void ReadMeasurement() + { + Int32 X1, X2, B5, B6, X3, B3, P, UP; + UInt32 B4, B7; + + //Temperatur + X1 = (UT - AC6) * AC5 >> 15; + + if (X1 + MD > 0) + { + X2 = ((Int32)MC << 11) / (X1 + MD); + B5 = X1 + X2; + Temperature = ((float)((B5 + 8) >> 4)) / 10; + + //Pressure + byte[] rawData2 = new byte[3]; + Read(0xF6, rawData2); + UP = (((Int32)(rawData2[0]) << 16) + ((Int32)(rawData2[1]) << 8) + (Int32)rawData2[2]) >> (8 - OS); + + B6 = B5 - 4000; + X1 = (B2 * (B6 * B6 >> 12)) >> 11; + X2 = AC2 * B6 >> 11; + X3 = X1 + X2; + B3 = ((((Int32)AC1 * 4 + X3) << OS) + 2) >> 2; + X1 = AC3 * B6 >> 13; + X2 = (B1 * (B6 * B6 >> 12)) >> 16; + X3 = ((X1 + X2) + 2) >> 2; + B4 = AC4 * (UInt32)(X3 + 32768) >> 15; + B7 = ((UInt32)(UP - B3)) * (UInt32)(50000 >> OS); + P = (B7 < 0x80000000) ? (Int32)((B7 * 2) / B4) : (Int32)((B7 / B4) * 2); + X1 = (P >> 8) * (P >> 8); + X1 = (X1 * 3038) >> 16; + X2 = (-7357 * P) >> 16; + Pressure = P + ((X1 + X2 + 3791) >> 4); + } + else + { + Pressure = 100000; + } + } + + private void CalculateAltitude() + { + Altitude = OffSetAltitude + (float)(44330 * (1 - MathEx.Pow(Pressure / 101325, 0.1903))); + } + + private void readCalibrationData() + { + byte[] rawData = new byte[22]; + + Read(0xAA, rawData); + + AC1 = (short)((short)(rawData[0] << 8) + (short)(rawData[1])); + AC2 = (short)((short)(rawData[2] << 8) + (short)(rawData[3])); + AC3 = (short)((short)(rawData[4] << 8) + (short)(rawData[5])); + AC4 = (ushort)((ushort)(rawData[6] << 8) + (ushort)(rawData[7])); + AC5 = (ushort)((ushort)(rawData[8] << 8) + (ushort)(rawData[9])); + AC6 = (ushort)((ushort)(rawData[10] << 8) + (ushort)(rawData[11])); + B1 = (short)((short)(rawData[12] << 8) + (short)(rawData[13])); + B2 = (short)((short)(rawData[14] << 8) + (short)(rawData[15])); + MB = (short)((short)(rawData[16] << 8) + (short)(rawData[17])); + MC = (short)((short)(rawData[18] << 8) + (short)(rawData[19])); + MD = (short)((short)(rawData[20] << 8) + (short)(rawData[21])); + + //if (AC1 * AC2 * AC3 * AC4 * AC5 * AC6 * B1 * B2 * MB * MC * MD == 0) return; + } + + public override I2CDevice.Configuration Config + { + get { return _config; } + } + } +} diff --git a/Hardware/BatteryMonitor.cs b/Hardware/BatteryMonitor.cs new file mode 100644 index 0000000..6aecc3d --- /dev/null +++ b/Hardware/BatteryMonitor.cs @@ -0,0 +1,60 @@ +using System; +using Microsoft.SPOT; +using Microsoft.SPOT.Hardware; +using GHIElectronics.NETMF.Hardware; + +namespace PlaneOnBoardSoftware +{ + class BatteryMonitor + { + AnalogIn aiPortU; + AnalogIn aiPortI; + + static float CurrentOffSet = 1.477f; + static float ShuntResistens = 0.005776f; + static float ShuntGain = 100.0f; + static float CurrFactor = 1 / ShuntGain / ShuntResistens; + float _voltage; + float _current; + double _charge = 0; + double _energy = 0; + + DateTime lastTimePoint = DateTime.Now; + + public float Voltage { get { return _voltage; } } + public float Current { get { return _current; } } + public float Charge { get { return (float)_charge; } } + public float Energy { get { return (float)_energy; } } + + public BatteryMonitor(AnalogIn VoltageAnalogInPort, AnalogIn CurrentAnalogInPort) + { + aiPortU = VoltageAnalogInPort; + aiPortI = CurrentAnalogInPort; + + aiPortU.SetLinearScale(0, 4861); //mV (4.8V = 3.3V*(22kOhm+10kOhm)/22kOhm), 4.861V <- calibration + aiPortI.SetLinearScale(0, 3300); //mV + } + + public void UpdateData() + { + float dt; + + DateTime newTime = DateTime.UtcNow; + dt = newTime.Subtract(lastTimePoint).Ticks / (float)TimeSpan.TicksPerSecond; + lastTimePoint = newTime; + + _voltage = aiPortU.Read() * 0.001f; //in V + + if (_voltage > 2.0f) + _current = (CurrentOffSet - aiPortI.Read() * 0.001f) * CurrFactor; //in A (1 V/A) + else + _current = 0; + + if (dt > 0 && dt < 10) + { + _charge += _current * dt / 3.6f; + _energy += _current * _voltage * dt; + } + } + } +} diff --git a/Hardware/DS18B20_Temperature.cs b/Hardware/DS18B20_Temperature.cs new file mode 100644 index 0000000..db8b1db --- /dev/null +++ b/Hardware/DS18B20_Temperature.cs @@ -0,0 +1,88 @@ +using System; +using Microsoft.SPOT; +using GHIElectronics.NETMF.Hardware; +using Microsoft.SPOT.Hardware; +using System.Threading; +using GHIElectronics.NETMF.FEZ; + +namespace PlaneOnBoardSoftware +{ + class DS18B20_Temperature + { + private const byte SearchROM = 0xF0; + private const byte ReadROM = 0x33; + private const byte MatchROM = 0x55; + private const byte SkipROM = 0xCC; + private const byte AlarmSearch = 0xEC; + private const byte StartTemperatureConversion = 0x44; + private const byte ReadScratchPad = 0xBE; + private const byte WriteScratchPad = 0x4E; + private const byte CopySratchPad = 0x48; + private const byte RecallEEPROM = 0xB8; + private const byte ReadPowerSupply = 0xB4; + + private OneWire DataPin; + + public float Temperature1 = -99; + public float Temperature2 = -99; + + private bool ConvStarted = false; + + private byte[][] IDs = new byte[][] { new byte[8], new byte[8], new byte[8] }; + + public DS18B20_Temperature(Cpu.Pin pin) + { + DataPin = new OneWire(pin); + + DataPin.Search_Restart(); + int i = 0; + while (DataPin.Search_GetNextDevice(IDs[i++])) + { + Debug.Print("Found thermometer" + i); + } + } + + public void ReadTemperature() + { + long data = 0; + + if (DataPin.ReadByte() > 0 && ConvStarted) + { + //Read temperature1 + DataPin.Reset(); + DataPin.WriteByte(MatchROM); + DataPin.Write(IDs[0], 0, 8); + DataPin.WriteByte(ReadScratchPad); + + data = DataPin.ReadByte(); // LSB + data |= (ushort)(DataPin.ReadByte() << 8); // MSB + Temperature1 = data * 0.0625f; + + //Read temperature2 + DataPin.Reset(); + DataPin.WriteByte(MatchROM); + DataPin.Write(IDs[1], 0, 8); + DataPin.WriteByte(ReadScratchPad); + + data = DataPin.ReadByte(); // LSB + data |= (ushort)(DataPin.ReadByte() << 8); // MSB + Temperature2 = data * 0.0625f; + + + ConvStarted = false; + } + + DataPin.Reset(); + DataPin.WriteByte(MatchROM); + DataPin.Write(IDs[0], 0, 8); + DataPin.WriteByte(StartTemperatureConversion); + + DataPin.Reset(); + DataPin.WriteByte(MatchROM); + DataPin.Write(IDs[1], 0, 8); + DataPin.WriteByte(StartTemperatureConversion); + + ConvStarted = true; + } + } +} \ No newline at end of file diff --git a/Hardware/FEZ_Low_Level.cs b/Hardware/FEZ_Low_Level.cs new file mode 100644 index 0000000..14fc4c4 --- /dev/null +++ b/Hardware/FEZ_Low_Level.cs @@ -0,0 +1,23 @@ +using System; +using GHIElectronics.NETMF.Hardware.LowLevel; +using System.IO.Ports; + +namespace PlaneOnBoardSoftware +{ + class FEZ_Low_Level + { + // add this function anywhere + static public void RemapCOM4to_TXAn2_RXAn3(SerialPort ser) + { + // call this function **after** you open COM4 port + if (ser.PortName != "COM4" || ser.IsOpen == false) + throw new Exception("Only use COM4 and make sure it is open"); + // remap COM4 RX (in) pin from P4.29/DIO17 to P0.26 (that is An3) + // remap COM4 TX (out) pin from P4.28/DIO13 to P0.25 (that is An2) + Register PINSEL9 = new Register(0xE002C024); + PINSEL9.Write(0);// COM4 is now disconnected from P4.28 and P4.29 + Register PINSEL1 = new Register(0xE002C004); + PINSEL1.SetBits(0xf << 18);// COM4 is now connected to An3 and An4 + } + } +} diff --git a/Hardware/I2CBase.cs b/Hardware/I2CBase.cs new file mode 100644 index 0000000..0a4f45c --- /dev/null +++ b/Hardware/I2CBase.cs @@ -0,0 +1,48 @@ +using System; +using Microsoft.SPOT; +using Microsoft.SPOT.Hardware; + +namespace PlaneOnBoardSoftware +{ + public abstract class I2CBase + { + public abstract I2CDevice.Configuration Config { get; } + + protected byte Read(byte address) + { + Byte[] data = new Byte[1]; + I2CDevice.I2CWriteTransaction write = I2CDevice.CreateWriteTransaction(new Byte[] { address }); + I2CDevice.I2CReadTransaction read = I2CDevice.CreateReadTransaction(data); + I2CDevice.I2CTransaction[] readTransaction = new I2CDevice.I2CTransaction[] { write, read }; + + var result = I2CBus.Execute(Config, readTransaction, 1000); + + return data[0]; + } + + protected float CombineBytes(byte high, byte low) + { + var data = (Int16)((high << 8) | low); + + return (float)data; + } + + protected byte Read(byte address, byte[] buffer) + { + I2CDevice.I2CWriteTransaction write = I2CDevice.CreateWriteTransaction(new Byte[] { address }); + I2CDevice.I2CReadTransaction read = I2CDevice.CreateReadTransaction(buffer); + I2CDevice.I2CTransaction[] readTransaction = new I2CDevice.I2CTransaction[] { write, read }; + + var result = I2CBus.Execute(Config, readTransaction, 1000); + + return buffer[0]; + } + + protected void Write(byte address, byte value) + { + var bytes = new byte[] { address, value }; + + I2CBus.Write(Config, bytes, 1000); + } + } +} diff --git a/Hardware/I2CBus.cs b/Hardware/I2CBus.cs new file mode 100644 index 0000000..a94c021 --- /dev/null +++ b/Hardware/I2CBus.cs @@ -0,0 +1,39 @@ +using System; +using Microsoft.SPOT; +using Microsoft.SPOT.Hardware; + +namespace PlaneOnBoardSoftware +{ + public static class I2CBus + { + private const int Timeout = 1000; + private static I2CDevice Device { get; set; } + + static I2CBus() + { + Device = new I2CDevice(new I2CDevice.Configuration(0x00, 400)); + } + + public static void Write(I2CDevice.Configuration configuration, byte[] bytes, int timeout = Timeout) + { + var actions = new I2CDevice.I2CTransaction[] { + I2CDevice.CreateWriteTransaction(bytes) + }; + + var result = Execute(configuration, actions); + if (result != bytes.Length) + { + //throw new ApplicationException("Expected to write " + bytes.Length + " bytes but only wrote " + result + " bytes"); + } + } + + public static int Execute(I2CDevice.Configuration configuration, I2CDevice.I2CTransaction[] actions, int timeout = Timeout) + { + lock (Device) + { + Device.Config = configuration; + return Device.Execute(actions, timeout); + } + } + } +} diff --git a/Hardware/Igniter.cs b/Hardware/Igniter.cs new file mode 100644 index 0000000..882cfa7 --- /dev/null +++ b/Hardware/Igniter.cs @@ -0,0 +1,27 @@ +using System; +using Microsoft.SPOT; +using Microsoft.SPOT.IO; +using Microsoft.SPOT.Hardware; +using System.Threading; + +namespace PlaneOnBoardSoftware +{ + class Igniter + { + private int onTime; + private OutputPort igniterPort; + + public Igniter(Cpu.Pin PortPin, int OnTime) + { + igniterPort = new OutputPort(PortPin, false); + this.onTime = OnTime; + } + + public void Fire() + { + igniterPort.Write(true); + Thread.Sleep(onTime); + igniterPort.Write(false);//<-temp + } + } +} diff --git a/Hardware/L3G4200D_Gyro.cs b/Hardware/L3G4200D_Gyro.cs new file mode 100644 index 0000000..357d3f7 --- /dev/null +++ b/Hardware/L3G4200D_Gyro.cs @@ -0,0 +1,145 @@ +using System; +using Microsoft.SPOT; +using Microsoft.SPOT.Hardware; + +namespace PlaneOnBoardSoftware +{ + /// + /// Accelerometer + /// + public class L3G4200D_Gyro : I2CBase + { + private class Register + { + public const byte CTRL_REG1 = 0x20; + public const byte OUT_TEMP = 0x26; + public const byte OUT_X_L_A = 0x28; + public const byte OUT_X_H_A = 0x29; + public const byte OUT_Y_L_A = 0x2a; + public const byte OUT_Y_H_A = 0x2b; + public const byte OUT_Z_L_A = 0x2c; + public const byte OUT_Z_H_A = 0x2d; + } + + private I2CDevice.Configuration _config; + private const float _scale = 250f / 0x8000 * 1.2f; + private bool _PowerDown = false; + + private static float calXoffSet = -0.238403308f; + private static float calYoffSet = -0.572611464f; + private static float calZoffSet = +0.439249654f; + + public Vector3D AngularRate = new Vector3D(); + + public L3G4200D_Gyro() + { + _config = new I2CDevice.Configuration(0x69, 400); //68 + + // 0x1F = 00011111 Normal mode + Write(Register.CTRL_REG1, 0x1F); + } + + private float GetAngularRate(byte high, byte low) + { + var data = (Int16)((Read(high) << 8) | Read(low)); + + return (((float)(data)) / _scale); + } + + public bool PowerDown + { + get + { + return _PowerDown; + } + set + { + if (value) + Write(Register.CTRL_REG1, 0x17); // 0x17 = 00010111 Power-down + else + Write(Register.CTRL_REG1, 0x1F); // 0x1F = 00011111 Normal mode + + _PowerDown = value; + } + } + + public float GetTemperature() + { + return (sbyte)(Read(Register.OUT_TEMP)); + } + + public int RequestAngularRate() + { + byte[] bufferXhi = new byte[1]; + byte[] bufferYhi = new byte[1]; + byte[] bufferZhi = new byte[1]; + byte[] bufferXlo = new byte[1]; + byte[] bufferYlo = new byte[1]; + byte[] bufferZlo = new byte[1]; + I2CDevice.I2CTransaction[] readTransaction = new I2CDevice.I2CTransaction[12]; + I2CDevice.I2CWriteTransaction write; + I2CDevice.I2CReadTransaction read; + + write = I2CDevice.CreateWriteTransaction(new Byte[] { Register.OUT_X_H_A }); + read = I2CDevice.CreateReadTransaction(bufferXhi); + readTransaction[0] = write; + readTransaction[1] = read; + + write = I2CDevice.CreateWriteTransaction(new Byte[] { Register.OUT_X_L_A }); + read = I2CDevice.CreateReadTransaction(bufferXlo); + readTransaction[2] = write; + readTransaction[3] = read; + + write = I2CDevice.CreateWriteTransaction(new Byte[] { Register.OUT_Y_H_A }); + read = I2CDevice.CreateReadTransaction(bufferYhi); + readTransaction[4] = write; + readTransaction[5] = read; + + write = I2CDevice.CreateWriteTransaction(new Byte[] { Register.OUT_Y_L_A }); + read = I2CDevice.CreateReadTransaction(bufferYlo); + readTransaction[6] = write; + readTransaction[7] = read; + + write = I2CDevice.CreateWriteTransaction(new Byte[] { Register.OUT_Z_H_A }); + read = I2CDevice.CreateReadTransaction(bufferZhi); + readTransaction[8] = write; + readTransaction[9] = read; + + write = I2CDevice.CreateWriteTransaction(new Byte[] { Register.OUT_Z_L_A }); + read = I2CDevice.CreateReadTransaction(bufferZlo); + readTransaction[10] = write; + readTransaction[11] = read; + + var result = I2CBus.Execute(Config, readTransaction, 1000); + + AngularRate.X = -CombineBytes(bufferXhi[0], bufferXlo[0]) * _scale + calXoffSet; + AngularRate.Y = -CombineBytes(bufferYhi[0], bufferYlo[0]) * _scale + calYoffSet; + AngularRate.Z = CombineBytes(bufferZhi[0], bufferZlo[0]) * _scale + calZoffSet; + + return result; + } + + + + public float GetX() + { + return GetAngularRate(Register.OUT_X_H_A, Register.OUT_X_L_A) + calXoffSet; + } + + public float GetY() + { + return GetAngularRate(Register.OUT_Y_H_A, Register.OUT_Y_L_A) + calYoffSet; +; + } + + public float GetZ() + { + return GetAngularRate(Register.OUT_Z_H_A, Register.OUT_Z_L_A) + calZoffSet; + } + + public override I2CDevice.Configuration Config + { + get { return _config; } + } + } +} diff --git a/Hardware/LSM303DLM_Accelerometer.cs b/Hardware/LSM303DLM_Accelerometer.cs new file mode 100644 index 0000000..5a1575a --- /dev/null +++ b/Hardware/LSM303DLM_Accelerometer.cs @@ -0,0 +1,130 @@ +using System; +using Microsoft.SPOT; +using Microsoft.SPOT.Hardware; + +namespace PlaneOnBoardSoftware +{ + public class LSM303DLM_Accelerometer : I2CBase + { + private class Register + { + public const byte CTRL_REG1_A = 0x20; + public const byte OUT_X_L_A = 0x28; + public const byte OUT_X_H_A = 0x29; + public const byte OUT_Y_L_A = 0x2a; + public const byte OUT_Y_H_A = 0x2b; + public const byte OUT_Z_L_A = 0x2c; + public const byte OUT_Z_H_A = 0x2d; + } + + private I2CDevice.Configuration _config; + private const float _scale = 1 / (0x4000 * 9.81f); + private bool _PowerDown = false; + + public Vector3D Acceleration = new Vector3D(); + + public LSM303DLM_Accelerometer() + { + _config = new I2CDevice.Configuration(0x18, 400); + + // 0x27 = 00100111 Normal mode + Write(Register.CTRL_REG1_A, 0x27); + } + + public bool PowerDown + { + get + { + return _PowerDown; + } + set + { + if (value) + Write(Register.CTRL_REG1_A, 0x07); // 0x07 = 00000111 Power-down + else + Write(Register.CTRL_REG1_A, 0x27); // 0x27 = 00100111 Normal mode + + _PowerDown = value; + } + } + + private float GetAccelerationInGs(byte high, byte low) + { + // 12 bit - left justified + var data = (Int16)((Read(high) << 8) | Read(low) >> 4); + + return (((float)(data)) / _scale); + } + + public int RequesAcceleration() + { + byte[] bufferXhi = new byte[1]; + byte[] bufferYhi = new byte[1]; + byte[] bufferZhi = new byte[1]; + byte[] bufferXlo = new byte[1]; + byte[] bufferYlo = new byte[1]; + byte[] bufferZlo = new byte[1]; + I2CDevice.I2CTransaction[] readTransaction = new I2CDevice.I2CTransaction[12]; + I2CDevice.I2CWriteTransaction write; + I2CDevice.I2CReadTransaction read; + + write = I2CDevice.CreateWriteTransaction(new Byte[] { Register.OUT_X_H_A }); + read = I2CDevice.CreateReadTransaction(bufferXhi); + readTransaction[0] = write; + readTransaction[1] = read; + + write = I2CDevice.CreateWriteTransaction(new Byte[] { Register.OUT_X_L_A }); + read = I2CDevice.CreateReadTransaction(bufferXlo); + readTransaction[2] = write; + readTransaction[3] = read; + + write = I2CDevice.CreateWriteTransaction(new Byte[] { Register.OUT_Y_H_A }); + read = I2CDevice.CreateReadTransaction(bufferYhi); + readTransaction[4] = write; + readTransaction[5] = read; + + write = I2CDevice.CreateWriteTransaction(new Byte[] { Register.OUT_Y_L_A }); + read = I2CDevice.CreateReadTransaction(bufferYlo); + readTransaction[6] = write; + readTransaction[7] = read; + + write = I2CDevice.CreateWriteTransaction(new Byte[] { Register.OUT_Z_H_A }); + read = I2CDevice.CreateReadTransaction(bufferZhi); + readTransaction[8] = write; + readTransaction[9] = read; + + write = I2CDevice.CreateWriteTransaction(new Byte[] { Register.OUT_Z_L_A }); + read = I2CDevice.CreateReadTransaction(bufferZlo); + readTransaction[10] = write; + readTransaction[11] = read; + + var result = I2CBus.Execute(Config, readTransaction, 1000); + + Acceleration.X = -CombineBytes(bufferXhi[0], bufferXlo[0]) * _scale; + Acceleration.Y = -CombineBytes(bufferYhi[0], bufferYlo[0]) * _scale; + Acceleration.Z = CombineBytes(bufferZhi[0], bufferZlo[0]) * _scale; + + return result; + } + + public float GetX() + { + return GetAccelerationInGs(Register.OUT_X_H_A, Register.OUT_X_L_A); + } + + public float GetY() + { + return GetAccelerationInGs(Register.OUT_Y_H_A, Register.OUT_Y_L_A); + } + + public float GetZ() + { + return GetAccelerationInGs(Register.OUT_Z_H_A, Register.OUT_Z_L_A); + } + + public override I2CDevice.Configuration Config + { + get { return _config; } + } + } +} diff --git a/Hardware/LSM303DLM_Magnetometer.cs b/Hardware/LSM303DLM_Magnetometer.cs new file mode 100644 index 0000000..7ed896a --- /dev/null +++ b/Hardware/LSM303DLM_Magnetometer.cs @@ -0,0 +1,193 @@ +using System; +using Microsoft.SPOT; +using Microsoft.SPOT.Hardware; + +namespace PlaneOnBoardSoftware +{ + public class LSM303DLM_Magnetometer : I2CBase + { + private class Register + { + public const byte CRA_REG_M = 0x00; + public const byte CRB_REG_M = 0x01; + public const byte MR_REG_M = 0x02; + public const byte OUT_X_L_M = 0x04; + public const byte OUT_X_H_M = 0x03; + public const byte OUT_Y_L_M = 0x06; + public const byte OUT_Y_H_M = 0x05; + public const byte OUT_Z_L_M = 0x08; + public const byte OUT_Z_H_M = 0x07; + } + + private I2CDevice.Configuration _config; + private bool _PowerDown = false; + + public float CalXoffSet = 0; + public float CalYoffSet = 0; + public float CalZoffSet = 0; + public float CalXscale = 1f / 320; + public float CalYscale = 1f / 320; + public float CalZscale = 1f / 320; + + private float maxX = 0; + private float minX = 0; + private float maxY = 0; + private float minY = 0; + private float maxZ = 0; + private float minZ = 0; + private bool _calibrationmode = false; + + public Vector3D MagValue = new Vector3D(); + + public bool CalibrationMode + { + get { return _calibrationmode; } + set + { + _calibrationmode = value; + if (_calibrationmode) + { + maxX = 0; + minX = 0; + maxY = 0; + minY = 0; + maxZ = 0; + minZ = 0; + } + else + { + CalXoffSet = (maxX + minX) / 2; + CalYoffSet = (maxY + minY) / 2; + CalZoffSet = (maxZ + minZ) / 2; + CalXscale = 2f / (maxX - minX); + CalYscale = 2f / (maxY - minY); + CalZscale = 2f / (maxZ - minZ); + } + } + } + + public LSM303DLM_Magnetometer() + { + _config = new I2CDevice.Configuration(0x1E, 400); + + // 0x10 = 00010000 -> 15 Hz update rate + Write(Register.CRA_REG_M, 0x10); + + // 0xC0 = 11000000 -> Gain: ±5.6 Gauss, 320 LSB/Gauss + Write(Register.CRB_REG_M, 0xC0); + + // 0x00 = 00000000 Normal mode + Write(Register.MR_REG_M, 0x00); + } + + public bool PowerDown + { + get + { + return _PowerDown; + } + set + { + if (value) + Write(Register.MR_REG_M, 0x03); // 0x03 = 00000011 Power-down + else + Write(Register.MR_REG_M, 0x00); // 0x00 = 00000000 Normal mode + + _PowerDown = value; + } + } + + private float GetMagValue(byte high, byte low) + { + var data = (Int16)((Read(high) << 8) | Read(low)); + + return (((float)(data))); + } + + public int RequestMagValue() + { + float x, y, z; + byte[] bufferXhi = new byte[1]; + byte[] bufferYhi = new byte[1]; + byte[] bufferZhi = new byte[1]; + byte[] bufferXlo = new byte[1]; + byte[] bufferYlo = new byte[1]; + byte[] bufferZlo = new byte[1]; + I2CDevice.I2CTransaction[] readTransaction = new I2CDevice.I2CTransaction[12]; + I2CDevice.I2CWriteTransaction write; + I2CDevice.I2CReadTransaction read; + + write = I2CDevice.CreateWriteTransaction(new Byte[] { Register.OUT_X_H_M }); + read = I2CDevice.CreateReadTransaction(bufferXhi); + readTransaction[0] = write; + readTransaction[1] = read; + + write = I2CDevice.CreateWriteTransaction(new Byte[] { Register.OUT_X_L_M }); + read = I2CDevice.CreateReadTransaction(bufferXlo); + readTransaction[2] = write; + readTransaction[3] = read; + + write = I2CDevice.CreateWriteTransaction(new Byte[] { Register.OUT_Y_H_M }); + read = I2CDevice.CreateReadTransaction(bufferYhi); + readTransaction[4] = write; + readTransaction[5] = read; + + write = I2CDevice.CreateWriteTransaction(new Byte[] { Register.OUT_Y_L_M }); + read = I2CDevice.CreateReadTransaction(bufferYlo); + readTransaction[6] = write; + readTransaction[7] = read; + + write = I2CDevice.CreateWriteTransaction(new Byte[] { Register.OUT_Z_H_M }); + read = I2CDevice.CreateReadTransaction(bufferZhi); + readTransaction[8] = write; + readTransaction[9] = read; + + write = I2CDevice.CreateWriteTransaction(new Byte[] { Register.OUT_Z_L_M }); + read = I2CDevice.CreateReadTransaction(bufferZlo); + readTransaction[10] = write; + readTransaction[11] = read; + + var result = I2CBus.Execute(Config, readTransaction, 1000); + + x = CombineBytes(bufferXhi[0], bufferXlo[0]); + z = CombineBytes(bufferYhi[0], bufferYlo[0]); // Y <--> Z Exchanged + y = CombineBytes(bufferZhi[0], bufferZlo[0]); // Z <--> Y Exchanged + + if (_calibrationmode) + { + if (x > maxX) maxX = x; + if (x < minX) minX = x; + if (y > maxY) maxY = y; + if (y < minY) minY = y; + if (z > maxZ) maxZ = z; + if (z < minZ) minZ = z; + } + + MagValue.X = -(x - CalXoffSet) * CalXscale; + MagValue.Z = (z - CalZoffSet) * CalZscale; // Y <--> Z Exchanged + MagValue.Y = -(y - CalYoffSet) * CalYscale; // Z <--> Y Exchanged + + return result; + } + + public float GetX() + { + return GetMagValue(Register.OUT_X_H_M, Register.OUT_X_L_M) * CalXscale + CalXoffSet; + } + + public float GetZ() + { + return GetMagValue(Register.OUT_Y_H_M, Register.OUT_Y_L_M) * CalYscale + CalYoffSet; + } + + public float GetY() + { + return GetMagValue(Register.OUT_Z_H_M, Register.OUT_Z_L_M) * CalZscale + CalZoffSet; + } + + public override I2CDevice.Configuration Config + { + get { return _config; } + } + } +} diff --git a/Hardware/Linksprite_Camera.cs b/Hardware/Linksprite_Camera.cs new file mode 100644 index 0000000..f0a2dce --- /dev/null +++ b/Hardware/Linksprite_Camera.cs @@ -0,0 +1,332 @@ +using System; +using Microsoft.SPOT; +using System.IO.Ports; +using System.Text; +using System.Threading; +using System.Collections; + +using System.IO; +using Microsoft.SPOT.IO; + + +namespace PlaneOnBoardSoftware +{ + class Linksprite_Camera : IDisposable + { + static readonly byte[] RESET_OK_RESPONSE = new byte[] { 0x76, 0x00, 0x26, 0x00 }; + static readonly byte[] RESET_COMMAND = new byte[] { 0x56, 0x00, 0x26, 0x00 }; + + static readonly byte[] STOP_OK_RESPONSE = new byte[] { 0x76, 0x00, 0x36, 0x00, 0x00 }; + static readonly byte[] STOP_COMMAND = new byte[] { 0x56, 0x00, 0x36, 0x01, 0x03 }; + + static readonly byte[] SNAP_OK_RESPONSE = new byte[] { 0x76, 0x00, 0x36, 0x00, 0x00 }; + static readonly byte[] SNAP_COMMAND = new byte[] { 0x56, 0x00, 0x36, 0x01, 0x00 }; + + static readonly byte[] SIZE_OK_RESPONSE = new byte[] { 0x76, 0x00, 0x34, 0x00, 0x04, 0x00, 0x00 }; + static readonly byte[] SIZE_COMMAND = new byte[] { 0x56, 0x00, 0x34, 0x01, 0x00 }; + + static readonly byte[] GET_CHUNK_OK_RESPONSE = new byte[] { 0x76, 0x00, 0x32, 0x00, 0x00 }; + + //56 00 32 0C 00 0A 00 00 MH ML 00 00 KH KL XX XX: + static readonly byte[] GET_CHUNK_COMMAND = new byte[] { 0x56, 0x00, 0x32, 0x0C, 0x00, 0x0A, 0x00, 0x00, 255, 255, 0x00, 0x00, 255, 255, 0x00, 0x0A }; + + static readonly byte[] SET_SIZE_160x120 = new byte[] { 0x56, 0x00, 0x31, 0x05, 0x04, 0x01, 0x00, 0x19, 0x22 }; + static readonly byte[] SET_SIZE_320x240 = new byte[] { 0x56, 0x00, 0x31, 0x05, 0x04, 0x01, 0x00, 0x19, 0x11 }; + static readonly byte[] SET_SIZE_640x480 = new byte[] { 0x56, 0x00, 0x31, 0x05, 0x04, 0x01, 0x00, 0x19, 0x00 }; + static readonly byte[] SET_SIZE_OK_RESPONSE = new byte[] { 0x76, 0, 0x31, 0 }; + + static readonly byte[] SET_BAUDRATE_115200 = new byte[] { 0x56, 0x00, 0x24, 0x03, 0x01, 0x0D, 0xA6 }; + static readonly byte[] SET_BAUDRATE_57600 = new byte[] { 0x56, 0x00, 0x24, 0x03, 0x01, 0x1C, 0x4C }; + static readonly byte[] SET_BAUDRATE_OK_RESPONSE = new byte[] { 0x76, 0x00, 0x24, 0x00, 0x00 }; + + static readonly byte[] SET_COMPRESSION = new byte[] { 0x56, 0x00, 0x31, 0x05, 0x01, 0x10, 0x12, 0x04, 255 }; + static readonly byte[] SET_COMPRESSION_OK_RESPONSE = new byte[] { 0x76, 0x00, 0x31, 0x00, 0x00}; + + SerialPort port; + FileStream opendJpegFile; + Thread WriteAsync; + + public delegate void ActionBytes(byte[] chunk); + public delegate void Complete(); + + const int IN_BUFFER_SIZE = 960; + byte[] InBuffer = new byte[IN_BUFFER_SIZE]; + + bool isActive = true; + bool newWork = false; + + public Linksprite_Camera(SerialPort port) + { + port.BaudRate = 38400; + port.Parity = Parity.None; + port.DataBits = 8; + port.StopBits = StopBits.One; + + WriteAsync = new Thread(startAsync); + WriteAsync.Priority = ThreadPriority.Lowest; + WriteAsync.Start(); + while (!WriteAsync.IsAlive) ; + WriteAsync.Suspend(); + + this.port = port; + + port.ReadTimeout = 250; //so read call doesn't block forever + port.Open(); + + //Reset(); + Debug.Print("Compr: " + SetCompression(1)); + Debug.Print("Size: " +SetPictureSize(SET_SIZE_640x480)); + Reset(); + + + //Debug.Print("Answ: " + SendAndLookFor(SET_BAUDRATE_115200, SET_BAUDRATE_OK_RESPONSE)); + + //port.Close(); + //port.BaudRate = 115200; + //port.Open(); + } + + public bool Reset() + { + bool sendAndLookFor = SendAndLookFor(RESET_COMMAND, RESET_OK_RESPONSE); + + //camera needs time after reset + if (sendAndLookFor) + { + ReadAllRemaining(); + Thread.Sleep(3000); + } + + return sendAndLookFor; + } + + public bool SetPictureSize(byte[] sizeBytes) + { + bool sendAndLookFor = SendAndLookFor(sizeBytes, SET_SIZE_OK_RESPONSE); + if (sendAndLookFor) + ReadAllRemaining(); + + return sendAndLookFor; + } + + public bool SetCompression(byte compression) + { + SET_COMPRESSION[8] = compression; + bool sendAndLookFor = SendAndLookFor(SET_COMPRESSION, SET_COMPRESSION_OK_RESPONSE); + if (sendAndLookFor) + ReadAllRemaining(); + + return sendAndLookFor; + } + + public bool Stop() + { + if (opendJpegFile != null) opendJpegFile.Close(); + ReadAllRemaining(); + return SendAndLookFor(STOP_COMMAND, STOP_OK_RESPONSE); + } + + public void GetPicture(ActionBytes bytesAction) + { + Send(SNAP_COMMAND); + if (LookFor(SNAP_OK_RESPONSE)) + { + Send(SIZE_COMMAND); + if (LookFor(SIZE_OK_RESPONSE)) + { + //MSB, LSB + var sizeBytesLength = Read(2); + int fileSize = (InBuffer[0] << 8) | InBuffer[1]; + + int startAddress = 0; + int bytesRead = 0; + + GET_CHUNK_COMMAND[12] = MSB(IN_BUFFER_SIZE); + GET_CHUNK_COMMAND[13] = LSB(IN_BUFFER_SIZE); + + bool endReached = false; + while (!endReached) + { + GET_CHUNK_COMMAND[8] = MSB(startAddress); + GET_CHUNK_COMMAND[9] = LSB(startAddress); + + Send(GET_CHUNK_COMMAND); + if (LookFor(GET_CHUNK_OK_RESPONSE)) + { + int chunkLength = 0; + do + { + //Thread.Sleep(10); + + chunkLength = Read(); + + //ditch footer + Read(junkBuffer, GET_CHUNK_OK_RESPONSE.Length); + + //publish byte data + if (chunkLength > 0) + { + bytesRead += chunkLength; + + Debug.Print(bytesRead.ToString() + " / " + fileSize.ToString() + " (" + chunkLength.ToString() + ")"); + + if (bytesRead >= fileSize) + { + endReached = true; + + chunkLength = FindEnd(chunkLength); + } + + bytesAction(NewArray(chunkLength)); + } + + startAddress += chunkLength; + + } while (!endReached && chunkLength > 0); + } + } + } + + Stop(); + } + } + + public void SavePictureToSD(string FilePath) + { + if (!newWork) + { + opendJpegFile = new FileStream(FilePath, FileMode.OpenOrCreate); + + newWork = true; + WriteAsync.Resume(); + } + } + + private void startAsync() + { + while (isActive) + { + if (newWork) + { + GetPicture(SaveChuck); + newWork = false; + WriteAsync.Suspend(); + } + } + } + + private void SaveChuck(byte[] chunk) + { + opendJpegFile.Write(chunk, 0, chunk.Length); + } + + private byte[] NewArray(int chunkLength) + { + //make new array for bytes event so receiver can consume + //in sep thread without it changing during processing + var chunk = new byte[chunkLength]; + Array.Copy(InBuffer, chunk, chunkLength); + return chunk; + } + + private int FindEnd(int chunkLength) + { + if (chunkLength >= 2) + { + bool foundEnd = false; + + for (int i = chunkLength - 1; i >= 2; i--) + { + if (InBuffer[i - 1] == 0xFF && + InBuffer[i - 0] == 0xD9 + ) + { + chunkLength = i + 1; //include end marker in output + foundEnd = true; + break; + } + } + + if (!foundEnd) + Debug.Print("Invalid JPG data"); + } + return chunkLength; + } + + private static byte LSB(int num) + { + return (byte)(num & 0xFF); + } + + private static byte MSB(int num) + { + return (byte)(num >> 8); + } + + private bool SendAndLookFor(byte[] command, byte[] lookFor) + { + Send(command); + + return LookFor(lookFor); + } + + byte[] junkBuffer = new byte[IN_BUFFER_SIZE]; + private void ReadAllRemaining() + { + int readCount = 0; + + do + { + readCount = Read(junkBuffer, IN_BUFFER_SIZE); + } while (readCount != 0); + } + + private bool LookFor(byte[] expectedResponse) + { + var inSize = Read(expectedResponse.Length); + if (AreEqual(expectedResponse, inSize)) + return true; + + return false; + } + + private int Read() + { + return Read(IN_BUFFER_SIZE); + } + + private int Read(int bytes) + { + return Read(InBuffer, bytes); + } + + private int Read(byte[] buffer, int bytes) + { + return port.Read(buffer, 0, bytes); + } + + private void Send(byte[] command) + { + port.Write(command, 0, command.Length); + } + + private bool AreEqual(byte[] left, int inSize) + { + if (left == null || left.Length != inSize) + return false; + + for (int i = 0; i < left.Length; i++) + if (left[i] != InBuffer[i]) + return false; + + return true; + } + + public void Dispose() + { + isActive = false; + if (port != null) + port.Dispose(); + } + } +} \ No newline at end of file diff --git a/Hardware/PitotTubeSpeedSensor.cs b/Hardware/PitotTubeSpeedSensor.cs new file mode 100644 index 0000000..d1ade2d --- /dev/null +++ b/Hardware/PitotTubeSpeedSensor.cs @@ -0,0 +1,34 @@ +using System; +using Microsoft.SPOT; +using Microsoft.SPOT.Hardware; +using GHIElectronics.NETMF.Hardware; +using GHIElectronics.NETMF.System; + +namespace PlaneOnBoardSoftware +{ + class PitotTubeSpeedSensor + { + AnalogIn aiPort; + const float RLuft = 287.058f; + public float BarometricPressure = 100000; //Pa; + public float DynamicPressure; //Pa + public float Temperature = 20; + + public PitotTubeSpeedSensor(AnalogIn AnalogInPort) + { + aiPort = AnalogInPort; + + aiPort.SetLinearScale(0, 3300); //mV + } + + public float GetSpeed() + { + float roh = BarometricPressure / (RLuft * (Temperature + 273.15f)); + DynamicPressure = (aiPort.Read() / 5000f - 0.04f) / 0.00009f; //in Pa + if (roh > 0 && DynamicPressure > 0) + return (float)MathEx.Sqrt(2 / roh * DynamicPressure); + else + return 0; + } + } +} diff --git a/Hardware/RFM22b.cs b/Hardware/RFM22b.cs new file mode 100644 index 0000000..19bd4b1 --- /dev/null +++ b/Hardware/RFM22b.cs @@ -0,0 +1,663 @@ +//Nach dem c-Code von Ulrich Radig (http://www.ulrichradig.de) + +using System; +using Microsoft.SPOT; +using Microsoft.SPOT.Hardware; +using System.Threading; + +namespace PlaneOnBoardSoftware +{ + class RFM22b + { + public class CycleByteBuffer + { + private byte[] byteBuffer; + + private int rInd = 0; + private int wInd = 0; + private bool isFull = false; + + public CycleByteBuffer(int BufferSize) + { + byteBuffer = new Byte[BufferSize]; + } + + public int GetBytesToRead() + { + if (isFull && wInd == rInd) + return byteBuffer.Length; + else + return (byteBuffer.Length + wInd - rInd) % byteBuffer.Length; + } + + public int GetFreeByteCount() + { + if (!isFull && wInd == rInd) + return byteBuffer.Length; + else + return (byteBuffer.Length + rInd - wInd) % byteBuffer.Length; + } + + public int WriteBytes(byte[] Data, int OffSet, int Lenght) + { + int len = Lenght; + int freeLen = GetFreeByteCount(); + + if (len >= freeLen) + { + len = freeLen; + isFull = true; + } + + addToBuffer(Data, OffSet, byteBuffer, wInd, len); + wInd = (wInd + len) % byteBuffer.Length; + + return len; + } + + public int ReadBytes(byte[] Data, int OffSet, int Lenght) + { + return ReadBytes(Data, OffSet, Lenght, false); + } + + public int ReadBytes(byte[] Data, int OffSet, int Lenght, bool KeepInBuffer) + { + int len = Lenght; + int bytesLen = GetBytesToRead(); + + if (len >= bytesLen) + { + len = bytesLen; + if (!KeepInBuffer) isFull = false; + } + + getFromBuffer(byteBuffer, rInd, Data, OffSet, len); + if (!KeepInBuffer) rInd = (rInd + len) % byteBuffer.Length; + + return len; + } + + public int GetNextLineBr() + { + int rPos = rInd; + int lastrPos; + int count = GetBytesToRead(); + + for (int i = 0; i < count - 1; i++) + { + lastrPos = rPos; + rPos++; + if (rPos == byteBuffer.Length) rPos = 0; + if (byteBuffer[lastrPos] == 13 && byteBuffer[rPos] == 10) return i; + } + + return 0; + } + + public int DropBytes(int ByteCount) + { + int bytesToRemove = GetBytesToRead(); + + if (ByteCount < bytesToRemove) + bytesToRemove = ByteCount; + else + isFull = false; + + rInd = (rInd + bytesToRemove) % byteBuffer.Length; + return bytesToRemove; + } + + public void ClearBuffer() + { + wInd = 0; + rInd = 0; + isFull = false; + } + + private void addToBuffer(byte[] Data, int srcOffSet, byte[] Buffer, int destOffSet, int Lenght) + { + if (destOffSet + Lenght > Buffer.Length) + { + int pLen = Buffer.Length - destOffSet; + Array.Copy(Data, srcOffSet, Buffer, destOffSet, pLen); + Array.Copy(Data, srcOffSet + pLen, Buffer, 0, Lenght - pLen); + } + else + { + Array.Copy(Data, srcOffSet, Buffer, destOffSet, Lenght); + } + } + + private void getFromBuffer(byte[] Buffer, int srcOffSet, byte[] Data, int destOffSet, int Lenght) + { + if (srcOffSet + Lenght > Buffer.Length) + { + int pLen = Buffer.Length - srcOffSet; + Array.Copy(Buffer, srcOffSet, Data, destOffSet, pLen); + Array.Copy(Buffer, 0, Data, destOffSet + pLen, Lenght - pLen); + } + else + { + Array.Copy(Buffer, srcOffSet, Data, destOffSet, Lenght); + } + } + } + + public class Channel + { + public byte chanID; + public RFM22b rfMod; + private CycleByteBuffer TxBuffer; + private CycleByteBuffer RxBuffer; + + public byte Priority = 0; + + public int BytesToRead + { + get { return RxBuffer.GetBytesToRead(); } + } + + public int BytesToSend + { + get { return TxBuffer.GetBytesToRead(); } + } + + public Channel(RFM22b RFModul, byte ChannelID, int RxBufferSize, int TxBufferSize) + { + TxBuffer = new CycleByteBuffer(TxBufferSize); + RxBuffer = new CycleByteBuffer(RxBufferSize); + + chanID = ChannelID; + rfMod = RFModul; + + rfMod.addChannel(this, chanID); + } + + public int Send(byte[] Data, int OffSet, int Lenght) + { + int retVal = TxBuffer.WriteBytes(Data, OffSet, Lenght); + + rfMod.sendNextPacket(); + + return retVal; + } + + public int Send(string Text) + { + byte[] txtData = System.Text.UTF8Encoding.UTF8.GetBytes(Text); + int retVal = TxBuffer.WriteBytes(txtData, 0, txtData.Length); + rfMod.sendNextPacket(); + return retVal; + } + + public int SendLine(string Text) + { + return Send(Text + "\r\n"); + } + + public int Recv(byte[] Data, int OffSet, int Lenght) + { + return RxBuffer.ReadBytes(Data, OffSet, Lenght, false); + } + + public string RecvLine() + { + int byteCount = RxBuffer.GetNextLineBr(); + byte[] Data = new byte[byteCount]; + + if (byteCount > 0) + { + RxBuffer.ReadBytes(Data, 0, byteCount); + RxBuffer.DropBytes(2); + return new string(System.Text.UTF8Encoding.UTF8.GetChars(Data)); + } + else + { + return ""; + } + } + + public int AddBytesToRxBuffer(byte[] Data, int OffSet, int Lenght) + { + return RxBuffer.WriteBytes(Data, OffSet, Lenght); + } + + public int GetBytesFromTxBuffer(byte[] Data, int OffSet, int Lenght) + { + return TxBuffer.ReadBytes(Data, OffSet, Lenght, true); + } + + public void RemoveBytesFromTxBuffer(int ByteCount) + { + TxBuffer.DropBytes(ByteCount); + } + + public void ClearTxBuffer() + { + TxBuffer.ClearBuffer(); + rfMod.CancelPacketFlag = true; + } + + public void ClearRxBuffer() + { + RxBuffer.ClearBuffer(); + } + } + + public enum ModemState + { + Init, + Ready, + Transmitting, + Receving, + Error + }; + + private enum PacketAttrib + { + Normal, + ClearRecvBuffer + }; + + SPI.Configuration SpiConfig; + SPI SpiBus; + InterruptPort IRQn; + Channel[] ChannelList = new Channel[8]; + Timer TimeOutTimer; + + public ModemState State = ModemState.Init; + + public int RSSI; + public int LostPackets = 0; + + private int rxPackNr = 0; + private int txPackNr = 0; + + private Channel lastTxChan; + private int lastTxPackLen = 60; + private int lastRxPackLen = 0; + private bool CancelPacketFlag = false; + private byte InterruptStatus1 = 0; + + private double _frequency = 869.545; + + public double Frequency + { + get + { + return _frequency; + } + set + { + rf22_setfreq((byte)((value - 860.0) * 3200)); + } + } + + public RFM22b(SPI.SPI_module SpiMmodule, Cpu.Pin ChipSelectPort, Cpu.Pin IRQPort, double FrequInHz) + { + SpiConfig = new SPI.Configuration(ChipSelectPort, false, 0, 0, false, true, 1000, SpiMmodule); + SpiBus = new SPI(SpiConfig); + IRQn = new InterruptPort(IRQPort, false, Port.ResistorMode.Disabled, Port.InterruptMode.InterruptEdgeLow); + + IRQn.OnInterrupt += new NativeEventHandler(IRQn_OnInterrupt); + + rf22_init(); + rf22_setfreq((byte)((_frequency - 860.0) * 3200)); + + rf22_rxmode(); + TimeOutTimer = new Timer(new TimerCallback(RecvTimeOut), null, 500, Timeout.Infinite); + + State = ModemState.Receving; + } + + public Channel NewChannel(byte ChannelID, int RxBufferSize, int TxBufferSize) + { + return new Channel(this, ChannelID, RxBufferSize, TxBufferSize); + } + + private void addChannel(Channel chanToAdd, byte chanID) + { + ChannelList[chanID] = chanToAdd; + } + + private void sendNextPacket() + { + int hcIndex = -1; + int prio = -1; + + //Debug.Print("--> Send State" + State + " <--"); + + if (State == ModemState.Ready) + { + for (int i = 0; i < ChannelList.Length; i++) + { + if (ChannelList[i] != null) + { + if (ChannelList[i].BytesToSend > 0 && ChannelList[i].Priority > prio) + { + hcIndex = i; + prio = ChannelList[i].Priority; + } + } + } + + int byteCount; + byte[] Buffer = new byte[64]; + + Buffer[0] = (byte)rxPackNr; + Buffer[1] = (byte)txPackNr; + Buffer[2] = (byte)hcIndex; + Buffer[3] = CancelPacketFlag ? (byte)PacketAttrib.ClearRecvBuffer : (byte)PacketAttrib.Normal; + + + //Debug.Print("--> Send hcIndex" + hcIndex + " <--"); + + if (hcIndex > -1) + { + byteCount = ChannelList[hcIndex].GetBytesFromTxBuffer(Buffer, 4, lastTxPackLen); + + //Debug.Print("** Send " + byteCount + " **"); + + State = ModemState.Transmitting; + rf22_sendpacket(Buffer, 0, byteCount + 4); + + lastTxChan = ChannelList[hcIndex]; + lastTxPackLen = byteCount; + } + else if (lastRxPackLen > 4) + { + //Debug.Print("** Send Empty **"); + lastRxPackLen = 0; + State = ModemState.Transmitting; + rf22_sendpacket(Buffer, 0, 4); + } + else + { + rf22_rxmode(); + } + } + } + + private void readPacket() + { + byte[] Buffer = new byte[64]; + int cInd; + int packetLen; + int newRxPacketNr; + + packetLen = rf22_getpacket(Buffer, 0); + lastRxPackLen = packetLen; + + + //Debug.Print("** Receve " + packetLen + " **"); + if (packetLen > 3) + { + + + newRxPacketNr = Buffer[1]; + + cInd = Buffer[2]; //Channel ID + + + if (Buffer[0] == txPackNr && lastTxChan != null) + { + lastTxChan.RemoveBytesFromTxBuffer(lastTxPackLen); + lastTxPackLen = 60; + txPackNr++; + if (txPackNr > 255) txPackNr = 0; + } + + if (cInd < ChannelList.Length) + { + if (ChannelList[cInd] != null && rxPackNr != newRxPacketNr && packetLen > 4) + { + if (Buffer[3] == (byte)PacketAttrib.ClearRecvBuffer) ChannelList[cInd].ClearRxBuffer(); + ChannelList[cInd].AddBytesToRxBuffer(Buffer, 4, packetLen - 4); + rxPackNr = newRxPacketNr; + } + } + } + } + + private void RecvTimeOut(object nullObj) + { + if (State == ModemState.Receving) + { + State = ModemState.Ready; + RSSI = -1; + if (LostPackets == int.MaxValue) LostPackets = 0; + LostPackets++; + sendNextPacket(); + } + } + + private void IRQn_OnInterrupt(uint port, uint state, DateTime time) + { + InterruptStatus1 = rf22_read(0x03); + + if (State == ModemState.Transmitting) + { + // All Data Transmitted + rf22_write(0x07, 0x01); // switch to ready mode + rf22_rxmode(); + TimeOutTimer.Dispose(); + TimeOutTimer = new Timer(new TimerCallback(RecvTimeOut), null, 500, Timeout.Infinite); + State = ModemState.Receving; + } + else if (State == ModemState.Receving || State == ModemState.Ready) + { + RSSI = rf22_read(0x26); + State = ModemState.Ready; + readPacket(); + sendNextPacket(); + } + } + + + #region Hardware Commands + + private void rf22_write(byte addr, byte data) + { + byte[] addrArray = new byte[2]; + + addrArray[0] = (byte)(128 | addr); + addrArray[1] = data; + + SpiBus.Write(addrArray); + } + + private byte rf22_read(byte addr) + { + byte[] ret = new byte[2]; + SpiBus.WriteRead(new byte[] { (byte)(addr & 127), 0xFF }, ret); + return ret[1]; + } + + private void rf22_burstread(byte addr, byte[] data, int OffSet, int Lenght) + { + byte[] arg = new byte[Lenght + 1]; + + arg[0] = (byte)(addr & 127); + + SpiBus.WriteRead(arg, 0, Lenght + 1, data, OffSet, Lenght, 1); + } + + private void rf22_init() + { + Thread.Sleep(20); + + rf22_write(0x07, 0x80); // software reset + + Thread.Sleep(20); + + rf22_write(0x05, 0x06); // valid packed received and packet send interrupt on + + rf22_write(0x06, 0x00); // all interrupts off + rf22_write(0x07, 0x01); // operating mode: ready mode + rf22_write(0x09, 0x7f); // xtal load capacitance + rf22_write(0x0A, 0x02); // uC CLK: 10MHz + + rf22_write(0x0b, 0xf2); // GPIO0: TX_ANT - f2 + rf22_write(0x0c, 0xf5); // GPIO1: RX ANT - f5 + rf22_write(0x0d, 0x00); // GPIO2: uC Clock out + rf22_write(0x0e, 0x00); + rf22_write(0x0f, 0x70); // ADC Input: GND + rf22_write(0x10, 0x00); // ADC offset: 0 + rf22_write(0x12, 0x00); // temp sensor calibration off + rf22_write(0x13, 0x00); // temp sensor offset: 0 + rf22_write(0x1d, 0x40); // enable AFC + rf22_write(0x1e, 0x0A); // afc timing + rf22_write(0x1f, 0x03); // afc timing + + rf22_write(0x1C, 0x05); // IF bandwidth + rf22_write(0x20, 0x83); // Clock Recovery Oversampling Rate + rf22_write(0x21, 0xC0); // Clock Recovery Offset 2 + rf22_write(0x22, 0x13); // Clock Recovery Offset 1 + rf22_write(0x23, 0xA9); // Clock Recovery Offset 0 + rf22_write(0x24, 0x00); // Clock Recovery Timing Loop Gain 1 + rf22_write(0x25, 0x04); // Clock Recovery Timing Loop Gain 0 + rf22_write(0x2A, 0x24); + + rf22_write(0x27, 0x10); // RSSI Threashold: -120dB + + rf22_write(0x30, 0x8c); // data access: RX/TX packet handling, enable crc: CCIT + rf22_write(0x32, 0xff); // header check enable + rf22_write(0x33, 0x42); // 2 word synchronisation + rf22_write(0x34, 0x10); // preamble length: 16 nibbles, = 64bits + rf22_write(0x35, 0x30); // preamble detection control: 6 nibbles = 24bits + rf22_write(0x36, 0x2d); // sync word 3 + rf22_write(0x37, 0xd4); // sync word 2 + rf22_write(0x38, 0xAA); // sync word 1 + rf22_write(0x39, 0xAA); // sync word 0 + rf22_write(0x3a, 101); // transmit header 3 + rf22_write(0x3b, 108); // transmit header 2 + rf22_write(0x3c, 103); // transmit header 1 + rf22_write(0x3d, 106); // transmit header 0 + rf22_write(0x3e, 17); // packet length + rf22_write(0x3f, 101); // check header 3 + rf22_write(0x40, 108); // check header 2 + rf22_write(0x41, 103); // check header 1 + rf22_write(0x42, 106); // check header 0 + rf22_write(0x43, 0xff); // header enable mask 3 + rf22_write(0x44, 0xff); // header enable mask 2 + rf22_write(0x45, 0xff); // header enable mask 1 + rf22_write(0x46, 0xff); // header enable mask 0 + + rf22_write(0x69, 0x60); // AGC on + rf22_write(0x6a, 0x0b); // agc override 2 + + //rf22_write(0x6d, 0x08); // tx power: +1dBm + rf22_write(0x6d, 0x0F); // tx power: +17dBm (nicht 20dbm??) + + //baud rate: 2,39 kBit/s = val("&H13A9")/2^(16+5)*1000 kHz + rf22_write(0x6E, 0x13); // set baud high + rf22_write(0x6F, 0xA9); // set baud low + + rf22_write(0x70, 0x2C); // modulation control + rf22_write(0x71, 0x22); // modulation control 2: FIFO mode, OOK //0x21 / 0x00 + + rf22_write(0x72, 0x50); // frequency deviation: 45kHz + rf22_write(0x73, 0x00); // offset: 0 + rf22_write(0x74, 0x00); // offset: 0 + + rf22_write(0x79, 0x0); // frequency hopping off + rf22_write(0x7a, 0x0); // frequency hopping off + + rf22_write(0x75, 0x73); // 860-880MHz range + //rf22_write(0x75, 0x53); // 430-440MHz range + + rf22_write(0x08, 0x00); // clear fifo, disable multi packet + } + + public void rf22_rxmode() + { + rf22_read(0x03); // clear interrupt status + rf22_read(0x04); // clear interrupt status + rf22_write(0x07, 0x01); // to_ready_mode(); + + rf22_write(0x07, 0x01); // to_ready_mode(); + rf22_write(0x7e, 0x40); // threshold for rx almost full, interrupt when 64 bytes received + + rf22_write(0x08, 0x03); // clear RX fifo + rf22_write(0x08, 0x00); // clear RX fifo + + rf22_write(0x07, 0x05); // RX on + + rf22_read(0x03); // clear interrupt status + rf22_read(0x04); // clear interrupt status + } + + + private void rf22_setfreq(UInt16 freq) + { + rf22_write(0x76, (byte)((freq & 0xFF00) >> 8)); + rf22_write(0x77, (byte)(freq & 0x00FF)); + } + + private void rf22_sendpacket(byte[] data, int offset, int lenght) + { + //Debug.Print("---> " + lenght); + + byte i; + byte size = (byte)lenght; + + if (size > 64) size = 64; + + rf22_write(0x07, 0x03); // switch to ready mode + rf22_read(0x03); // clear interrupt status + rf22_read(0x04); // clear interrupt status + + rf22_write(0x08, 0x01); // clear TX fifo + rf22_write(0x08, 0x00); // clear TX fifo + + //rf22_write(0x34, 32); // premable length: 32 nibbles -> 128 Bits + rf22_write(0x3e, size); // packet length + + for (i = 0; i < size; i++) + { + rf22_write(0x7f, data[i + offset]); + } + + rf22_write(0x07, 0x09); // TX on + } + + private int rf22_getpacket(byte[] data, int offset) + { + byte cnt; + + //if ((rf22_read(0x31) & 0x1A) > 0) // receiving a packet + //{ + if ((InterruptStatus1 & 2) > 0) // packet received & not read && ((rf22_read(0x02) & 32) == 0) + { + cnt = rf22_read(0x4B); // packet length + //Debug.Print(cnt + " " + data[cnt-2].ToString()); + + /*for (int i = 0; i < cnt; i++) // Daten (cnt - 2 für CRC) + { + data[i + offset] = rf22_read(0x7f); + }*/ + rf22_burstread(0x7f, data, offset, cnt); + + /*try + { + byte[] Test = new byte[cnt - 4]; + Array.Copy(data, 4, Test, 0, cnt - 4); + + string tmpCa = (new string(System.Text.UTF8Encoding.UTF8.GetChars(Test))); + Debug.Print(tmpCa); + } + catch + { + Debug.Print("Error"); + }*/ + + return (cnt); + } + return 0; + //} + //return 0; + } + + #endregion + } +} \ No newline at end of file diff --git a/Hardware/SdCardLogger.cs b/Hardware/SdCardLogger.cs new file mode 100644 index 0000000..9eef390 --- /dev/null +++ b/Hardware/SdCardLogger.cs @@ -0,0 +1,54 @@ +using System; +using Microsoft.SPOT; +using System.IO; +using Microsoft.SPOT.IO; +using System.Text; +using System.Threading; +using System.IO.Ports; + +namespace PlaneOnBoardSoftware +{ + class SdCardLogger + { + SerialPort Uart; + static byte[] newLineBytes = { 13, 10 }; + + public SdCardLogger(SerialPort SerialPort) + { + Uart = SerialPort; + + Uart.BaudRate = 9600; + Uart.Parity = Parity.None; + Uart.DataBits = 8; + Uart.StopBits = StopBits.One; + + Uart.Open(); + } + + public void Write(string Text) + { + sendToOpenLog(Encoding.UTF8.GetBytes(Text)); + } + + public void Write(Single value) + { + sendToOpenLog(Encoding.UTF8.GetBytes(value.ToString())); + } + + public void Write(Double value) + { + sendToOpenLog(Encoding.UTF8.GetBytes(value.ToString())); + } + + public void NewLine() + { + sendToOpenLog(newLineBytes); + } + + void sendToOpenLog(byte[] data) + { + Uart.Write(data, 0, data.Length); + //Uart.Flush(); + } + } +} diff --git a/Hardware/ServoController.cs b/Hardware/ServoController.cs new file mode 100644 index 0000000..b471c38 --- /dev/null +++ b/Hardware/ServoController.cs @@ -0,0 +1,36 @@ +using System; +using Microsoft.SPOT; +using GHIElectronics.NETMF.Hardware; +using GHIElectronics.NETMF.FEZ; +using System.Threading; + +namespace PlaneOnBoardSoftware +{ + class ServoController + { + float _position = 0; + float usedAngle = 0; + PWM servoPwm; + + public ServoController(PWM.Pin ServoPin, float Angle) + { + servoPwm = new PWM(ServoPin); + this.Position = 0; + usedAngle = Angle / 2; + } + + public float Position + { + get { + return _position; + } + set { + _position = value; + if (_position > usedAngle) _position = usedAngle; + if (_position < -usedAngle) _position = -usedAngle; + + servoPwm.SetPulse(20000000, (uint)(1500000 + _position / 90 * 1250000)); + } + } + } +} diff --git a/Hardware/SkyTraqVenus6_GPS.cs b/Hardware/SkyTraqVenus6_GPS.cs new file mode 100644 index 0000000..1d77cdc --- /dev/null +++ b/Hardware/SkyTraqVenus6_GPS.cs @@ -0,0 +1,187 @@ +using System; +using System.IO; +using System.IO.Ports; +using Microsoft.SPOT; +using System.Threading; +using GHIElectronics.NETMF.System; + +namespace PlaneOnBoardSoftware +{ + public class SkyTraqVenus6_GPS + { + private byte MESSAGE_TYPE = 0x09; + private byte POSITION_RATE = 0x0E; + private DateTime GpsStartDate = new DateTime(1980, 1, 6); + + SerialPort Uart; + byte[] rxBuffer = new byte[256]; + int bufferPos = 0; + int packOffSet = 0; + + private int gpsTimeOfWeek; + private int gpsWeekNumber; + + //private Vector3D ECEFCoordinate = new Vector3D(); + private Vector3D ECEFSpeed = new Vector3D(); + + public bool Has3DFix = false; + public double Latitude = 0; + public double Longitude = 0; + public float Altitude = 0; + public float EllipsoidAltitude = 0; + public float HorizontalDilution = 0; + public float VerticalDilution = 0; + public Vector3D Speed = new Vector3D(); + public DateTime GpsTime = new DateTime(); + public float Heading = 0; + public float GroundSpeed = 0; + + public SkyTraqVenus6_GPS(SerialPort SerialPort) + { + Uart = SerialPort; + + Uart.BaudRate = 9600; + Uart.Parity = Parity.None; + Uart.DataBits = 8; + Uart.StopBits = StopBits.One; + + Uart.Open(); + if (SerialPort.PortName.ToUpper() == "COM4") + { + FEZ_Low_Level.RemapCOM4to_TXAn2_RXAn3(Uart); + } + + SendCommand(MESSAGE_TYPE, 2, 0); //Switch to Binary Output + //SendCommand(POSITION_RATE, 3, 0); //Switch to 1 Hz update rate + } + + private void SendCommand(byte Command, byte Value1, byte Value2) + { + //<0xA0,0xA1>< PL><09>< message body><0x0D,0x0A> + byte[] newCom = new byte[] { 0xA0, 0xA1, 0, 3, Command, Value1, Value2, 0xFF, 0x0D, 0x0A }; + + SetChecksum(newCom, 3); + + Uart.Write(newCom, 0, 10); + Uart.Flush(); + } + + private byte GetChecksum(byte[] data, int PayloadLen) + { + int cs = 0; + + for (int i = 0; i < PayloadLen; i++) + { + cs = cs ^ data[i + 4]; + } + + return (byte)cs; + } + + private void SetChecksum(byte[] data, int PayloadLen) + { + data[PayloadLen + 4] = GetChecksum(data, PayloadLen); + } + + public void ReadGPSData() + { + int read_count = 1; + int readLenght = 1; + int portBuffCount = Uart.BytesToRead; + + while (portBuffCount > 0) + { + readLenght = portBuffCount; + if (readLenght > 128) readLenght = 128; + + if (bufferPos > 128) bufferPos = 0; + read_count = Uart.Read(rxBuffer, bufferPos, readLenght); + + portBuffCount -= read_count; + bufferPos += read_count; + } + + for (int i = 0; i < 66; i++) + { + packOffSet = bufferPos - i - 66; + + if (packOffSet < 0) break; + if (rxBuffer[packOffSet + 65] == 10 && rxBuffer[packOffSet + 64] == 13 && + rxBuffer[packOffSet + 1] == 0xA1 && rxBuffer[packOffSet] == 0xA0) + break; + } + + if (packOffSet > -1) + { + Has3DFix = (GetByte(2)==2); + + Latitude = GetSignedInteger(10) * 10e-8; + Longitude = GetSignedInteger(14) * 10e-8; + + /*ECEFCoordinate.X = GetSignedInteger(36) * 0.01f; + ECEFCoordinate.Y = GetSignedInteger(40) * 0.01f; + ECEFCoordinate.Z = GetSignedInteger(44) * 0.01f;*/ + + ECEFSpeed.X = GetSignedInteger(48) * 0.01f; + ECEFSpeed.Y = GetSignedInteger(52) * 0.01f; + ECEFSpeed.Z = GetSignedInteger(56) * 0.01f; + + EllipsoidAltitude = GetSignedInteger(18) * 0.01f; //ellipsoid altitude + Altitude = GetSignedInteger(22) * 0.01f; //mean sea level altitude + + HorizontalDilution = GetUSignInt16(30) * 0.01f; + VerticalDilution = GetUSignInt16(32) * 0.01f; + + gpsWeekNumber = GetUSignInt16(4); + gpsTimeOfWeek = GetSignedInteger(6); + + GpsTime = GpsStartDate.AddDays(gpsWeekNumber * 7).AddMilliseconds(gpsTimeOfWeek * 10); + + Speed = ECEFtoLTPspeed(ECEFSpeed, Latitude, Longitude); + } + } + + private Vector3D ECEFtoLTPspeed(Vector3D ECEFSpeed, double Latitude, double Longitude) + { + Vector3D GetLTPSpeed = new Vector3D(); + + double SinLat = MathEx.Sin(Latitude); + double SinLon = MathEx.Sin(Longitude); + double CosLat = MathEx.Cos(Latitude); + double CosLon = MathEx.Cos(Longitude); + + + GetLTPSpeed.X = (float)(-ECEFSpeed.X * SinLat * SinLon - ECEFSpeed.Y * SinLat * SinLon + ECEFSpeed.Z * CosLat); //Vnorth + GetLTPSpeed.Y = (float)(-ECEFSpeed.X * SinLon + ECEFSpeed.Y * CosLon); //Veast + GetLTPSpeed.Z = (float)(-ECEFSpeed.X * CosLat * CosLon - ECEFSpeed.Y * CosLat * SinLon + ECEFSpeed.Z * SinLat); //Vdown + + return GetLTPSpeed; + } + + private byte GetByte(int Field) + { + return rxBuffer[packOffSet + 3 + Field]; + } + + private int GetUSignInt16(int StartField) + { + int bufPos = packOffSet + 3 + StartField; + return (rxBuffer[bufPos] << 8) + rxBuffer[bufPos + 1]; + } + + private int GetSignedInteger(int StartField) + { + int buffer = 0; + + for (int i = 0; i < 4; i++) + { + int bufPos = packOffSet + 3 + StartField + i; + + buffer = (buffer << 8) + rxBuffer[bufPos]; + } + + return buffer; + } + + } +} diff --git a/HighAltitude.csproj.vspscc b/HighAltitude.csproj.vspscc new file mode 100644 index 0000000..feffdec --- /dev/null +++ b/HighAltitude.csproj.vspscc @@ -0,0 +1,10 @@ +"" +{ +"FILE_VERSION" = "9237" +"ENLISTMENT_CHOICE" = "NEVER" +"PROJECT_FILE_RELATIVE_PATH" = "" +"NUMBER_OF_EXCLUDED_FILES" = "0" +"ORIGINAL_PROJECT_FILE_PATH" = "" +"NUMBER_OF_NESTED_PROJECTS" = "0" +"SOURCE_CONTROL_SETTINGS_PROVIDER" = "PROVIDER" +} diff --git a/MadgwickAHRS.cs b/MadgwickAHRS.cs new file mode 100644 index 0000000..9830ff1 --- /dev/null +++ b/MadgwickAHRS.cs @@ -0,0 +1,203 @@ +using System; +using Microsoft.SPOT; +using GHIElectronics.NETMF.System; + +namespace PlaneOnBoardSoftware +{ + /// + /// MadgwickAHRS class. Implementation of Madgwick's IMU and AHRS algorithms. + /// GNU General Public Licence + /// + /// + /// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms + /// Modified 2012 by Nicolas Kruse + /// + class MadgwickAHRS + { + /// AirSpeed in m/s + public float InpAirSpeed = 0; + + /// Acceleration in m/s² + public Vector3D InpAcc = new Vector3D(); + + /// Angular rate in deg/s + public Vector3D InpGyr = new Vector3D(); + + public Vector3D InpMag = new Vector3D(); + + /// Ascent/descent angle + public float Pitch = 0; + /// Roll angle + public float Roll = 0; + /// Attitude angle + public float Yaw = 0; + + /// + /// Gets or sets the algorithm gain beta. + /// + public float Beta = 0.1f; + + /// + /// Gets or sets the Quaternion output. + /// + private float[] Quat = {0, 0, 0, 1}; + + private float SamplePeriod; + + private static float piPerDeg = 0.017453292519f; + private static float degPerPi = 57.29577951308f; + + /// + /// Algorithm AHRS update method. Requires only gyroscope and accelerometer data. + /// + /// + /// Gyroscope x axis measurement in radians/s. + /// + /// + /// Gyroscope y axis measurement in radians/s. + /// + /// + /// Gyroscope z axis measurement in radians/s. + /// + /// + /// Accelerometer x axis measurement in any calibrated units. + /// + /// + /// Accelerometer y axis measurement in any calibrated units. + /// + /// + /// Accelerometer z axis measurement in any calibrated units. + /// + /// + /// Magnetometer x axis measurement in any calibrated units. + /// + /// + /// Magnetometer y axis measurement in any calibrated units. + /// + /// + /// Magnetometer z axis measurement in any calibrated units. + /// + /// + /// Optimised for minimal arithmetic. + /// Total ±: 160 + /// Total *: 172 + /// Total /: 5 + /// Total sqrt: 5 + /// + public void Update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) + { + float q1 = Quat[0], q2 = Quat[1], q3 = Quat[2], q4 = Quat[3]; // short name local variable for readability + float norm; + float hx, hy, _2bx, _2bz; + float s1, s2, s3, s4; + float qDot1, qDot2, qDot3, qDot4; + + // Auxiliary variables to avoid repeated arithmetic + float _2q1mx; + float _2q1my; + float _2q1mz; + float _2q2mx; + float _4bx; + float _4bz; + float _2q1 = 2f * q1; + float _2q2 = 2f * q2; + float _2q3 = 2f * q3; + float _2q4 = 2f * q4; + float _2q1q3 = 2f * q1 * q3; + float _2q3q4 = 2f * q3 * q4; + float q1q1 = q1 * q1; + float q1q2 = q1 * q2; + float q1q3 = q1 * q3; + float q1q4 = q1 * q4; + float q2q2 = q2 * q2; + float q2q3 = q2 * q3; + float q2q4 = q2 * q4; + float q3q3 = q3 * q3; + float q3q4 = q3 * q4; + float q4q4 = q4 * q4; + + // Normalise accelerometer measurement + norm = (float)MathEx.Sqrt(ax * ax + ay * ay + az * az); + if (norm == 0f) return; // handle NaN + norm = 1 / norm; // use reciprocal for division + ax *= norm; + ay *= norm; + az *= norm; + + // Normalise magnetometer measurement + norm = (float)MathEx.Sqrt(mx * mx + my * my + mz * mz); + if (norm == 0f) return; // handle NaN + norm = 1 / norm; // use reciprocal for division + mx *= norm; + my *= norm; + mz *= norm; + + // Reference direction of Earth's magnetic field + _2q1mx = 2f * q1 * mx; + _2q1my = 2f * q1 * my; + _2q1mz = 2f * q1 * mz; + _2q2mx = 2f * q2 * mx; + hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4; + hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4; + _2bx = (float)MathEx.Sqrt(hx * hx + hy * hy); + _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4; + _4bx = 2f * _2bx; + _4bz = 2f * _2bz; + + // Gradient decent algorithm corrective step + s1 = -_2q3 * (2f * q2q4 - _2q1q3 - ax) + _2q2 * (2f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); + s2 = _2q4 * (2f * q2q4 - _2q1q3 - ax) + _2q1 * (2f * q1q2 + _2q3q4 - ay) - 4f * q2 * (1 - 2f * q2q2 - 2f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); + s3 = -_2q1 * (2f * q2q4 - _2q1q3 - ax) + _2q4 * (2f * q1q2 + _2q3q4 - ay) - 4f * q3 * (1 - 2f * q2q2 - 2f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); + s4 = _2q2 * (2f * q2q4 - _2q1q3 - ax) + _2q3 * (2f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); + norm = 1f / (float)MathEx.Sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude + s1 *= norm; + s2 *= norm; + s3 *= norm; + s4 *= norm; + + // Compute rate of change of quaternion + qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - Beta * s1; + qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - Beta * s2; + qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - Beta * s3; + qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - Beta * s4; + + // Integrate to yield quaternion + q1 += qDot1 * SamplePeriod; + q2 += qDot2 * SamplePeriod; + q3 += qDot3 * SamplePeriod; + q4 += qDot4 * SamplePeriod; + norm = 1f / (float)MathEx.Sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion + Quat[0] = q1 * norm; + Quat[1] = q2 * norm; + Quat[2] = q3 * norm; + Quat[3] = q4 * norm; + } + + public void Calculate(float dt) + { + //Correcting centrifugal acceleration + //CorrAccZ = InpAcc.Z - InpAirSpeed * InpGyr.X / degPerPi; //Check sign! + + if (dt < 0.5 && dt > 0) + { + SamplePeriod = dt; + + Update(InpGyr.X * piPerDeg, InpGyr.Y * piPerDeg, InpGyr.Z * piPerDeg, InpAcc.X, InpAcc.Y, InpAcc.Z, InpMag.X, InpMag.Y, InpMag.Z); + + float a01 = 2 * Quat[0] * Quat[1]; + float a02 = 2 * Quat[0] * Quat[2]; + float a03 = 2 * Quat[0] * Quat[3]; + float a11 = 2 * Quat[1] * Quat[1]; + float a12 = 2 * Quat[1] * Quat[2]; + float a13 = 2 * Quat[1] * Quat[3]; + float a22 = 2 * Quat[2] * Quat[2]; + float a23 = 2 * Quat[2] * Quat[3]; + float a33 = 2 * Quat[3] * Quat[3]; + + Pitch = (float)MathEx.Asin(a01 + a23) * degPerPi; + Roll = -(float)MathEx.Atan2(a02 - a13, 1 - (a22 + a11)) * degPerPi; + Yaw = (float)MathEx.Atan2((a11 + a33) - 1, a12 - a03) * degPerPi; + } + } + } +} diff --git a/Navigation.cs b/Navigation.cs new file mode 100644 index 0000000..5de9835 --- /dev/null +++ b/Navigation.cs @@ -0,0 +1,79 @@ +using System; +using Microsoft.SPOT; +using GHIElectronics.NETMF.System; + +namespace PlaneOnBoardSoftware +{ + class Navigation + { + const float R = 6378137; // meter + const double pi180 = MathEx.PI / 180; + const float Rpi180 = R * (float)pi180; + + public float Radius = 50; // meter + public double Latitude = 0; + public double Longitude = 0; + public double DestLatitude = 0; + public double DestLongitude = 0; + + public bool GpsHasFix = false; + public double GpsLatitude = 0; + public double GpsLongitude = 0; + + public float AirSpeed = 0; + public float Heading = 0; + public float Pitch = 0; + public float TargetHeading = 0; + public float Distance = 0; // meter + + public void Calculate(float dt) + { + double Lat1; + double Lon1; + double Lat2; + double Lon2; + float bearing; + float rdRatio; + double dLon; + double CosLat2; + double y; + double x; + + + //Calculate position + if (GpsHasFix) + { + Latitude = GpsLatitude; + Longitude = GpsLongitude; + } + else + { + Latitude += AirSpeed * dt * MathEx.Cos(Heading * pi180) / Rpi180; + Longitude += AirSpeed * dt * MathEx.Sin(Heading * pi180) * MathEx.Cos(Latitude * pi180) / Rpi180; + } + + + //Calculate flight path + Lat1 = Latitude * pi180; + Lon1 = Longitude * pi180; + Lat2 = DestLatitude * pi180; + Lon2 = DestLongitude * pi180; + dLon = Lon2 - Lon1; + CosLat2 = MathEx.Cos(Lat2); + y = MathEx.Sin(dLon) * CosLat2; + x = MathEx.Cos(Lat1) * MathEx.Sin(Lat2) - + MathEx.Sin(Lat1) * CosLat2 * MathEx.Cos(dLon); + bearing = (float)(MathEx.Atan2(y, x) / pi180); + + x = (Lon2 - Lon1) * MathEx.Cos((Lat1 + Lat2) / 2); + y = (Lat2 - Lat1); + Distance = (float)MathEx.Sqrt(x * x + y * y) * R; + + if (Distance > 1.0) + { + rdRatio = (Radius / Distance); + TargetHeading = (bearing + 90 * (rdRatio * rdRatio) + 540) % 360 - 180; + } + } + } +} diff --git a/PidController.cs b/PidController.cs new file mode 100644 index 0000000..e6565e3 --- /dev/null +++ b/PidController.cs @@ -0,0 +1,76 @@ +using System; + +namespace PlaneOnBoardSoftware +{ + class PidController + { + private float lastError = 0; + private DateTime lastTime; + private float integral = 0; + private float _minOutput = 0; + private float _maxOutput = 0; + private float Spann = 0; + + public float P = 0; + public float I = 0; + public float D = 0; + public float SetPoint = 0; + public float InputVariable = 0; + + public bool Circle360DegMode = false; + + public PidController(float MinOutput, float MaxOutput, float PropValue, float InteValue, float DiffValue, bool Circle360DegreeMode) + { + Circle360DegMode = Circle360DegreeMode; + + P = PropValue; + I = InteValue; + D = DiffValue; + + lastTime = DateTime.UtcNow; + _minOutput = MinOutput; + _maxOutput = MaxOutput; + Spann = MaxOutput - MinOutput; + } + + public PidController(float MinOutput, float MaxOutput, float PropValue, float InteValue, float DiffValue) + { + P = PropValue; + I = InteValue; + D = DiffValue; + + lastTime = DateTime.UtcNow; + _minOutput = MinOutput; + _maxOutput = MaxOutput; + Spann = MaxOutput - MinOutput; + } + + public float GetOutput(float dt, float inputValue) + { + InputVariable = inputValue; + float error = SetPoint - InputVariable; + + if (Circle360DegMode) + { + float errev = error - 360; + if (errev*errev < error*error) error = errev; + } + + float iError = integral + P * error * dt / I; + + if (iError < Spann && iError > 0) + integral = iError; + + float correction = _minOutput + P * (error + D * (error - lastError) / dt) + integral; + + lastError = error; + + if (correction > _maxOutput) + correction = _maxOutput; + else if (correction < _minOutput) + correction = _minOutput; + + return correction; + } + } +} \ No newline at end of file diff --git a/PlaneOnBoardSoftware.csproj b/PlaneOnBoardSoftware.csproj new file mode 100644 index 0000000..22b078c --- /dev/null +++ b/PlaneOnBoardSoftware.csproj @@ -0,0 +1,88 @@ + + + + FEZ Mini Application + Exe + HighAltitude + {b69e3092-b931-443c-abe7-7e7b65f2a37f};{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC} + 9.0.21022 + 2.0 + {2CC55691-0251-403D-BFC2-C44CDAA6B2B7} + v4.1 + $(MSBuildExtensionsPath32)\Microsoft\.NET Micro Framework\ + SAK + SAK + SAK + SAK + + + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + + + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + + + + + + + + + + + + + + + + + + + + + True + True + Resources.resx + + + + + + + + + + Designer + ResXFileCodeGenerator + Resources.Designer.cs + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/PlaneOnBoardSoftware.csproj.user b/PlaneOnBoardSoftware.csproj.user new file mode 100644 index 0000000..dfb9997 --- /dev/null +++ b/PlaneOnBoardSoftware.csproj.user @@ -0,0 +1,7 @@ + + + + USB + USBizi + + \ No newline at end of file diff --git a/PlaneOnBoardSoftware.csproj.vspscc b/PlaneOnBoardSoftware.csproj.vspscc new file mode 100644 index 0000000..feffdec --- /dev/null +++ b/PlaneOnBoardSoftware.csproj.vspscc @@ -0,0 +1,10 @@ +"" +{ +"FILE_VERSION" = "9237" +"ENLISTMENT_CHOICE" = "NEVER" +"PROJECT_FILE_RELATIVE_PATH" = "" +"NUMBER_OF_EXCLUDED_FILES" = "0" +"ORIGINAL_PROJECT_FILE_PATH" = "" +"NUMBER_OF_NESTED_PROJECTS" = "0" +"SOURCE_CONTROL_SETTINGS_PROVIDER" = "PROVIDER" +} diff --git a/Program.cs b/Program.cs new file mode 100644 index 0000000..81130d5 --- /dev/null +++ b/Program.cs @@ -0,0 +1,508 @@ +using System; +using System.IO; +using System.Threading; + +using Microsoft.SPOT; +using Microsoft.SPOT.IO; +using Microsoft.SPOT.Hardware; + +using GHIElectronics.NETMF.FEZ; +using GHIElectronics.NETMF.IO; +using GHIElectronics.NETMF.Hardware; +using GHIElectronics.NETMF.Hardware.LowLevel; + +using System.IO.Ports; +using System.Text; + + +namespace PlaneOnBoardSoftware +{ + public class Program + { + //static PersistentStorage sdPS; + //static string rootDirectory; + + private static ExtendedWeakReference settingsReference; + + static ServoController Servo1 = new ServoController((PWM.Pin)FEZ_Pin.PWM.Di5, 90); + static ServoController Servo2 = new ServoController((PWM.Pin)FEZ_Pin.PWM.Di6, 90); + static LSM303DLM_Accelerometer Accelerometer = new LSM303DLM_Accelerometer(); + static LSM303DLM_Magnetometer Magnetometer = new LSM303DLM_Magnetometer(); + static L3G4200D_Gyro Gyroscope = new L3G4200D_Gyro(); + static BMP085_Barometer Barometer = new BMP085_Barometer(); + static DS18B20_Temperature Thermometer = new DS18B20_Temperature((Cpu.Pin)FEZ_Pin.Digital.Di9); + static PitotTubeSpeedSensor PitotTube = new PitotTubeSpeedSensor(new AnalogIn((AnalogIn.Pin)FEZ_Pin.AnalogIn.An0)); + static BatteryMonitor PowerSource = new BatteryMonitor(new AnalogIn((AnalogIn.Pin)FEZ_Pin.AnalogIn.An7), new AnalogIn((AnalogIn.Pin)FEZ_Pin.AnalogIn.An6)); + static SkyTraqVenus6_GPS GpsRecever = new SkyTraqVenus6_GPS(new SerialPort("COM4")); + static Linksprite_Camera Camera = new Linksprite_Camera(new SerialPort("COM3")); + static RFM22b RadioModem = new RFM22b(SPI.SPI_module.SPI1, (Cpu.Pin)FEZ_Pin.Digital.UEXT5, (Cpu.Pin)FEZ_Pin.Digital.UEXT6, 869.545); + static RFM22b.Channel StatusChannel = RadioModem.NewChannel(0, 32, 512); + static RFM22b.Channel CmdChannel = RadioModem.NewChannel(1, 64, 128); + static SdCardLogger Logger = new SdCardLogger(new SerialPort("COM1")); + static MadgwickAHRS Imu = new MadgwickAHRS(); + static Navigation Navi = new Navigation(); + static PidController PitchCtr = new PidController(-45, 45, 5, 1, 0); + static PidController RollCtr = new PidController(-45, 45, 5, 1, 0); + static PidController YawCtr = new PidController(-45, 45, 1, 5, 0, true); + static PidController SpeedCtr = new PidController(0, -60, 0.5f, 15, 0); + + static Igniter ParachuteRelese = new Igniter((Cpu.Pin)FEZ_Pin.Digital.Di3, 200); + static Igniter BalloonRelese = new Igniter((Cpu.Pin)FEZ_Pin.Digital.Di4, 200); + + static Thread MainLoopThread; + static Thread HelperLoopThread; + static float dt; + static bool ValueUnknown = false; + static bool UseStatusChannel = true; + + static bool DisableServos = false; + static float TargetAirSpeed = 8; //m/s + + public static void Main() + { + Debug.EnableGCMessages(false); + + //sdPS = new PersistentStorage("SD"); + //sdPS.MountFileSystem(); + //rootDirectory = VolumeInfo.GetVolumes()[0].RootDirectory; + + HelperLoopThread = Thread.CurrentThread; + HelperLoopThread.Priority = ThreadPriority.Lowest; + + SetUpMainLoop(); + + int pcount = 260000; + + byte[] Test = new byte[32]; + int PackLen = 1; + + //GHIElectronics.NETMF.Hardware.LowLevel.Watchdog.Enable(5000); + LoadData(); + + while(true) + { + pcount++; + //Camera.SavePictureToSD(rootDirectory + @"\TestPicture_" + pcount + ".jpg"); + Thread.Sleep(1000); + + + GpsRecever.ReadGPSData(); + PowerSource.UpdateData(); + Thermometer.ReadTemperature(); + + PitotTube.Temperature = Thermometer.Temperature2; + PitotTube.BarometricPressure = Barometer.Pressure; + + CheckCml(); + GHIElectronics.NETMF.Hardware.LowLevel.Watchdog.ResetCounter(); + + + if (GpsRecever.Has3DFix && System.Math.Abs(DateTime.UtcNow.Subtract(GpsRecever.GpsTime).Seconds) > 10) + { + Microsoft.SPOT.Time.TimeService.SetTimeZoneOffset(0); + Utility.SetLocalTime(GpsRecever.GpsTime); + Debug.Print("Set Time To: " + GpsRecever.GpsTime); + Microsoft.SPOT.Time.TimeService.SetTimeZoneOffset(120); + } + + + //Debug.Print(Imu.InpMag.X + "; " + Imu.InpMag.Y + "; " + Imu.InpMag.Z); + //Debug.Print("X/Y/Z: " + Imu.Pitch + " / " + Imu.Roll + " / " + Imu.Yaw + " / dt=" + dt); + //Debug.Print("X/Y/Z: " + Imu.InpGyr.X + " / " + Imu.InpGyr.Y + " / " + Imu.InpGyr.Z + " / dt=" + dt); + //Debug.Print("Voltage: " + PowerSource.GetVoltage() + " Temperatur: " + Barometer.Temperature + " °C Pressure: " + (Barometer.Pressure / 100f) + " hPa" + " dt=" + dt); + //Debug.Print("Latitude: " + GpsRecever.Latitude + " Longitude: " + GpsRecever.Longitude + " Höhe: " + GpsRecever.Altitude + " Speed: " + GpsRecever.Speed.Length + " m/s"); + //Debug.Print("vd: " + GpsRecever.VerticalDilution + " hd: " + GpsRecever.HorizontalDilution + " fix: " + GpsRecever.Has3DFix + " Time: " + DateTime.Now + " UTC-Time: " + DateTime.UtcNow + " Voltage: " + PowerSource.GetVoltage() + Barometer.Temperature ); + //Debug.Print("RSSI: " + RadioModem.RSSI); + //Debug.Print("Voltage: " + PowerSource.Voltage + " Current: " + PowerSource.Current + " Energie: " + PowerSource.Energy + " Voltage: " + PowerSource.Charge); + + /*PackLen = 1; + while (PackLen > 0) + { + PackLen = StatusChannel.Recv(Test, 0, 32); + + if (PackLen > 0) + { + try + { + string tmpCa = (new string(UTF8Encoding.UTF8.GetChars(Test))).Substring(0, PackLen); + //Debug.Print(tmpCa); + + } + catch + { + Debug.Print("Error"); + } + + + } + }*/ + //"Latitude: " + GpsRecever.Latitude + " Longitude: " + GpsRecever.Longitude + " Höhe: " + GpsRecever.Altitude + " Speed: " + GpsRecever.Speed.Length + " m/s + + //Debug.Print("Voltage: " + PowerSource.Voltage + " Current: " + PowerSource.Current + " A Charge: " + PowerSource.Charge + " mAh Pressure: " + PitotTube.GetSpeed() + " kPa GPS-Speed: " + GpsRecever.Speed.Length / 3.6f + " km/h Bytes To Send: " + StatusChannel.BytesToSend + " 3D-Fix: " + GpsRecever.Has3DFix.ToString() + " Druck: " + (Barometer.Pressure / 100f) + " mbar Temperature: " + Thermometer.Temperature1 + " °C Temperature: " + Thermometer.Temperature2 + " °C"); + //Debug.Print("Druck: " + (Barometer.Pressure / 100f) + " mbar Temper.:" + Barometer.Temperature + " °C"); + + if (GpsRecever.Has3DFix || true) + { + Logger.Write(DateTime.UtcNow.ToString()); + Logger.Write(";"); + Logger.Write(Navi.Latitude); + Logger.Write(";"); + Logger.Write(Navi.Longitude); + Logger.Write(";"); + Logger.Write(GpsRecever.Altitude); + Logger.Write(";"); + Logger.Write(GpsRecever.Speed.Length); + Logger.Write(";"); + Logger.Write(Imu.Yaw); + Logger.Write(";"); + Logger.Write(Imu.Roll); + Logger.Write(";"); + Logger.Write(Imu.Pitch); + Logger.Write(";"); + Logger.Write(PitotTube.GetSpeed()); + Logger.Write(";"); + Logger.Write(PowerSource.Voltage); + Logger.Write(";"); + Logger.Write(PowerSource.Current); + Logger.Write(";"); + Logger.Write(PowerSource.Energy); + Logger.Write(";"); + Logger.Write(Barometer.Temperature); + Logger.Write(";"); + Logger.Write((Barometer.Pressure / 100f)); + Logger.Write(";"); + Logger.Write(Thermometer.Temperature1); + Logger.Write(";"); + Logger.Write(Thermometer.Temperature2); + Logger.Write(";"); + Logger.Write(Servo1.Position); + Logger.Write(";"); + Logger.Write(Servo2.Position); + Logger.Write(";"); + Logger.Write(Navi.TargetHeading); + Logger.Write(";"); + Logger.Write(Navi.Distance); + Logger.Write(";"); + Logger.Write(GpsRecever.HorizontalDilution); + Logger.Write(";"); + Logger.Write(GpsRecever.Has3DFix.ToString()); + Logger.Write(";"); + Logger.Write(Magnetometer.MagValue.X); + Logger.Write(";"); + Logger.Write(Magnetometer.MagValue.Y); + Logger.Write(";"); + Logger.Write(Magnetometer.MagValue.Z); + Logger.NewLine(); + } + + if (StatusChannel.BytesToSend == 0 && UseStatusChannel) + { + /*byte[] tmp = Encoding.UTF8.GetBytes(DateTime.UtcNow + ";" + GpsRecever.Latitude + ";" + GpsRecever.Longitude + ";" + + GpsRecever.Altitude + ";" + GpsRecever.Speed.Length + ";" + Imu.Yaw +";" + PitotTube.GetSpeed() + + ";" + PowerSource.Voltage + ";" + PowerSource.Current + ";" + PowerSource.Energy + ";" + Barometer.Temperature + ";" + + (Barometer.Pressure / 100f) + ";" + Thermometer.Temperature1 + ";" + Thermometer.Temperature2 + ";" + + Servo1.Position + ";" + Servo2.Position + ";" + RadioModem.RSSI + ";" + RadioModem.LostPackets + "\r\n"); + StatusChannel.Send(tmp, 0, tmp.Length);*/ + + StatusChannel.Send(DateTime.UtcNow + ";" + Navi.Latitude.ToString("f6") + ";" + Navi.Longitude.ToString("f6") + ";" + + GpsRecever.Altitude.ToString("f0") + ";" + GpsRecever.Speed.Length.ToString("f1") + ";" + Imu.Yaw.ToString("f1") + ";" + Imu.Roll.ToString("f1") + ";" + Imu.Pitch.ToString("f1") + ";" + PitotTube.GetSpeed().ToString("f1") + ";" + + PowerSource.Voltage.ToString("f2") + ";" + PowerSource.Current.ToString("f2") + ";" + PowerSource.Energy.ToString("f0") + ";" + Barometer.Temperature.ToString("f1") + ";" + + (Barometer.Pressure / 100f).ToString("f2") + ";" + Thermometer.Temperature1.ToString("f1") + ";" + Thermometer.Temperature2.ToString("f1") + ";" + + Servo1.Position.ToString("f0") + ";" + Servo2.Position.ToString("f0") + ";" + Navi.TargetHeading.ToString("f1") + ";" + Navi.Distance.ToString("f0") + ";" + GpsRecever.HorizontalDilution.ToString("f0") + ";" + GpsRecever.Has3DFix.ToString() + ";" + RadioModem.RSSI + ";" + RadioModem.LostPackets + ";" + Magnetometer.MagValue.Length + "\r\n"); + } + } + } + + public static void CheckCml() + { + string comLine; + string comName; + string comValue; + int i; + + while ((comLine = CmdChannel.RecvLine()).Length > 0) + { + i = comLine.IndexOf('='); + if (i > 0) + { + comName = comLine.Substring(0, i).Trim(); + comValue = comLine.Substring(i + 1).Trim(); + + SetDevProperty(comName, comValue); + if (ValueUnknown) + CmdChannel.SendLine("err " + comName.Trim()); + else + CmdChannel.SendLine("set " + comName + "=" + comValue); + } + else + { + string tempRes = GetDevProperty(comLine.Trim()); + + if (ValueUnknown) + CmdChannel.SendLine("err " + comLine.Trim()); + else + CmdChannel.SendLine("get " + comLine.Trim() + "=" + tempRes); + } + + + } + } + + public static void SetUpMainLoop() + { + MainLoopThread = new Thread(MainLoop); + MainLoopThread.Priority = ThreadPriority.Highest; + MainLoopThread.Start(); + while (!MainLoopThread.IsAlive) ; + } + + public static void MainLoop() + { + bool ledState = false; + //int pressureCount = 0; + OutputPort led = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.LED, ledState); + + DateTime startTime = DateTime.Now; + DateTime newTime; + DateTime lastTimePoint = DateTime.Now; + float AirSpeed; + + while (true) + { + newTime = DateTime.UtcNow; + dt = newTime.Subtract(lastTimePoint).Ticks / (float)TimeSpan.TicksPerSecond; + lastTimePoint = newTime; + + HelperLoopThread.Suspend(); + Accelerometer.RequesAcceleration(); + Magnetometer.RequestMagValue(); + Gyroscope.RequestAngularRate(); + Barometer.RequestSensorData(); + HelperLoopThread.Resume(); + + AirSpeed = PitotTube.GetSpeed(); + + Imu.InpAcc = Accelerometer.Acceleration; + Imu.InpMag = Magnetometer.MagValue; + Imu.InpGyr = Gyroscope.AngularRate; + Imu.InpAirSpeed = AirSpeed; + Imu.Calculate(dt); + + Debug.Print(Imu.InpMag.X + "; " + Imu.InpMag.Y + "; " + Imu.InpMag.Z + "; " + Imu.InpAcc.X + "; " + Imu.InpAcc.Y + "; " + Imu.InpAcc.Z); + + Navi.GpsHasFix = GpsRecever.Has3DFix; + Navi.GpsLongitude = GpsRecever.Longitude; + Navi.GpsLatitude = GpsRecever.Latitude; + Navi.Heading = Imu.Yaw; + Navi.Pitch = Imu.Pitch; + Navi.AirSpeed = AirSpeed; + Navi.Calculate(dt); + + YawCtr.SetPoint = Navi.TargetHeading; + PitchCtr.SetPoint = SpeedCtr.GetOutput(dt, TargetAirSpeed); + RollCtr.SetPoint = YawCtr.GetOutput(dt, Imu.Yaw); + var pitchValue = PitchCtr.GetOutput(dt, Imu.Pitch); + var rollValue = RollCtr.GetOutput(dt, Imu.Roll); + + if (!DisableServos) + { + Servo1.Position = -rollValue - pitchValue; + Servo2.Position = -rollValue + pitchValue; + } + else + { + Servo1.Position = 0; + Servo2.Position = 0; + } + + ledState = !ledState; + led.Write(ledState); + + Thread.Sleep(1); + } + } + + public static void SaveValues() + { + ValueStorageClass Flash = new ValueStorageClass(); + + Flash.WriteFloat(1234f); + Flash.WriteFloat(PitchCtr.P); + Flash.WriteFloat(PitchCtr.I); + Flash.WriteFloat(RollCtr.P); + Flash.WriteFloat(RollCtr.I); + Flash.WriteFloat(YawCtr.P); + Flash.WriteFloat(YawCtr.I); + Flash.WriteBool(DisableServos); + Flash.WriteDouble(RadioModem.Frequency); + Flash.WriteDouble(Navi.DestLatitude); + Flash.WriteDouble(Navi.DestLongitude); + Flash.WriteFloat(TargetAirSpeed); + Flash.WriteFloat(Magnetometer.CalXoffSet); + Flash.WriteFloat(Magnetometer.CalYoffSet); + Flash.WriteFloat(Magnetometer.CalZoffSet); + Flash.WriteFloat(Magnetometer.CalXscale); + Flash.WriteFloat(Magnetometer.CalYscale); + Flash.WriteFloat(Magnetometer.CalZscale); + Flash.WriteFloat(Barometer.OffSetAltitude); + + Flash.WriteToFlash(); + } + + public static void LoadData() + { + ValueStorageClass Flash = new ValueStorageClass(); + + if (Flash.ReadFloat() == 1234) + { + PitchCtr.P = Flash.ReadFloat(); + PitchCtr.I = Flash.ReadFloat(); + RollCtr.P = Flash.ReadFloat(); + RollCtr.I = Flash.ReadFloat(); + YawCtr.P = Flash.ReadFloat(); + YawCtr.I = Flash.ReadFloat(); ; + DisableServos = Flash.ReadBool(); + RadioModem.Frequency = Flash.ReadDouble(); + Navi.DestLatitude = Flash.ReadDouble(); + Navi.DestLongitude = Flash.ReadDouble(); + TargetAirSpeed = Flash.ReadFloat(); + Magnetometer.CalXoffSet = Flash.ReadFloat(); + Magnetometer.CalYoffSet = Flash.ReadFloat(); + Magnetometer.CalZoffSet = Flash.ReadFloat(); + Magnetometer.CalXscale = Flash.ReadFloat(); + Magnetometer.CalYscale = Flash.ReadFloat(); + Magnetometer.CalZscale = Flash.ReadFloat(); + Barometer.OffSetAltitude = Flash.ReadFloat(); + } + } + + public static void SetDevProperty(string Name, string StrValue) + { + double Value; + + try + { + Value = double.Parse(StrValue); + } + catch + { + Value = 0; + } + ValueUnknown = false; + + switch(Name) + { + case "PitchCtrP": + PitchCtr.P = (float)Value; + break; + case "PitchCtrI": + PitchCtr.I = (float)Value; + break; + case "RollCtrP": + RollCtr.P = (float)Value; + break; + case "RollCtrI": + RollCtr.I = (float)Value; + break; + case "YawCtrP": + YawCtr.P = (float)Value; + break; + case "YawCtrI": + YawCtr.I = (float)Value; + break; + case "DisableServos": + DisableServos = (Value == 1); + break; + case "FireParachute": + if (Value > 0) ParachuteRelese.Fire(); + break; + case "ReleseBalloon": + if (Value > 0) BalloonRelese.Fire(); + break; + case "RadioFrequency": + RadioModem.Frequency = Value; + break; + case "UseStatusChannel": + UseStatusChannel = (Value > 0); + break; + case "DestLatitude": + Navi.DestLatitude = (float)Value; + break; + case "DestLongitude": + Navi.DestLongitude = (float)Value; + break; + case "TargetAirSpeed": + TargetAirSpeed = (float)Value; + break; + case "CalibrationMode": + Magnetometer.CalibrationMode = (Value == 1); + break; + case "Altitude": + Barometer.OffSetAltitude = (float)Value - Barometer.Altitude; + break; + default: + ValueUnknown = true; + break; + } + + if (!ValueUnknown) SaveValues(); + } + + public static string GetDevProperty(string Name) + { + string ret = ""; + + ValueUnknown = false; + switch (Name) + { + case "PitchCtrP": + ret = PitchCtr.P.ToString(); + break; + case "PitchCtrI": + ret = PitchCtr.I.ToString(); + break; + case "RollCtrP": + ret = RollCtr.P.ToString(); + break; + case "RollCtrI": + ret = RollCtr.I.ToString(); + break; + case "YawCtrP": + ret = YawCtr.P.ToString(); + break; + case "YawCtrI": + ret = YawCtr.I.ToString(); + break; + case "DisableServos": + ret = DisableServos ? "1" : "0"; + break; + case "RadioFrequency": + ret = RadioModem.Frequency.ToString(); + break; + case "UseStatusChannel": + ret = UseStatusChannel ? "1" : "0"; + break; + case "DestLatitude": + ret = Navi.DestLatitude.ToString(); + break; + case "DestLongitude": + ret = Navi.DestLongitude.ToString(); + break; + case "TargetAirSpeed": + ret = TargetAirSpeed.ToString(); + break; + case "CalibrationMode": + ret = Magnetometer.CalibrationMode ? "1" : "0"; + break; + case "Altitude": + ret = Barometer.Altitude.ToString(); + break; + default: + ValueUnknown = true; + break; + } + + return ret; + } + } +} diff --git a/Resources.Designer.cs b/Resources.Designer.cs new file mode 100644 index 0000000..6540924 --- /dev/null +++ b/Resources.Designer.cs @@ -0,0 +1,38 @@ +//------------------------------------------------------------------------------ +// +// This code was generated by a tool. +// Runtime Version:2.0.50727.42 +// +// Changes to this file may cause incorrect behavior and will be lost if +// the code is regenerated. +// +//------------------------------------------------------------------------------ + +namespace PlaneOnBoardSoftware +{ + + internal class Resources + { + private static System.Resources.ResourceManager manager; + internal static System.Resources.ResourceManager ResourceManager + { + get + { + if ((Resources.manager == null)) + { + Resources.manager = new System.Resources.ResourceManager("HighAltitude.Resources", typeof(Resources).Assembly); + } + return Resources.manager; + } + } + internal static string GetString(Resources.StringResources id) + { + return ((string)(Microsoft.SPOT.ResourceUtility.GetObject(ResourceManager, id))); + } + [System.SerializableAttribute()] + internal enum StringResources : short + { + String1 = 1228, + } + } +} diff --git a/Resources.resx b/Resources.resx new file mode 100644 index 0000000..91007d5 --- /dev/null +++ b/Resources.resx @@ -0,0 +1,123 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + text/microsoft-resx + + + 2.0 + + + System.Resources.ResXResourceReader, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + System.Resources.ResXResourceWriter, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + Hello World! + + \ No newline at end of file diff --git a/Rotation.cs b/Rotation.cs new file mode 100644 index 0000000..4a57cfd --- /dev/null +++ b/Rotation.cs @@ -0,0 +1,266 @@ +using System; +using Microsoft.SPOT; +using GHIElectronics.NETMF.System; + +namespace PlaneOnBoardSoftware +{ + public class Vector3D + { + public float X; + public float Y; + public float Z; + + private static float degPerPi = 57.29577951308f; + + public static Vector3D xAxis = new Vector3D(1, 0, 0); + public static Vector3D yAxis = new Vector3D(0, 1, 0); + public static Vector3D zAxis = new Vector3D(0, 0, 1); + + public Vector3D() + { + this.X = 0; + this.Y = 0; + this.Z = 0; + } + + public Vector3D(float x, float y, float z) + { + this.X = x; + this.Y = y; + this.Z = z; + } + + public float Length + { + get { return (float)MathEx.Sqrt(X * X + Y * Y + Z * Z); } + } + + public Vector3D Normated() + { + return new Vector3D(X / Length, Y / Length, Z / Length); + } + + public static float VectorAngle(Vector3D Vector1, Vector3D Vector2) + { + return (float)MathEx.Acos(DotProduct(Vector1, Vector2) / Vector1.Length / Vector2.Length) * degPerPi; + } + + public static float DotProduct(Vector3D Vector1, Vector3D Vector2) + { + return Vector1.X * Vector2.X + Vector1.Y * Vector2.Y + Vector1.Z * Vector2.Z; + } + + public static Vector3D CrossProduct(Vector3D Vector1, Vector3D Vector2) + { + Vector3D result = new Vector3D(); + + result.X = Vector1.Y * Vector2.Z - Vector1.Z * Vector2.Y; + result.Y = Vector1.Z * Vector2.X - Vector1.X * Vector2.Z; + result.Z = Vector1.X * Vector2.Y - Vector1.Y * Vector2.X; + + return result; + } + + public static Vector3D operator *(Vector3D c1, Vector3D c2) + { + return CrossProduct(c1, c2); + } + } + + public class Quaternion + { + public float X; + public float Y; + public float Z; + public float W; + + private static float degPerPi = 57.29577951308f; + + public Quaternion() + { + this.X = 0; + this.Y = 0; + this.Z = 0; + this.W = 1; + } + + public Quaternion(float x, float y, float z, float w) + { + this.X = x; + this.Y = y; + this.Z = z; + this.W = w; + } + + public Quaternion(Vector3D RotationAxis, float Angle) + { + Angle = Angle / degPerPi / 2; + + float c = (float)MathEx.Cos(Angle); + float s = (float)MathEx.Sin(Angle); + + if (RotationAxis.Length > float.Epsilon) + { + RotationAxis = RotationAxis.Normated(); + + X = RotationAxis.X * s; + Y = RotationAxis.Y * s; + Z = RotationAxis.Z * s; + W = c; + } + else + { + X = 0; Y = 0; Z = 0; + W = 1; + } + } + + public static Quaternion GetFastQuaternion(Vector3D NormatedRotationAxis, float Angle) + { + Angle = Angle / degPerPi / 2f; + float s = (float)MathEx.Sin(Angle); + + return new Quaternion(NormatedRotationAxis.X * s, NormatedRotationAxis.Y * s, NormatedRotationAxis.Z * s, (float)MathEx.Cos(Angle)); + } + + public void XAxisTurn(float Angle) + { + Angle = Angle / degPerPi / 2f; + float s = (float)MathEx.Sin(Angle); + float c = (float)MathEx.Cos(Angle); + + float tx = X; //s + float ty = Y; //0 + float tz = Z; //0 + float tw = W; //c + + W = tw * c - tx * s; + X = tw * s + tx * c; + Y = ty * c + tz * s; + Z = -ty * s + tz * c; + } + + public void YAxisTurn(float Angle) + { + Angle = Angle / degPerPi / 2f; + float s = (float)MathEx.Sin(Angle); + float c = (float)MathEx.Cos(Angle); + + float tx = X; //0 + float ty = Y; //s + float tz = Z; //0 + float tw = W; //c + + W = tw * c - ty * s; + X = tx * c - tz * s; + Y = tw * s + ty * c; + Z = tx * s + tz * c; + } + + public void ZAxisTurn(float Angle) + { + Angle = Angle / degPerPi / 2f; + float s = (float)MathEx.Sin(Angle); + float c = (float)MathEx.Cos(Angle); + + float tx = X; //0 + float ty = Y; //0 + float tz = Z; //s + float tw = W; //c + + W = tw * c - tz * s; + X = tx * c + ty * s; + Y = -tx * s + ty * c; + Z = tw * s + tz * c; + } + + public float Angle + { + get { return (float)MathEx.Acos(W) * 2.0f * degPerPi; } + } + + public static Quaternion operator *(Quaternion c1, Quaternion c2) + { + return hamiltonProduct(c1, c2); + } + + private static Quaternion hamiltonProduct(Quaternion q1, Quaternion q2) + { + var result = new Quaternion(); + + result.W = q1.W * q2.W - q1.X * q2.X - q1.Y * q2.Y - q1.Z * q2.Z; + result.X = q1.W * q2.X + q1.X * q2.W + q1.Y * q2.Z - q1.Z * q2.Y; + result.Y = q1.W * q2.Y - q1.X * q2.Z + q1.Y * q2.W + q1.Z * q2.X; + result.Z = q1.W * q2.Z + q1.X * q2.Y - q1.Y * q2.X + q1.Z * q2.W; + + return result; + } + + public void Normate() + { + float length = (float)MathEx.Sqrt(X * X + Y * Y + Z * Z); + + X = X / length; + Y = Y / length; + Z = Z / length; + } + + public Quaternion Conjugate() + { + return new Quaternion(W, -X, -Y, -Z); + } + + public Vector3D RotateVector(Vector3D Vector) + { + return QuaternionRotation(this, Vector); + } + + private static Vector3D QuaternionRotation(Quaternion q, Vector3D v) + { + Vector3D result = new Vector3D(); + float a00 = q.W * q.W; + float a01 = q.W * q.X; + float a02 = q.W * q.Y; + float a03 = q.W * q.Z; + float a11 = q.X * q.X; + float a12 = q.X * q.Y; + float a13 = q.X * q.Z; + float a22 = q.Y * q.Y; + float a23 = q.Y * q.Z; + float a33 = q.Z * q.Z; + result.X = v.X * (a00 + a11 - a22 - a33) + 2 * (a12 * v.Y + a13 * v.Z + a02 * v.Z - a03 * v.Y); + result.Y = v.Y * (a00 - a11 + a22 - a33) + 2 * (a12 * v.X + a23 * v.Z + a03 * v.X - a01 * v.Z); + result.Z = v.Z * (a00 - a11 - a22 + a33) + 2 * (a13 * v.X + a23 * v.Y - a02 * v.X + a01 * v.Y); + + return result; + } + + public Vector3D GetEulerAngles() + { + return GetEulerAnglesFromQuaternion(this); + } + + private static Vector3D GetEulerAnglesFromQuaternion(Quaternion q) + { + Vector3D result = new Vector3D(); + float a01 = 2 * q.W * q.X; + float a02 = 2 * q.W * q.Y; + float a03 = 2 * q.W * q.Z; + float a11 = 2 * q.X * q.X; + float a12 = 2 * q.X * q.Y; + float a13 = 2 * q.X * q.Z; + float a22 = 2 * q.Y * q.Y; + float a23 = 2 * q.Y * q.Z; + float a33 = 2 * q.Z * q.Z; + //result.Y = (float)MathEx.Atan2(a01 + a23, 1 - (a11 + a22)) * degPerPi; + //result.X = (float)MathEx.Asin(a02 - a13) * degPerPi; + //result.Z = (float)MathEx.Atan2(a03 + a12, 1 - (a22 + a33)) * degPerPi; + + result.Y = (float)MathEx.Atan2(a02 - a13, 1 - (a22 + a11)) * degPerPi; + result.X = (float)MathEx.Asin(a01 + a23) * degPerPi; + result.Z = (float)MathEx.Atan2(a03 - a12, 1 - (a11 + a33)) * degPerPi; + + return result; + } + } +} diff --git a/ValueStorageClass.cs b/ValueStorageClass.cs new file mode 100644 index 0000000..2fc0e8b --- /dev/null +++ b/ValueStorageClass.cs @@ -0,0 +1,164 @@ +using System; +using System.Runtime.InteropServices; +using GHIElectronics.NETMF.System; +using GHIElectronics.NETMF.Hardware; + +namespace PlaneOnBoardSoftware +{ + class ValueStorageClass + { + class Serializing + { + private static double Abs(double Number) + { + if (Number > 0) + return Number; + else + return -Number; + } + + public static void WriteBool(bool Value, ref byte[] ByteArray, ref int OffSet) + { + ByteArray[OffSet] = (byte)(Value ? 1 : 0); + OffSet += 1; + } + + public static bool ReadBool(ref byte[] ByteArray, ref int OffSet) + { + OffSet += 1; + return ByteArray[OffSet-1] == 1; + } + + public static void WriteByte(byte Number, ref byte[] ByteArray, ref int OffSet) + { + ByteArray[OffSet] = Number; + OffSet += 1; + } + + public static byte ReadByte(ref byte[] ByteArray, ref int OffSet) + { + OffSet += 1; + return ByteArray[OffSet-1]; + } + + public static void WriteFloat(float Number, ref byte[] ByteArray, ref int OffSet) + { + int sigPos = (int)MathEx.Floor(MathEx.Log10(Abs(Number))); + int intNumb = (int)(Number * MathEx.Pow(10, -sigPos + 5)); + + if (intNumb < 0) intNumb += 0xFFFFFF; + + ByteArray[OffSet + 0] = (byte)(sigPos + 128); + ByteArray[OffSet + 1] = (byte)(intNumb >> 16); + ByteArray[OffSet + 2] = (byte)((intNumb >> 8) & 0xFF); + ByteArray[OffSet + 3] = (byte)(intNumb & 0xFF); + + OffSet += 4; + } + + public static float ReadFloat(ref byte[] ByteArray, ref int OffSet) + { + int sigPos = ByteArray[OffSet + 0] - 128; + int intNumb = ByteArray[OffSet + 1]; + intNumb = (intNumb << 8) + ByteArray[OffSet + 2]; + intNumb = (intNumb << 8) + ByteArray[OffSet + 3]; + + if (intNumb > 0x7FFFFF) intNumb -= 0xFFFFFF; + + OffSet += 4; + return (float)(intNumb * MathEx.Pow(10, sigPos - 5)); + } + + public static void WriteDouble(double Number, ref byte[] ByteArray, ref int OffSet) + { + + int sigPos = (int)MathEx.Floor(MathEx.Log10(Abs(Number))); + long intNumb = (long)(Number * MathEx.Pow(10, -sigPos + 14)); + + if (intNumb < 0) intNumb += 0xFFFFFFFFFFFFFF; + + ByteArray[OffSet + 0] = (byte)(sigPos + 128); + ByteArray[OffSet + 7] = (byte)(intNumb & 0xFF); + intNumb = (intNumb >> 8); + ByteArray[OffSet + 6] = (byte)(intNumb & 0xFF); + intNumb = (intNumb >> 8); + ByteArray[OffSet + 5] = (byte)(intNumb & 0xFF); + intNumb = (intNumb >> 8); + ByteArray[OffSet + 4] = (byte)(intNumb & 0xFF); + intNumb = (intNumb >> 8); + ByteArray[OffSet + 3] = (byte)(intNumb & 0xFF); + intNumb = (intNumb >> 8); + ByteArray[OffSet + 2] = (byte)(intNumb & 0xFF); + intNumb = (intNumb >> 8); + ByteArray[OffSet + 1] = (byte)(intNumb); + + OffSet += 4; + } + + public static double ReadDouble(ref byte[] ByteArray, ref int OffSet) + { + long sigPos = ByteArray[OffSet + 0] - 128; + long intNumb = ByteArray[OffSet + 1]; + + intNumb = (intNumb << 8) + ByteArray[OffSet + 2]; + intNumb = (intNumb << 8) + ByteArray[OffSet + 3]; + intNumb = (intNumb << 8) + ByteArray[OffSet + 4]; + intNumb = (intNumb << 8) + ByteArray[OffSet + 5]; + intNumb = (intNumb << 8) + ByteArray[OffSet + 6]; + intNumb = (intNumb << 8) + ByteArray[OffSet + 7]; + + if (intNumb > 0x7FFFFFFFFFFFFF) intNumb -= 0xFFFFFFFFFFFFFF; + + OffSet += 4; + return (intNumb * MathEx.Pow(10, sigPos - 14)); + } + } + + private int StorageSize = 0; + private int OffSet = 0; + private byte[] buffer; + + public ValueStorageClass() + { + StorageSize = InternalFlashStorage.Size; + OffSet = 0; + buffer = new byte[StorageSize]; + InternalFlashStorage.Read(buffer); + } + + public void WriteToFlash() + { + InternalFlashStorage.Write(buffer); + } + + public void WriteBool(bool Value) + { + Serializing.WriteBool(Value, ref buffer, ref OffSet); + } + + public void WriteFloat(float Value) + { + Serializing.WriteFloat(Value, ref buffer, ref OffSet); + } + + public void WriteDouble(double Value) + { + Serializing.WriteDouble(Value, ref buffer, ref OffSet); + } + + public bool ReadBool() + { + return Serializing.ReadBool(ref buffer, ref OffSet); + } + + public float ReadFloat() + { + return Serializing.ReadFloat(ref buffer, ref OffSet); + } + + public double ReadDouble() + { + return Serializing.ReadDouble(ref buffer, ref OffSet); + } + } +}