nicolas
/
NETMF_Autopilot
mirror of https://github.com/Nonannet/NETMF_Autopilot.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity

Add files via upload

This commit is contained in:
Nonannet 2018-12-05 10:50:00 +01:00 committed by GitHub
commit 38e94e2d80
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
28 changed files with 3769 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

138
Hardware/BMP085_Barometer.cs Normal file
View File

@ -0,0 +1,138 @@
using System;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using GHIElectronics.NETMF.System;
namespace PlaneOnBoardSoftware
{
/// <summary>
/// Absolute Pressure
/// </summary>
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; }
}
}
}

60
Hardware/BatteryMonitor.cs Normal file
View File

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

88
Hardware/DS18B20_Temperature.cs Normal file
View File

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

23
Hardware/FEZ_Low_Level.cs Normal file
View File

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

48
Hardware/I2CBase.cs Normal file
View File

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

39
Hardware/I2CBus.cs Normal file
View File

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

27
Hardware/Igniter.cs Normal file
View File

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

145
Hardware/L3G4200D_Gyro.cs Normal file
View File

@ -0,0 +1,145 @@
using System;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
namespace PlaneOnBoardSoftware
{
/// <summary>
/// Accelerometer
/// </summary>
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; }
}
}
}

130
Hardware/LSM303DLM_Accelerometer.cs Normal file
View File

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

193
Hardware/LSM303DLM_Magnetometer.cs Normal file
View File

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

332
Hardware/Linksprite_Camera.cs Normal file
View File

@ -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();
}
}
}

34
Hardware/PitotTubeSpeedSensor.cs Normal file
View File

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

663
Hardware/RFM22b.cs Normal file
View File

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

54
Hardware/SdCardLogger.cs Normal file
View File

@ -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();
}
}
}

36
Hardware/ServoController.cs Normal file
View File

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

187
Hardware/SkyTraqVenus6_GPS.cs Normal file
View File

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

10
HighAltitude.csproj.vspscc Normal file
View File

@ -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"
}

203
MadgwickAHRS.cs Normal file
View File

@ -0,0 +1,203 @@
using System;
using Microsoft.SPOT;
using GHIElectronics.NETMF.System;
namespace PlaneOnBoardSoftware
{
/// <summary>
/// MadgwickAHRS class. Implementation of Madgwick's IMU and AHRS algorithms.
/// GNU General Public Licence
/// </summary>
/// <remarks>
/// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
/// Modified 2012 by Nicolas Kruse
/// </remarks>
class MadgwickAHRS
{
/// <summary>AirSpeed in m/s</summary>
public float InpAirSpeed = 0;
/// <summary>Acceleration in m/s²</summary>
public Vector3D InpAcc = new Vector3D();
/// <summary>Angular rate in deg/s</summary>
public Vector3D InpGyr = new Vector3D();
public Vector3D InpMag = new Vector3D();
/// <summary>Ascent/descent angle</summary>
public float Pitch = 0;
/// <summary>Roll angle</summary>
public float Roll = 0;
/// <summary>Attitude angle</summary>
public float Yaw = 0;
/// <summary>
/// Gets or sets the algorithm gain beta.
/// </summary>
public float Beta = 0.1f;
/// <summary>
/// Gets or sets the Quaternion output.
/// </summary>
private float[] Quat = {0, 0, 0, 1};
private float SamplePeriod;
private static float piPerDeg = 0.017453292519f;
private static float degPerPi = 57.29577951308f;
/// <summary>
/// Algorithm AHRS update method. Requires only gyroscope and accelerometer data.
/// </summary>
/// <param name="gx">
/// Gyroscope x axis measurement in radians/s.
/// </param>
/// <param name="gy">
/// Gyroscope y axis measurement in radians/s.
/// </param>
/// <param name="gz">
/// Gyroscope z axis measurement in radians/s.
/// </param>
/// <param name="ax">
/// Accelerometer x axis measurement in any calibrated units.
/// </param>
/// <param name="ay">
/// Accelerometer y axis measurement in any calibrated units.
/// </param>
/// <param name="az">
/// Accelerometer z axis measurement in any calibrated units.
/// </param>
/// <param name="mx">
/// Magnetometer x axis measurement in any calibrated units.
/// </param>
/// <param name="my">
/// Magnetometer y axis measurement in any calibrated units.
/// </param>
/// <param name="mz">
/// Magnetometer z axis measurement in any calibrated units.
/// </param>
/// <remarks>
/// Optimised for minimal arithmetic.
/// Total ±: 160
/// Total *: 172
/// Total /: 5
/// Total sqrt: 5
/// </remarks>
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;
}
}
}
}

79
Navigation.cs Normal file
View File

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

76
PidController.cs Normal file
View File

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

88
PlaneOnBoardSoftware.csproj Normal file
View File

@ -0,0 +1,88 @@
<?xml version="1.0" encoding="utf-8"?>
<Project DefaultTargets="Build" ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
<PropertyGroup>
<AssemblyName>FEZ Mini Application</AssemblyName>
<OutputType>Exe</OutputType>
<RootNamespace>HighAltitude</RootNamespace>
<ProjectTypeGuids>{b69e3092-b931-443c-abe7-7e7b65f2a37f};{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}</ProjectTypeGuids>
<ProductVersion>9.0.21022</ProductVersion>
<SchemaVersion>2.0</SchemaVersion>
<ProjectGuid>{2CC55691-0251-403D-BFC2-C44CDAA6B2B7}</ProjectGuid>
<TargetFrameworkVersion>v4.1</TargetFrameworkVersion>
<NetMfTargetsBaseDir Condition="'$(NetMfTargetsBaseDir)'==''">$(MSBuildExtensionsPath32)\Microsoft\.NET Micro Framework\</NetMfTargetsBaseDir>
<SccProjectName>SAK</SccProjectName>
<SccLocalPath>SAK</SccLocalPath>
<SccAuxPath>SAK</SccAuxPath>
<SccProvider>SAK</SccProvider>
</PropertyGroup>
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
<DebugSymbols>true</DebugSymbols>
<DebugType>full</DebugType>
<Optimize>false</Optimize>
<OutputPath>bin\Debug\</OutputPath>
<DefineConstants>DEBUG;TRACE</DefineConstants>
<ErrorReport>prompt</ErrorReport>
<WarningLevel>4</WarningLevel>
</PropertyGroup>
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
<DebugType>pdbonly</DebugType>
<Optimize>true</Optimize>
<OutputPath>bin\Release\</OutputPath>
<DefineConstants>TRACE</DefineConstants>
<ErrorReport>prompt</ErrorReport>
<WarningLevel>4</WarningLevel>
</PropertyGroup>
<Import Project="$(NetMfTargetsBaseDir)$(TargetFrameworkVersion)\CSharp.Targets" />
<ItemGroup>
<Compile Include="Hardware\BatteryMonitor.cs" />
<Compile Include="Hardware\DS18B20_Temperature.cs" />
<Compile Include="Hardware\Igniter.cs" />
<Compile Include="Hardware\SdCardLogger.cs" />
<Compile Include="Hardware\SkyTraqVenus6_GPS.cs" />
<Compile Include="Hardware\Linksprite_Camera.cs" />
<Compile Include="MadgwickAHRS.cs" />
<Compile Include="PidController.cs" />
<Compile Include="Hardware\PitotTubeSpeedSensor.cs" />
<Compile Include="Hardware\FEZ_Low_Level.cs" />
<Compile Include="Hardware\I2CBase.cs" />
<Compile Include="Hardware\I2CBus.cs" />
<Compile Include="Hardware\BMP085_Barometer.cs" />
<Compile Include="Hardware\L3G4200D_Gyro.cs" />
<Compile Include="Hardware\LSM303DLM_Magnetometer.cs" />
<Compile Include="Hardware\LSM303DLM_Accelerometer.cs" />
<Compile Include="Properties\AssemblyInfo.cs" />
<Compile Include="Program.cs" />
<Compile Include="Resources.Designer.cs">
<AutoGen>True</AutoGen>
<DesignTime>True</DesignTime>
<DependentUpon>Resources.resx</DependentUpon>
</Compile>
<Compile Include="Hardware\RFM22b.cs" />
<Compile Include="Hardware\ServoController.cs" />
<Compile Include="Navigation.cs" />
<Compile Include="Rotation.cs" />
<Compile Include="ValueStorageClass.cs" />
</ItemGroup>
<ItemGroup>
<EmbeddedResource Include="Resources.resx">
<SubType>Designer</SubType>
<Generator>ResXFileCodeGenerator</Generator>
<LastGenOutput>Resources.Designer.cs</LastGenOutput>
</EmbeddedResource>
</ItemGroup>
<ItemGroup>
<Reference Include="FEZMini_GHIElectronics.NETMF.FEZ, Version=4.1.8.0, Culture=neutral, processorArchitecture=MSIL" />
<Reference Include="GHIElectronics.NETMF.Hardware, Version=4.1.8.0, Culture=neutral, processorArchitecture=MSIL" />
<Reference Include="GHIElectronics.NETMF.IO, Version=4.1.8.0, Culture=neutral, processorArchitecture=MSIL" />
<Reference Include="GHIElectronics.NETMF.System, Version=4.1.8.0, Culture=neutral, processorArchitecture=MSIL" />
<Reference Include="Microsoft.SPOT.Hardware" />
<Reference Include="Microsoft.SPOT.Hardware.SerialPort" />
<Reference Include="Microsoft.SPOT.IO" />
<Reference Include="Microsoft.SPOT.Native" />
<Reference Include="Microsoft.SPOT.Time" />
<Reference Include="System.IO" />
</ItemGroup>
<ItemGroup>
<Folder Include="NotUsed\" />
</ItemGroup>
</Project>

7
PlaneOnBoardSoftware.csproj.user Normal file
View File

@ -0,0 +1,7 @@
<?xml version="1.0" encoding="utf-8"?>
<Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
<PropertyGroup Condition="'$(Configuration)|$(Platform)' == 'Debug|AnyCPU'">
<DeployTransport>USB</DeployTransport>
<DeployDevice>USBizi</DeployDevice>
</PropertyGroup>
</Project>

10
PlaneOnBoardSoftware.csproj.vspscc Normal file
View File

@ -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"
}

508
Program.cs Normal file
View File

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

38
Resources.Designer.cs generated Normal file
View File

@ -0,0 +1,38 @@
//------------------------------------------------------------------------------
// <auto-generated>
// 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.
// </auto-generated>
//------------------------------------------------------------------------------
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,
}
}
}

123
Resources.resx Normal file
View File

@ -0,0 +1,123 @@
<?xml version="1.0" encoding="utf-8"?>
<root>
<!--
Microsoft ResX Schema
Version 2.0
The primary goals of this format is to allow a simple XML format
that is mostly human readable. The generation and parsing of the
various data types are done through the TypeConverter classes
associated with the data types.
Example:
... ado.net/XML headers & schema ...
<resheader name="resmimetype">text/microsoft-resx</resheader>
<resheader name="version">2.0</resheader>
<resheader name="reader">System.Resources.ResXResourceReader, System.Windows.Forms, ...</resheader>
<resheader name="writer">System.Resources.ResXResourceWriter, System.Windows.Forms, ...</resheader>
<data name="Name1"><value>this is my long string</value><comment>this is a comment</comment></data>
<data name="Color1" type="System.Drawing.Color, System.Drawing">Blue</data>
<data name="Bitmap1" mimetype="application/x-microsoft.net.object.binary.base64">
<value>[base64 mime encoded serialized .NET Framework object]</value>
</data>
<data name="Icon1" type="System.Drawing.Icon, System.Drawing" mimetype="application/x-microsoft.net.object.bytearray.base64">
<value>[base64 mime encoded string representing a byte array form of the .NET Framework object]</value>
<comment>This is a comment</comment>
</data>
There are any number of "resheader" rows that contain simple
name/value pairs.
Each data row contains a name, and value. The row also contains a
type or mimetype. Type corresponds to a .NET class that support
text/value conversion through the TypeConverter architecture.
Classes that don't support this are serialized and stored with the
mimetype set.
The mimetype is used for serialized objects, and tells the
ResXResourceReader how to depersist the object. This is currently not
extensible. For a given mimetype the value must be set accordingly:
Note - application/x-microsoft.net.object.binary.base64 is the format
that the ResXResourceWriter will generate, however the reader can
read any of the formats listed below.
mimetype: application/x-microsoft.net.object.binary.base64
value : The object must be serialized with
: System.Runtime.Serialization.Formatters.Binary.BinaryFormatter
: and then encoded with base64 encoding.
mimetype: application/x-microsoft.net.object.soap.base64
value : The object must be serialized with
: System.Runtime.Serialization.Formatters.Soap.SoapFormatter
: and then encoded with base64 encoding.
mimetype: application/x-microsoft.net.object.bytearray.base64
value : The object must be serialized into a byte array
: using a System.ComponentModel.TypeConverter
: and then encoded with base64 encoding.
-->
<xsd:schema id="root" xmlns="" xmlns:xsd="http://www.w3.org/2001/XMLSchema" xmlns:msdata="urn:schemas-microsoft-com:xml-msdata">
<xsd:import namespace="http://www.w3.org/XML/1998/namespace" />
<xsd:element name="root" msdata:IsDataSet="true">
<xsd:complexType>
<xsd:choice maxOccurs="unbounded">
<xsd:element name="metadata">
<xsd:complexType>
<xsd:sequence>
<xsd:element name="value" type="xsd:string" minOccurs="0" />
</xsd:sequence>
<xsd:attribute name="name" use="required" type="xsd:string" />
<xsd:attribute name="type" type="xsd:string" />
<xsd:attribute name="mimetype" type="xsd:string" />
<xsd:attribute ref="xml:space" />
</xsd:complexType>
</xsd:element>
<xsd:element name="assembly">
<xsd:complexType>
<xsd:attribute name="alias" type="xsd:string" />
<xsd:attribute name="name" type="xsd:string" />
</xsd:complexType>
</xsd:element>
<xsd:element name="data">
<xsd:complexType>
<xsd:sequence>
<xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
<xsd:element name="comment" type="xsd:string" minOccurs="0" msdata:Ordinal="2" />
</xsd:sequence>
<xsd:attribute name="name" type="xsd:string" use="required" msdata:Ordinal="1" />
<xsd:attribute name="type" type="xsd:string" msdata:Ordinal="3" />
<xsd:attribute name="mimetype" type="xsd:string" msdata:Ordinal="4" />
<xsd:attribute ref="xml:space" />
</xsd:complexType>
</xsd:element>
<xsd:element name="resheader">
<xsd:complexType>
<xsd:sequence>
<xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
</xsd:sequence>
<xsd:attribute name="name" type="xsd:string" use="required" />
</xsd:complexType>
</xsd:element>
</xsd:choice>
</xsd:complexType>
</xsd:element>
</xsd:schema>
<resheader name="resmimetype">
<value>text/microsoft-resx</value>
</resheader>
<resheader name="version">
<value>2.0</value>
</resheader>
<resheader name="reader">
<value>System.Resources.ResXResourceReader, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</resheader>
<resheader name="writer">
<value>System.Resources.ResXResourceWriter, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
</resheader>
<data name="String1" xml:space="preserve">
<value>Hello World!</value>
</data>
</root>

266
Rotation.cs Normal file
View File

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

164
ValueStorageClass.cs Normal file
View File

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