*

Unity/Assets/Scripts/AccumulatorAlarm.cs

@@ -0,0 +1,20 @@
+using UnityEngine;
+using System.Collections;
+
+public class AccumulatorAlarm : MonoBehaviour
+{
+    
+
+    // Use this for initialization
+	void Start ()
+	{
+        //   var val = voltage.ToString("0.00");
+
+
+	}
+	
+	// Update is called once per frame
+	void Update () {
+	
+	}
+}

Unity/Assets/Scripts/AccumulatorAlarm.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 0429b6da746ae534daab4982b75de5dc
+timeCreated: 1452356245
+licenseType: Free
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/AdvancedGizmo.cs

@@ -0,0 +1,64 @@
+using UnityEngine;
+using System.Collections;
+
+public static class AdvancedGizmo {
+
+	public enum CircleCloseType{
+		None, 
+		Direct,
+		ToCenter
+	}
+
+	public static void Circle(float radius, float angle = 360.0f, int tesselation = 12, CircleCloseType close = CircleCloseType.None, Matrix4x4? transform = null){
+		var matrixBackup = Gizmos.matrix;
+		Gizmos.matrix = transform.HasValue ? transform.Value : Matrix4x4.identity;
+	
+		for (int i = 0; i < tesselation; ++i) {
+			float angle0 = i * (angle * Mathf.Deg2Rad / tesselation);
+			float angle1 = (i + 1) * (angle * Mathf.Deg2Rad / tesselation);
+
+			var p0 = new Vector3(Mathf.Sin(angle0) * radius, 0, Mathf.Cos(angle0) * radius);
+			var p1 = new Vector3(Mathf.Sin(angle1) * radius, 0, Mathf.Cos(angle1) * radius);
+
+			Gizmos.DrawLine(p0, p1);
+		}
+
+		if (close != CircleCloseType.None) {
+			float angle1 = angle * Mathf.Deg2Rad;
+			var p0 = new Vector3(Mathf.Sin(0) * radius, 0, Mathf.Cos(0) * radius);
+			var p1 = new Vector3(Mathf.Sin(angle1) * radius, 0, Mathf.Cos(angle1) * radius);
+
+			if(close == CircleCloseType.Direct){
+				Gizmos.DrawLine(p0, p1);
+			}else{
+				Gizmos.DrawLine(p0, Vector3.zero);
+				Gizmos.DrawLine(p1, Vector3.zero);
+			}
+		}
+		Gizmos.matrix = matrixBackup;
+	}
+
+	public static void CircleArrow(float radius, float angle = 360.0f, int tesselation = 12,  CircleCloseType close = CircleCloseType.None, Matrix4x4? transform = null){
+		Circle(radius, angle, tesselation, close, transform);
+		float len = 0.03f;
+		float wid = 0.01f;
+		float angleOffset = 2.0f * Mathf.Asin (len / (2.0f * radius)) * Mathf.Sign(angle);
+
+		var matrixBackup = Gizmos.matrix;
+		Gizmos.matrix = transform.HasValue ? transform.Value : Matrix4x4.identity;
+
+		var radiusVector = new Vector3(Mathf.Sin(0), 0, Mathf.Cos(0));
+
+		var p1 = new Vector3(Mathf.Sin(angleOffset), 0, Mathf.Cos(angleOffset)) * radius;
+		var p0 = radiusVector * radius;
+
+		var dir = (p1 - p0).normalized;
+		var dir2 = Vector3.Cross (dir, Vector3.up).normalized;
+
+		Gizmos.DrawLine(p0, p1 + dir2 * wid);
+		Gizmos.DrawLine(p0, p1 - dir2 * wid);
+		Gizmos.DrawLine(p1 + dir2 * wid, p1 - dir2 * wid);
+
+		Gizmos.matrix = matrixBackup;
+	}
+}

Unity/Assets/Scripts/AdvancedGizmo.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 10ff9e1069ff610488e0106f7baa9503
+timeCreated: 1423391656
+licenseType: Pro
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/AdvancedPhysics.cs

@@ -0,0 +1,26 @@
+using UnityEngine;
+using System.Collections;
+
+public static class AdvancedPhysics {
+
+	public static void AddTorqueAtPosition(this Rigidbody body, Vector3 torque, Vector3 position, ref Vector3 force, ref Vector3 ortho){
+		Vector3 torqueAxis = torque.normalized;
+		ortho = new Vector3(1,0,0);
+
+		if ( (torqueAxis-ortho).sqrMagnitude < 0.01f ) {
+			ortho = new Vector3(0,1,0);
+		}
+
+		Vector3.OrthoNormalize(ref torqueAxis, ref ortho);
+		force = Vector3.Cross(0.5f * torque, ortho);
+
+		body.AddForceAtPosition (force, position + ortho);
+		body.AddForceAtPosition (-force, position - ortho);
+	}
+
+	public static void AddTorqueAtPosition(this Rigidbody body, Vector3 torque, Vector3 position){
+		Vector3 force = new Vector3 ();
+		Vector3 ortho = new Vector3 ();
+		AddTorqueAtPosition (body, torque, position, ref force, ref ortho);
+	}
+}

Unity/Assets/Scripts/AdvancedPhysics.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: af667e3c43d9f2a418ed51318ecc1a3b
+timeCreated: 1423542666
+licenseType: Pro
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/Altimeter.cs

@@ -0,0 +1,6 @@
+using UnityEngine;
+using System.Collections;
+
+public abstract class Altimeter : Device {
+	public abstract float GetHeight();
+}

Unity/Assets/Scripts/Altimeter.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 68169fe171afc8c4da28278f090c7b84
+timeCreated: 1423550689
+licenseType: Pro
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/BiquadFilter.cs

@@ -0,0 +1,172 @@
+using UnityEngine;
+using System.Collections;
+using System;
+
+public enum BiquadFilterType {
+	Lowpass = 0,
+	Highpass,
+	Bandpass,
+	Notch,
+	Peak,
+	Lowshelf,
+	Highshelf
+};
+
+[Serializable]
+public class BiquadFilter {
+	//new Biquad(bq_type_peak, 200.0 / sampleRate, 1.0, -3.0);
+
+	protected BiquadFilterType filterType;
+	protected double a0, a1, a2, b1, b2;
+	public double Fc, Q, peakGain;
+	protected double z1, z2;
+
+	public BiquadFilter() {
+		filterType = BiquadFilterType.Lowpass;
+		a0 = 1.0f;
+		a1 = a2 = b1 = b2 = 0.0f;
+		Fc = 0.50f;
+		Q = 0.707f;
+		peakGain = 0.0f;
+		z1 = z2 = 0.0f;
+	}
+
+	public BiquadFilter(BiquadFilterType _filterType, double _Fc, double _Q, double _peakGainD) {
+		SetBiquad(_filterType, _Fc, _Q, _peakGainD);
+		z1 = z2 = 0.0f;
+	}
+	public void SetBiquad(BiquadFilterType _filterType, double _Fc, double _Q, double _peakGainDB) {
+		filterType = _filterType;
+		Q = _Q;
+		Fc = _Fc;
+		SetPeakGain(_peakGainDB);
+	}
+
+	public void SetType(BiquadFilterType _filterType) {
+		filterType = _filterType;
+		CalcBiquad();
+	}
+	
+	public void SetQ(double _Q) {
+		Q = _Q;
+		CalcBiquad();
+	}
+	
+	public void SetFc(double _Fc) {
+		Fc = _Fc;
+		CalcBiquad();
+	}
+
+
+
+	public void SetPeakGain(double _peakGainDB) {
+		peakGain = _peakGainDB;
+		CalcBiquad();
+	}
+
+	public void CalcBiquad() {
+		double norm;
+		double V = System.Math.Pow(10.0, System.Math.Abs(peakGain) / 20.0);
+		double K = System.Math.Tan(3.14159265358979 * Fc);
+		switch (filterType) {
+		case BiquadFilterType.Lowpass:
+			norm = 1 / (1 + K / Q + K * K);
+			a0 = K * K * norm;
+			a1 = 2 * a0;
+			a2 = a0;
+			b1 = 2 * (K * K - 1) * norm;
+			b2 = (1 - K / Q + K * K) * norm;
+			break;
+			
+		case BiquadFilterType.Highpass:
+			norm = 1 / (1 + K / Q + K * K);
+			a0 = 1 * norm;
+			a1 = -2 * a0;
+			a2 = a0;
+			b1 = 2 * (K * K - 1) * norm;
+			b2 = (1 - K / Q + K * K) * norm;
+			break;
+			
+		case BiquadFilterType.Bandpass:
+			norm = 1 / (1 + K / Q + K * K);
+			a0 = K / Q * norm;
+			a1 = 0;
+			a2 = -a0;
+			b1 = 2 * (K * K - 1) * norm;
+			b2 = (1 - K / Q + K * K) * norm;
+			break;
+			
+		case BiquadFilterType.Notch:
+			norm = 1 / (1 + K / Q + K * K);
+			a0 = (1 + K * K) * norm;
+			a1 = 2 * (K * K - 1) * norm;
+			a2 = a0;
+			b1 = a1;
+			b2 = (1 - K / Q + K * K) * norm;
+			break;
+			
+		case BiquadFilterType.Peak:
+			if (peakGain >= 0) {    // boost
+				norm = 1 / (1 + 1/Q * K + K * K);
+				a0 = (1 + V/Q * K + K * K) * norm;
+				a1 = 2 * (K * K - 1) * norm;
+				a2 = (1 - V/Q * K + K * K) * norm;
+				b1 = a1;
+				b2 = (1 - 1/Q * K + K * K) * norm;
+			}
+			else {    // cut
+				norm = 1 / (1 + V/Q * K + K * K);
+				a0 = (1 + 1/Q * K + K * K) * norm;
+				a1 = 2 * (K * K - 1) * norm;
+				a2 = (1 - 1/Q * K + K * K) * norm;
+				b1 = a1;
+				b2 = (1 - V/Q * K + K * K) * norm;
+			}
+			break;
+		case BiquadFilterType.Lowshelf:
+			if (peakGain >= 0) {    // boost
+				norm = 1 / (1 + System.Math.Sqrt(2) * K + K * K);
+				a0 = (1 + System.Math.Sqrt(2*V) * K + V * K * K) * norm;
+				a1 = 2 * (V * K * K - 1) * norm;
+				a2 = (1 - System.Math.Sqrt(2*V) * K + V * K * K) * norm;
+				b1 = 2 * (K * K - 1) * norm;
+				b2 = (1 - System.Math.Sqrt(2) * K + K * K) * norm;
+			}
+			else {    // cut
+				norm = 1 / (1 + System.Math.Sqrt(2*V) * K + V * K * K);
+				a0 = (1 + System.Math.Sqrt(2) * K + K * K) * norm;
+				a1 = 2 * (K * K - 1) * norm;
+				a2 = (1 - System.Math.Sqrt(2) * K + K * K) * norm;
+				b1 = 2 * (V * K * K - 1) * norm;
+				b2 = (1 - System.Math.Sqrt(2*V) * K + V * K * K) * norm;
+			}
+			break;
+		case BiquadFilterType.Highshelf:
+			if (peakGain >= 0) {    // boost
+				norm = 1 / (1 + System.Math.Sqrt(2) * K + K * K);
+				a0 = (V + System.Math.Sqrt(2*V) * K + K * K) * norm;
+				a1 = 2 * (K * K - V) * norm;
+				a2 = (V - System.Math.Sqrt(2*V) * K + K * K) * norm;
+				b1 = 2 * (K * K - 1) * norm;
+				b2 = (1 - System.Math.Sqrt(2) * K + K * K) * norm;
+			}
+			else {    // cut
+				norm = 1 / (V + System.Math.Sqrt(2*V) * K + K * K);
+				a0 = (1 + System.Math.Sqrt(2) * K + K * K) * norm;
+				a1 = 2 * (K * K - 1) * norm;
+				a2 = (1 - System.Math.Sqrt(2) * K + K * K) * norm;
+				b1 = 2 * (K * K - V) * norm;
+				b2 = (V - System.Math.Sqrt(2*V) * K + K * K) * norm;
+			}
+			break;
+		}
+		return;
+	}
+
+	public double Process(double input) {
+		double output = input * a0 + z1;
+		z1 = input * a1 + z2 - b1 * output;
+		z2 = input * a2 - b2 * output;
+		return output;
+	}
+}

Unity/Assets/Scripts/BiquadFilter.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: f6464d43440f27d438b7a694cc926299
+timeCreated: 1425818051
+licenseType: Free
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/CameraController.cs

@@ -0,0 +1,77 @@
+using UnityEngine;
+using System.Collections;
+
+
+[ExecuteInEditMode]
+public class CameraController : MonoBehaviour {
+	public bool Active = false;
+	public float sensitivity = 2F;
+
+	float rotationY = 0F;
+	
+	Vector3 Speed = new Vector3();
+	public float MaxSpeed = 10f;
+	public float AccelerationTime = 1f;
+	public float BrakingTime = 0.5f;
+	
+	
+	void Start ()
+	{
+		rotationY = -transform.localEulerAngles.x;
+	}
+	
+	void Update ()
+	{
+		if(Input.GetKeyDown("mouse 1")){
+			Active = !Active;
+		}
+		
+		Screen.lockCursor = Active;
+		
+		if (!Active) return;
+		
+		
+		
+	
+		float rotationX = transform.localEulerAngles.y + Input.GetAxis("Mouse X") * sensitivity;
+			
+		rotationY += Input.GetAxis("Mouse Y") * sensitivity;
+		rotationY = Mathf.Clamp (rotationY, -90, 90);
+		
+		transform.localEulerAngles = new Vector3(-rotationY, rotationX, 0);
+		
+		
+		Vector3 AccelerationForce = new Vector3(
+			((Input.GetKey("d")? 1f:0f)-(Input.GetKey("a")? 1f:0f)),
+			((Input.GetKey("space")? 1f:0f)-(Input.GetKey("left ctrl")? 1f:0f)),
+			((Input.GetKey("w")? 1f:0f)-(Input.GetKey("s")? 1f:0f))
+		);
+		AccelerationForce *= (Time.deltaTime/AccelerationTime);
+		Speed += AccelerationForce;
+		
+		Vector3 BrakingForce = new Vector3(
+			(Input.GetKey("d")|Input.GetKey("a"))? 0f:1f,
+			(Input.GetKey("space")|Input.GetKey("left ctrl"))? 0f:1f,
+			(Input.GetKey("w")|Input.GetKey("s"))? 0f:1f
+		);
+		
+		BrakingForce *= (Time.deltaTime/BrakingTime);
+		
+		
+	
+		Speed.Set(
+			Speed.x-(Mathf.Sign(Speed.x)*Mathf.Min(BrakingForce.x,Mathf.Abs(Speed.x))),
+			Speed.y-(Mathf.Sign(Speed.y)*Mathf.Min(BrakingForce.y,Mathf.Abs(Speed.y))),
+			Speed.z-(Mathf.Sign(Speed.z)*Mathf.Min(BrakingForce.z,Mathf.Abs(Speed.z))));
+		
+		Speed.Set(
+			Mathf.Clamp(Speed.x,-1f,1f),
+			Mathf.Clamp(Speed.y,-1f,1f),
+			Mathf.Clamp(Speed.z,-1f,1f));
+		
+		
+		transform.Translate(Time.deltaTime*Speed*MaxSpeed,Space.Self);
+	}
+	
+
+}

Unity/Assets/Scripts/CameraController.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: cc5ee2ad9517a694cb75ffbb0484358f
+timeCreated: 1423679043
+licenseType: Pro
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/CameraFollowController.cs

@@ -0,0 +1,25 @@
+using UnityEngine;
+using System.Collections;
+
+public class CameraFollowController : MonoBehaviour {
+
+	public Transform target;
+	public float distance = 1.0f;
+	public float height = 1.0f;
+
+	// Use this for initialization
+	void Start () {
+	
+	}
+	
+	// Update is called once per frame
+	void Update () {
+		if (target != null) {
+			var forward = target.transform.forward;
+			forward.y = 0;
+			forward.Normalize();
+			transform.position = target.transform.position - forward * distance + Vector3.up * height;
+			transform.LookAt(target.transform);
+		}
+	}
+}

Unity/Assets/Scripts/CameraFollowController.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 0bd8f3cbef6afba4db1d564a22882f54
+timeCreated: 1424010338
+licenseType: Pro
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/Device.cs

@@ -0,0 +1,7 @@
+using UnityEngine;
+using System.Collections;
+
+public class Device : MonoBehaviour {
+
+
+}

Unity/Assets/Scripts/Device.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 4a4ccac7b93309e47aefca780f9ba0a3
+timeCreated: 1423550428
+licenseType: Pro
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/Editor.meta

@@ -0,0 +1,9 @@
+fileFormatVersion: 2
+guid: 5284d4a98c08d4e44aacf3bda118844e
+folderAsset: yes
+timeCreated: 1423910038
+licenseType: Pro
+DefaultImporter:
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/Editor/AutoSaveOnRun.cs

@@ -0,0 +1,20 @@
+using UnityEngine;
+ using UnityEditor;
+ 
+ [InitializeOnLoad]
+ public class AutosaveOnRun
+ {
+     static AutosaveOnRun()
+     {
+         EditorApplication.playmodeStateChanged = () =>
+         {
+             if(EditorApplication.isPlayingOrWillChangePlaymode && !EditorApplication.isPlaying)
+             {
+                 Debug.Log("Auto-Saving scene before entering Play mode: " + EditorApplication.currentScene);
+                 
+                 EditorApplication.SaveScene();
+                 EditorApplication.SaveAssets();
+             }
+         };
+     }
+ }

Unity/Assets/Scripts/Editor/AutoSaveOnRun.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: b9a811bc67f439d42af246b9e870de83
+timeCreated: 1451992256
+licenseType: Free
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/Editor/InputEditor.cs

@@ -0,0 +1,75 @@
+using UnityEditor;
+using UnityEngine;
+
+class InputEditor : EditorWindow {
+	[MenuItem ("L/InputEditor")]
+	static void ShowWindow () {
+		EditorWindow.GetWindow (typeof(InputEditor));
+	}
+
+	public enum AxisType{
+		KeyOrMouseButton = 0,
+		MouseMovement = 1,
+		JoystickAxis = 2
+	}
+	private bool _renameAxis;
+
+	void OnGUI () {
+
+		_renameAxis = GUILayout.Toggle (_renameAxis, "Rename axis");
+
+		if (GUILayout.Button ("Reset input axes")) {
+			var serializedObject = new SerializedObject(AssetDatabase.LoadAllAssetsAtPath("ProjectSettings/InputManager.asset")[0]);
+			
+			int axisId = 0;
+			while(true){
+				var axesProperty = serializedObject.FindProperty("m_Axes");
+				axesProperty.Next(true);
+				axesProperty.Next(true);
+				axesProperty.Next(true);
+				
+				bool fail = false;
+				for(int i = 0; i < axisId; ++i){
+					if(!axesProperty.Next(false)){
+						fail = true;
+						break;
+					}
+				}
+				
+				if(fail){
+					break;
+				}
+				
+				int propId = 0;
+				//go deeper - into axis properties
+				axesProperty.Next(true);
+				do{
+					switch(propId){
+					case 0:
+						if(_renameAxis){
+							axesProperty.stringValue = "Axis" + axisId;
+						}
+
+						break;
+					case 9: //sensitivity
+						axesProperty.floatValue = 1.0f;
+						break;
+					case 10: //snap
+					case 11: //invert
+						break;
+					case 12: //type
+						axesProperty.enumValueIndex = (int)AxisType.JoystickAxis;
+						break;
+					case 13: //axis
+						axesProperty.intValue = axisId;
+						break;
+					}
+
+					propId++;
+				}while(axesProperty.Next(false) && propId < 15);
+				axisId++;
+			}
+			serializedObject.ApplyModifiedProperties();
+		}
+	}
+}

Unity/Assets/Scripts/Editor/InputEditor.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 1583d288f90da744085ad61412896bb2
+timeCreated: 1423910048
+licenseType: Pro
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/Editor/MotorEditor.cs

@@ -0,0 +1,23 @@
+using UnityEngine;
+using UnityEditor;
+using System.Collections;
+
+[CustomEditor(typeof(MotorPlot))]
+public class MotorEditor : Editor {
+    public override void OnInspectorGUI() {
+        base.OnInspectorGUI();
+
+        if (GUILayout.Button("Update unbound plots")) {
+            (target as MotorPlot).DrawUnboundPlots();
+        }
+
+        if (GUILayout.Button("Update plots")) {
+            (target as MotorPlot).IntegratePlotsVisual();
+        }
+
+        if (GUILayout.Button("Optimize")) {
+            (target as MotorPlot).Optimize();
+        }
+    }
+}
+

Unity/Assets/Scripts/Editor/MotorEditor.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 074d4210038f618409b1f613122e619f
+timeCreated: 1452020381
+licenseType: Free
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/Editor/TransformEx.cs

@@ -0,0 +1,22 @@
+using UnityEngine;
+using System.Collections;
+using UnityEditor;
+
+[CustomEditor(typeof(Transform))]
+public class TransformEditor : Editor {
+	public override void OnInspectorGUI (){
+
+		base.OnInspectorGUI ();
+		EditorGUILayout.BeginHorizontal();
+		if (GUILayout.Button ("Reset rotaton")) {
+			(target as Transform).rotation = Quaternion.identity;
+		}
+		if (GUILayout.Button ("R +90")) {
+			(target as Transform).Rotate(0, 90, 0);
+		}
+		if (GUILayout.Button ("R -90")) {
+			(target as Transform).Rotate(0, -90, 0);
+		}
+		EditorGUILayout.EndHorizontal();
+	}
+}

Unity/Assets/Scripts/Editor/TransformEx.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 72c86113d776a354397477e591e59d48
+timeCreated: 1425919706
+licenseType: Free
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/Editor/UnityResourcesEditor.cs

@@ -0,0 +1,59 @@
+using UnityEngine;
+using UnityEditor;
+using System.Linq;
+using System;
+
+public class UnityResourcesEditor : EditorWindow {
+	public string resourcesAssetPath = @"Assets/unity editor resources.asset";
+	public GUISkin LightSkin;
+	public GUISkin DarkSkin;
+
+	[MenuItem ("L/UnityResourcesEditor")]
+	static void Init () {
+		UnityResourcesEditor window = (UnityResourcesEditor)EditorWindow.GetWindow (typeof (UnityResourcesEditor));
+		window.Show();
+	}
+
+	void CopyProperties<T>(T from, T to, string[] exclude = null){
+		var props = typeof(T).GetProperties ().Where(v=>v.CanWrite && v.CanWrite).ToArray();
+		foreach (var p in props) {
+			Debug.Log(p.Name);
+			if(exclude != null && exclude.Any(v => v == p.Name)){
+				Debug.Log("Skin property: " + p.Name);
+				continue;
+			}
+			p.SetValue(to,  p.GetValue(from, null), null);
+		}
+	}
+
+	void CopyGuiSkin(GUISkin from, GUISkin to){
+		Debug.Log ("Copy skins");
+		CopyProperties (from, to, new []{"name"});
+
+		CopyProperties (from.settings, to.settings);
+	}
+	
+	void OnGUI () {
+		GUILayout.Label ("Asset path", EditorStyles.boldLabel);
+		resourcesAssetPath = GUILayout.TextField (resourcesAssetPath);
+
+		if (GUILayout.Button ("Load resources")) {
+			var resources = AssetDatabase.LoadAllAssetsAtPath (resourcesAssetPath);
+			var skins = resources.Where(v=>v is GUISkin).Cast<GUISkin>().ToArray();
+
+			var lightSkin = skins.First(v=>v.name.ToLower() == "LightSkin".ToLower());
+			var darkSkin = skins.First(v=>v.name.ToLower() == "DarkSkin".ToLower());
+
+			CopyGuiSkin(darkSkin, lightSkin);
+
+			Debug.Log("Light skin name: " + lightSkin.name);
+			Debug.Log("Dark skin name: " + darkSkin.name);
+
+			AssetDatabase.SaveAssets();
+
+
+
+		}
+
+	}
+}

Unity/Assets/Scripts/Editor/UnityResourcesEditor.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 7a576cb8317355048a2b1652b6df62a2
+timeCreated: 1425716261
+licenseType: Free
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/ElectronicSpeedController.cs

@@ -0,0 +1,31 @@
+using UnityEngine;
+using System.Collections;
+
+public class ElectronicSpeedController : MonoBehaviour {
+	public Motor motor;
+	public bool inverseDirection;
+
+	[Range(0, 1)]
+	public float inputSpeed;
+
+	// Use this for initialization
+	void Start () {
+	
+	}
+
+	void OnDrawGizmos(){
+		AdvancedGizmo.CircleArrow (0.1f, inverseDirection ? -270 : 270, 37, AdvancedGizmo.CircleCloseType.None, motor.transform.localToWorldMatrix);
+	}
+
+	// Update is called once per frame
+	void Update () {
+		inputSpeed = Mathf.Clamp01 (inputSpeed);
+
+		if (motor != null) {
+			motor.speed = motor.maxRpm * inputSpeed * (inverseDirection ? -1.0f : 1.0f);
+			var rps = motor.speed / 60.0f;
+			var rotateAngle = rps * 360.0f * Time.deltaTime;
+			motor.rotor.transform.Rotate(0, 0, rotateAngle);
+		}
+	}
+}

Unity/Assets/Scripts/ElectronicSpeedController.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: ecbaaf54d5305d04db5720bb7624cea1
+timeCreated: 1423328533
+licenseType: Pro
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/FlightController.cs

@@ -0,0 +1,8 @@
+using UnityEngine;
+using System.Collections;
+
+public class FlightController : Device {
+	public Quaternion targetOrientation = Quaternion.identity;
+	public float targetThrolle;
+	public OrientationSensor orientationSensor;
+}

Unity/Assets/Scripts/FlightController.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 86fc4779855f897408768cb72207114b
+timeCreated: 1423552125
+licenseType: Pro
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/IdealAltimeter.cs

@@ -0,0 +1,8 @@
+using UnityEngine;
+using System.Collections;
+
+public class IdealAltimeter : Altimeter {
+	public override float GetHeight (){
+		return transform.position.y;
+	}
+}

Unity/Assets/Scripts/IdealAltimeter.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: dc6c39b608ed5e347a44373f193265d6
+timeCreated: 1423551172
+licenseType: Pro
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/Joystick.cs

@@ -0,0 +1,32 @@
+using UnityEngine;
+using System.Collections;
+
+[RequireComponent(typeof(RectTransform))]
+public class Joystick : MonoBehaviour {
+	public JoystickViewController view;
+
+	public string XAxis = "Yaw";
+	public string YAxis = "Throlle";
+
+	private Vector2 _state;
+
+	public Vector2 State{
+		get{ return _state;}
+	}
+
+	// Use this for initialization
+	void Start () {
+
+	}
+	
+	// Update is called once per frame
+	void Update () {
+		_state.x = Input.GetAxis (XAxis);
+		_state.y = Input.GetAxis (YAxis);
+		view.SetAxis (_state.x, _state.y);
+	}
+
+	void OnGUI(){
+
+	}
+}

Unity/Assets/Scripts/Joystick.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: dbb9dd3ef7d4b28499544535a3be7fcb
+timeCreated: 1423866951
+licenseType: Pro
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/JoystickView.cs

@@ -0,0 +1,74 @@
+using UnityEngine;
+using UnityEditor;
+using System.Collections;
+using System.Collections.Generic;
+using UnityEditor;
+
+public class JoystickView : MonoBehaviour {
+
+	// Use this for initialization
+	void Start () {
+	
+	}
+	
+	// Update is called once per frame
+	void Update () {
+	
+	}
+
+	void OnGUI(){
+
+		DoGui ();
+	}
+
+	public static List<string> GetInputAxis()
+	{
+		var allAxis = new List<string>();
+		
+		var serializedObject = new SerializedObject(AssetDatabase.LoadAllAssetsAtPath("ProjectSettings/InputManager.asset")[0]);
+		var axesProperty = serializedObject.FindProperty("m_Axes");
+		
+		axesProperty.Next(true);
+		axesProperty.Next(true);
+		while (axesProperty.Next(false))
+		{
+			SerializedProperty axis = axesProperty.Copy();
+			axis.Next(true);
+			allAxis.Add(axis.stringValue);
+		}
+		
+		return allAxis;
+	}
+
+	void DoGui(){
+		var joysticks = Input.GetJoystickNames ();
+		
+		#if UNITY_EDITOR
+		Handles.BeginGUI ();
+		#endif
+
+		var axes = GetInputAxis ();
+		
+		float height = 20;
+		for (int i = 0; i < axes.Count; ++i) {
+			GUI.Label(new Rect(10,i * height + 10, 300,height), string.Format("Axis {0}: {1}", axes[i], Input.GetAxis(axes[i])));
+		}
+
+		for (int i = 0; i < 20; ++i) {
+			GUI.Label(new Rect(410,i * height + 10, 300,height), string.Format("Button {0}: {1}", i, Input.GetKey (KeyCode.Joystick1Button0 + i)));
+		}
+
+
+		/*foreach(var j in joysticks){
+
+			//EditorGUILayout.TextField(j);
+		}*/
+		#if UNITY_EDITOR
+		Handles.EndGUI ();
+		#endif
+	}
+	
+	void OnDrawGizmos(){
+		DoGui ();
+	}
+}

Unity/Assets/Scripts/JoystickView.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 16d4b57de1550a940ab33fd142eeb0de
+timeCreated: 1423907729
+licenseType: Pro
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/JoystickViewController.cs

@@ -0,0 +1,39 @@
+using UnityEngine;
+using System.Collections;
+using UnityEngine.UI;
+
+public class JoystickViewController : MonoBehaviour {
+	private Vector2 _axisValue;
+	public RectTransform axisView;
+	public RectTransform axisViewParent;
+
+	public void SetAxis(float x, float y){
+		_axisValue.x = Mathf.Clamp(x, -1.0f, 1.0f);
+		_axisValue.y = Mathf.Clamp(y, -1.0f, 1.0f);
+
+		if (_axisValue.sqrMagnitude > 1.0f) {
+			_axisValue.Normalize ();
+		}
+
+
+
+		float parentWidth = axisViewParent.rect.width;
+		float parentHeight = axisViewParent.rect.height;
+
+		var pos = axisView.localPosition;
+		pos.x = _axisValue.x * (parentWidth * 0.5f);
+		pos.y = _axisValue.y * (parentHeight * 0.5f);
+		axisView.localPosition = pos;
+	}
+
+
+	// Use this for initialization
+	void Start () {
+	
+	}
+	
+	// Update is called once per frame
+	void Update () {
+
+	}
+}

Unity/Assets/Scripts/JoystickViewController.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 70459ef9cfb07c64b8a545219e0ba3b0
+timeCreated: 1423904821
+licenseType: Pro
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/Motor.cs

@@ -0,0 +1,51 @@
+using UnityEngine;
+using System.Collections;
+
+public class Motor : MonoBehaviour {
+	public GameObject rotor;
+	public float maxRpm = 3000;
+	public float speed;
+	public float minPitch = 0.8f;
+	public float maxPitch = 1.2f;
+	public AudioSource soundSource;
+
+	private float[] speedHistory;
+	int historyId = 0;
+
+	void Start(){
+		speedHistory = new float[15];
+		for (int i = 0; i < speedHistory.Length; ++i) {
+			speedHistory[i] = 0;
+		}
+	}
+
+	void Update(){
+		if (soundSource != null) {
+
+			speedHistory[historyId] = speed;
+			historyId = (historyId + 1) % speedHistory.Length;
+			float smoothSpeed = 0;
+			
+			for (int i = 0; i < speedHistory.Length; ++i) {
+				smoothSpeed += speedHistory[i];
+			}
+			smoothSpeed = smoothSpeed / (float)speedHistory.Length;
+
+
+			float pitch = (smoothSpeed / maxRpm) * (maxPitch - minPitch) + minPitch;
+		
+
+
+			soundSource.pitch = pitch;
+
+			if(soundSource.isPlaying && speed < 5){
+				soundSource.Stop();
+			}
+
+			if(!soundSource.isPlaying && speed >=5){
+				soundSource.Play();
+			}
+		
+		}
+	}
+}

Unity/Assets/Scripts/Motor.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 0b1922fe0093758438859302e80d5f1f
+timeCreated: 1423314461
+licenseType: Pro
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/MotorPlot.cs

@@ -0,0 +1,264 @@
+using UnityEngine;
+using System.Collections;
+using System.Collections.Generic;
+using UnityEditor;
+using System.Linq;
+using System;
+
+public class MotorPlot : MonoBehaviour {
+
+    private Vector3[] plot_current;
+    private Vector3[] plot_rps;
+    private Vector3[] plot_power;
+    private Vector3[] plot_in_power;
+    private Vector3[] plot_eff;
+
+
+    public Motor motor;
+
+    const float plotHeight = 10.0f;
+    const float plotWidth = plotHeight * 1.25f;
+
+    Vector3[] CreatePlot(Plot plot) {
+        Vector3[] result = new Vector3[plot.Samples.Count];
+        for (int i = 0; i < plot.Samples.Count; ++i) {
+            float x = (plotWidth / plot.Samples.Count) * i;
+            result[i] = new Vector3(x, 0, (float)(plot.Samples[i] * plotHeight * plot.NormalizeValue));
+        }
+        return result;
+    }
+
+    public class Plot {
+        public Plot(double _NormalizeValue) {
+            NormalizeValue = _NormalizeValue;
+        }
+        public double NormalizeValue;
+        public List<double> Samples = new List<double>();
+
+        public double GetMin() {
+            return Samples.Min();
+        }
+
+        public double GetMinNormalized() {
+            return Samples.Min() * NormalizeValue;
+        }
+
+        public double GetMaxNormalized() {
+            return Samples.Max() * NormalizeValue;
+        }
+
+        public double GetMax() {
+            return Samples.Max();
+        }
+    }
+
+    public class Plots {
+        public Plot current = new Plot(1000.0 / 200.0);
+        public Plot rpm = new Plot(1.0 / 24000.0);
+        public Plot power = new Plot((1.0 * 1000.0f / 150.0));
+        public Plot in_power = new Plot((1.0 * 1000.0f / 150.0));
+        public Plot eff = new Plot(100.0 / 54.0);
+    }
+
+    private double k_torque_backup;
+    private double k_friction_backup;
+    private double k_backemf_backup;
+    public int maxOptimizationIterations = 10000;
+    public int integrationIterations = 1000;
+
+    private static void fvec(double[] arg, double[] fi, object obj) {
+        //errors:  eff_max, pow_max, rpm_max, cur_min, cur_max
+        //k_Friction, k_Torque, k_BackEMF, rotorInertia
+
+        var m = (MotorPlot)obj;
+        m.motor.k_Friction = m.k_friction_backup + arg[0];
+        m.motor.k_Torque = m.k_torque_backup + arg[1];
+        m.motor.k_BackEMF = m.k_backemf_backup + arg[2];
+
+        var plots = m.IntegratePlots();
+
+        var eff_max_norm = plots.eff.GetMaxNormalized();
+        var pow_max_norm = plots.power.GetMaxNormalized();
+        var cur_max_norm = plots.current.GetMaxNormalized();
+
+        var target_eff_max_norm = (52.0 / 100.0) * plots.eff.NormalizeValue;
+        var target_pow_max_norm = (120.3 / 1000.0) * plots.power.NormalizeValue;
+        var target_rpm_max_norm = 23050.0 * plots.rpm.NormalizeValue;
+        var target_cur_min_norm = (17.0 / 1000.0) * plots.current.NormalizeValue;
+        var target_cur_max_norm = (180.0 / 1000.0) * plots.current.NormalizeValue;
+
+
+        var target_rpm_derivative = (0 - target_rpm_max_norm) / plotWidth;
+        var iter_rpm_derivative = (plots.rpm.GetMinNormalized() - (plots.rpm.Samples[plots.rpm.Samples.Count / 2] * plots.rpm.NormalizeValue)) / (plotWidth / 2);
+
+        var target_cur_derivative = (target_cur_max_norm - target_cur_min_norm) / plotWidth;
+        var iter_cur_derivative = (plots.current.GetMaxNormalized() - (plots.current.Samples[plots.current.Samples.Count / 2] * plots.current.NormalizeValue)) / (plotWidth / 2);
+
+        var err_eff = target_eff_max_norm - eff_max_norm;
+        var err_pow = target_pow_max_norm - pow_max_norm;
+        var err_rpm_deriv = target_rpm_derivative - iter_rpm_derivative;
+        var err_cur = target_cur_derivative - iter_cur_derivative;
+        var err_cur_max = target_cur_max_norm - cur_max_norm;
+
+        fi[0] = err_pow * err_pow;
+        fi[1] = err_rpm_deriv * err_rpm_deriv;
+        fi[2] = err_cur * err_cur;
+        fi[3] = err_cur_max * err_cur_max;
+        fi[4] = err_eff * err_eff;
+    }
+
+    public void Optimize() {
+        const double optStep = 0.0000001;
+        k_torque_backup = motor.k_Torque;
+        k_friction_backup = motor.k_Friction;
+        k_backemf_backup = motor.k_BackEMF;
+
+        double[] Params = new double[3] { 0, 0, 0 };
+
+        alglib.minlmstate state;
+        alglib.minlmreport rep;
+
+        try {
+            //params count, errors count, params
+            alglib.minlmcreatev(Params.Length, 6, Params, optStep, out state);
+        } catch (Exception ex) {
+            Debug.Log("Failed to optimize, bad params! " + ex.Message);
+            return;
+        }
+
+
+        alglib.minlmsetcond(state, 0.00000000001, 0.00000000001, 0.00000000001, maxOptimizationIterations);
+        //alglib.minlmsetbc(state, FunctionParametersMin, FunctionParametersMax);
+        alglib.minlmoptimize(state, fvec, null, this);
+
+        alglib.minlmresults(state, out Params, out rep);
+
+
+        motor.k_Friction = k_friction_backup + Params[0];
+        motor.k_Torque = k_torque_backup + Params[1];
+        motor.k_BackEMF = k_backemf_backup + Params[2];
+
+        Debug.Log(rep.iterationscount);
+        IntegratePlotsVisual();
+    }
+
+
+
+    public void DrawUnboundPlots() {
+        Plots result = new Plots();
+        motor.ResetState();
+
+        plot_eff = null;
+        plot_in_power = null;
+        plot_power = null;
+        plot_rps = null;
+        plot_current = null;
+
+        var oldVoltage = motor.Vin;
+
+        for (int i = 0; i < integrationIterations; ++i) {
+
+            if (i > (integrationIterations / 2)) {
+                motor.Vin = 0;
+            }
+
+            result.current.Samples.Add(motor.current);
+            result.rpm.Samples.Add(motor.angularSpeedRPM);
+            motor.Integrate(motor.integrationStep);
+        }
+        motor.Vin = oldVoltage;
+        plot_rps = CreatePlot(result.rpm);
+        plot_current = CreatePlot(result.current);
+        SceneView.RepaintAll();
+    }
+
+    public Plots IntegratePlots() {
+        Plots result = new Plots();
+        motor.ResetState();
+
+        motor.LoadTorque = 0;
+        double loadStep = 0.04185 * motor.integrationStep;
+
+        //warm up
+        for (int i = 0; i < (int)(1.0f / motor.integrationStep) * 100; ++i) {
+            motor.Integrate(motor.integrationStep);
+        }
+
+        int iteration = 0;
+
+        //add load torque until motor stall
+        while (motor.angularSpeedRPM > 0.0) {
+            ++iteration;
+            motor.LoadTorque = loadStep * iteration;
+
+            // double power = (k_BackEMF / k_Torque) * angularSpeedRPS * (torque - torqueOffset);
+            double power = motor.angularSpeedRPS * motor.LoadTorque / 1000.0;
+            double inPower = motor.current * motor.Vin;
+            double eff = power / inPower;
+
+            result.current.Samples.Add(motor.current);
+            result.rpm.Samples.Add(motor.angularSpeedRPM);
+            result.eff.Samples.Add(eff);
+            result.power.Samples.Add(power);
+            result.in_power.Samples.Add(inPower);
+
+            for (int m = 0; m < 10; ++m) {
+                motor.Integrate(motor.integrationStep);
+            }
+
+        }
+        Debug.Log("Load torque = " + motor.LoadTorque);
+        return result;
+    }
+
+    public void IntegratePlotsVisual() {
+        var plots = IntegratePlots();
+
+        //convert values to plots
+        plot_current = CreatePlot(plots.current);
+        plot_rps = CreatePlot(plots.rpm);
+        plot_power = CreatePlot(plots.power);
+        plot_in_power = CreatePlot(plots.in_power);
+        plot_eff = CreatePlot(plots.eff);
+
+        SceneView.RepaintAll();
+    }
+
+    void OnDrawGizmos() {
+        Handles.matrix = Matrix4x4.identity;
+        Handles.color = Color.red;
+        if (plot_current != null) {
+            Handles.DrawPolyLine(plot_current);
+        }
+
+        Handles.color = Color.blue;
+        if (plot_rps != null) {
+            Handles.DrawPolyLine(plot_rps);
+        }
+
+        Handles.color = Color.yellow;
+        if (plot_power != null) {
+            Handles.DrawPolyLine(plot_power);
+        }
+
+        Handles.color = Color.green;
+        if (plot_eff != null) {
+            Handles.DrawPolyLine(plot_eff);
+        }
+
+        Handles.color = Color.gray;
+        if (plot_in_power != null) {
+            Handles.DrawPolyLine(plot_in_power);
+        }
+
+        Handles.color = Color.black;
+        //draw borders
+        Vector3 upOffset = Vector3.forward * plotHeight;
+        Vector3 rightOffset = Vector3.right * plotWidth;
+
+        Handles.DrawLine(Vector3.zero, upOffset);
+        Handles.DrawLine(Vector3.zero, rightOffset);
+        Handles.DrawLine(upOffset, upOffset + rightOffset);
+        Handles.DrawLine(rightOffset, upOffset + rightOffset);
+    }
+}

Unity/Assets/Scripts/MotorPlot.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 3ad79268f57596f4da62ff8db17ffc6c
+timeCreated: 1452189955
+licenseType: Free
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/NewtonIntegrator.cs

@@ -0,0 +1,9 @@
+using UnityEngine;
+using System.Collections;
+
+public class NewtonIntegrator : MonoBehaviour {
+
+   // public delegate void DerivativeFunc()
+
+	//public void 
+}

Unity/Assets/Scripts/NewtonIntegrator.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 27465a0c48a740742b56587f1299cc79
+timeCreated: 1452188047
+licenseType: Free
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/OrientationSensor.cs

@@ -0,0 +1,22 @@
+using UnityEngine;
+using System.Collections;
+
+public class OrientationSensor : MonoBehaviour {
+	public Quaternion orientation = Quaternion.identity;
+	public Vector3 right = Vector3.right;
+	public Vector3 up = Vector3.right;
+	public Vector3 forward = Vector3.right;
+
+	// Use this for initialization
+	void Start () {
+	
+	}
+	
+	// Update is called once per frame
+	public void FixedUpdate () {
+		orientation = transform.rotation;
+		right = orientation * Vector3.right;
+		up = orientation * Vector3.up;
+		forward = orientation * Vector3.forward;
+	}
+}

Unity/Assets/Scripts/OrientationSensor.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 8f9e446d96d8cb246904fcd464bce598
+timeCreated: 1423770347
+licenseType: Pro
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/Oscilloscope.cs

@@ -0,0 +1,219 @@
+using UnityEngine;
+using System.Collections;
+using System;
+
+[Serializable]
+public class OscilloscopeChannel{
+	public bool sample = true;
+	public bool draw = true;
+	public Color color = Color.green;
+	public float cellTime = 0.2f;
+	public float cellValue = 0.2f;
+	public OscilloscopeSampler sampler;
+
+
+	private float[] sampleValueBuffer;
+
+	public float[] SampleValueBuffer {
+		get {
+			return sampleValueBuffer;
+		}
+	}
+
+	private float[] sampleTimeBuffer;
+
+	public float[] SampleTimeBuffer {
+		get {
+			return sampleTimeBuffer;
+		}
+	}
+
+	private int sampleBufferPosition;
+
+	public int SampleBufferPosition {
+		get {
+			return sampleBufferPosition;
+		}
+	}
+
+	public void Reset(int cellCount, int sampleRate){
+		float totalTime = cellCount * cellTime;
+		int maxSampleCount = Mathf.CeilToInt (totalTime * sampleRate);
+		sampleTimeBuffer = new float[maxSampleCount];
+		sampleValueBuffer = new float[maxSampleCount];
+	}
+
+	public void Sample(){
+		sampleBufferPosition = (sampleBufferPosition + 1) % sampleValueBuffer.Length;
+		float val = sampler == null ? 0 : sampler();
+		sampleTimeBuffer[sampleBufferPosition] = Time.time;
+		sampleValueBuffer[sampleBufferPosition] = val;
+	}
+}
+
+public delegate float OscilloscopeSampler();
+
+[ExecuteInEditMode]
+public class Oscilloscope : MonoBehaviour {
+	public Vector2 pos = new Vector2(10,10);
+	public Vector2 size;
+
+	public OscilloscopeChannel[] channels;
+	
+
+	private Vector3[] screenCorners;
+	private const float cellSize = 40.0f;
+	private const int hCellCountHalf = 6;
+	private const int vCellCountHalf = 4;
+	public int sampleRate = 50;
+	private float lastSampleTime;
+	public bool showInEditor = true;
+	
+	public int xMatrixOffset = 0;
+	public int yMatrixOffset = 0;
+	private GUIStyle richLabelStyle;
+
+
+
+	// Use this for initialization
+	void Start () {
+		size = new Vector2 (hCellCountHalf * 2 * cellSize, vCellCountHalf * 2 * cellSize);
+		screenCorners = new []{
+			new Vector3(pos.x, pos.y, 0.5f),
+			new Vector3(pos.x + size.x, pos.y, 0.5f),
+			new Vector3(pos.x + size.x, pos.y + size.y, 0.5f),
+			new Vector3(pos.x, pos.y + size.y, 0.5f)
+		};
+
+		foreach (var c in channels) {
+			c.Reset(hCellCountHalf * 2, sampleRate);
+		}
+
+	}
+
+	void UpdateSamples(){
+		float currentTime = Time.time;
+		float timeElapsed = currentTime - lastSampleTime;
+
+		if(timeElapsed >= (1.0f / sampleRate)){
+			if(channels != null){
+				foreach( var c in channels){
+					if(c.sample)
+						c.Sample();
+				}
+			}
+			lastSampleTime = currentTime;
+		}
+	}
+
+	Vector2[] GenerateLineDividers(Vector2 from, Vector2 to, float step, int count, float leftSize, float rightSize){
+		Vector2[] result = new Vector2[count * 2];
+		Vector2 dir = (to - from).normalized;
+		Vector2 right = new Vector2 (dir.y, -dir.x);
+		Vector2 leftOffset = right * -leftSize;
+		Vector2 rightOffset = right * rightSize;
+
+		for (int i = 0; i < count; ++i) {
+			var p = from + dir * step * i;
+			result[i * 2 + 0] = p + leftOffset;
+			result[i * 2 + 1] = p + rightOffset;
+		}
+
+		return result;
+	}
+
+	void DrawLinesArray(Vector2[] lines){
+		for (int i = 0; i < lines.Length / 2; ++i) {
+			var p0 = lines[i * 2 + 0];
+			var p1 = lines[i * 2 + 1];
+			GL.Vertex(new Vector3(p0.x, p0.y, 0.4f));
+			GL.Vertex(new Vector3(p1.x, p1.y, 0.4f));
+		}
+	}
+
+	void OnValidate(){
+
+	}
+
+	void DrawDisplay(){
+		size = new Vector2 (hCellCountHalf * 2 * cellSize, vCellCountHalf * 2 * cellSize);
+		screenCorners = new []{
+			new Vector3(pos.x, pos.y, 0.5f),
+			new Vector3(pos.x + size.x, pos.y, 0.5f),
+			new Vector3(pos.x + size.x, pos.y + size.y, 0.5f),
+			new Vector3(pos.x, pos.y + size.y, 0.5f)
+		};
+		
+		UpdateSamples ();
+
+		return;
+	}
+
+
+
+	void DrawHandles(){
+		/*Rect bottomRect = new Rect (pos.x, pos.y + size.y, size.x, 20);
+
+		int digitWidth = 20;
+		int labelWidth = 60;
+		int channelInfoLength = digitWidth + labelWidth;
+
+		if (channels != null) {
+			int channelId = 0;
+		
+			foreach(var c in channels){
+				if(c != null){
+					float offsetX = channelInfoLength * channelId + bottomRect.x;
+					Rect digitRect = new Rect(offsetX, bottomRect.y, digitWidth, bottomRect.height);
+					Rect labelRect = new Rect(offsetX + digitWidth, bottomRect.y, labelWidth, bottomRect.height);
+
+					GUI.Label(digitRect, string.Format("<color=green>{0}</color>", channelId.ToString()),richLabelStyle);
+					GUI.Label(labelRect, string.Format("<color=white>{0}</color>", c.cellValue.ToString()),richLabelStyle);
+
+					int colorLineHeight = 3;
+					Rect colorLineRect = new Rect(offsetX, labelRect.yMax - colorLineHeight, channelInfoLength / 3, colorLineHeight);
+					DrawRect(colorLineRect, c.color, 0.3f);
+
+					channelId++;
+				}
+			}
+
+
+		}*/
+	}
+
+	void OnWillRenderObject() {
+		Debug.Log ("On will render");
+	}
+
+	void Draw(){
+		if (!enabled) {
+			return;
+		}
+		if (richLabelStyle == null) {
+			richLabelStyle = new GUIStyle ();
+			richLabelStyle.richText = true;
+		}
+		GL.PushMatrix();
+		GL.LoadPixelMatrix (0, Screen.width + xMatrixOffset, Screen.height + yMatrixOffset, 0);
+	
+		DrawDisplay ();
+		//DrawHandles ();
+		GL.PopMatrix();
+	}
+
+	void OnPostRender(){
+		Draw ();
+	}
+
+	void OnDrawGizmos(){
+		//if (showInEditor) {
+			Draw ();
+		//}
+	}
+
+	// Update is called once per frame
+	void Update () {
+
+	}
+}

Unity/Assets/Scripts/Oscilloscope.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: d44e2f9a2da50b242aab19dd970b8c3d
+timeCreated: 1423678116
+licenseType: Pro
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/OscilloscopeDisplay.cs

@@ -0,0 +1,244 @@
+using UnityEngine;
+using System.Collections;
+
+public class OscilloscopeDisplay : MonoBehaviour {
+	public Material displayMaterial;
+	private Texture2D diplayBackground;
+	public Oscilloscope oscilloscope;
+
+	public int halfXCellCount = 6;
+	public int halfYCellCount = 4;
+	public int cellSize = 50;
+	public int shortLineSize = 5;
+	public Vector2 position;
+
+	public Color backgroundColor = new Color(0,0,0,0.6f);
+	public Color linesColor = new Color(0.27f,0.57f,0.9f, 1.0f);
+
+	static Material lineMaterial;
+	static void CreateLineMaterial() {
+		if( !lineMaterial ) {
+			lineMaterial = new Material( "   Shader \"Lines/Colored Blended\" {" +
+			                            "    SubShader { Pass { " +
+			                            "    Blend SrcAlpha OneMinusSrcAlpha " +
+			                            "    ZWrite Off Cull Off ZTest Off Fog { Mode Off } " +
+			                            "    BindChannels {" +
+			                            "    Bind \"vertex\", vertex Bind \"color\", color }" +
+			                            "} } }" );
+			lineMaterial.hideFlags = HideFlags.HideAndDontSave;
+			lineMaterial.shader.hideFlags = HideFlags.HideAndDontSave;
+		}
+	}
+
+	// Use this for initialization
+	void Start () {
+	
+	}
+
+	void GenerateBackground(Texture2D tex){
+		if (tex == null) {
+			return;
+		}
+		Debug.Log ("GenerateBackground");
+
+		Color[] pixels = new Color[tex.width * tex.height];
+
+		//fill background
+		for (int i = 0; i < pixels.Length; ++i) {
+			pixels[i] = backgroundColor;
+		}
+
+		//draw grid
+		//-horizontal lines
+		for (int i = 0; i <= halfYCellCount * 2; ++i) {
+			int y = i * cellSize;
+			for(int x = 0; x < tex.width; ++x){
+				if(y != 0 && y != tex.height - 1){
+					if((x % 2) == 1){
+						continue;
+					}
+				}
+
+				int id = y * tex.width + x;
+				pixels[id] = linesColor;
+			}
+		}
+		//-vertical lines
+		for (int i = 0; i <= halfXCellCount * 2; ++i) {
+			int x = i * cellSize;
+			for(int y = 0; y < tex.height; ++y){
+				if(x != 0 && x != tex.width - 1){
+					if((y % 2) == 1){
+						continue;
+					}
+				}
+				int id = y * tex.width + x;
+				pixels[id] = linesColor;
+			}
+		}
+
+		//draw helper short lines
+		//-vertical
+		int[] cells = new int[]{0,halfXCellCount, halfXCellCount*2};
+		int[] offsets = new int[]{0, -shortLineSize / 2, -shortLineSize + 1 };
+		for (int i = 0; i < cells.Length; ++i) {
+			int x = cells[i] * cellSize + offsets[i];
+			for(int y = 0; y < tex.height; y += (cellSize / 5)){
+				for(int j = x; j < (x + shortLineSize); ++j){
+					pixels[ y * tex.width + j] = linesColor;
+				}
+			}
+		}
+
+		//horizontal
+		cells = new int[]{0, halfYCellCount, halfYCellCount*2};
+		for (int i = 0; i < cells.Length; ++i) {
+			int y = cells[i] * cellSize + offsets[i];
+			for(int x = 0; x < tex.width; x += (cellSize / 5)){
+				for(int j = y; j < (y + shortLineSize); ++j){
+					pixels[ j * tex.width + x] = linesColor;
+				}
+			}
+		}
+
+
+
+		tex.SetPixels (pixels);
+		tex.Apply ();
+
+	}
+
+	void OnValidate(){
+		int texWidth = cellSize * halfXCellCount * 2 + 1;
+		int texHeight = cellSize * halfYCellCount * 2 + 1;
+
+		if (diplayBackground == null || diplayBackground.width != texWidth || diplayBackground.height != texHeight) {
+			diplayBackground = new Texture2D(texWidth, texHeight, TextureFormat.ARGB32, false, true);
+		}
+
+		GenerateBackground (diplayBackground);
+	}
+
+
+	void OnDrawGizmos(){
+
+		GL.PushMatrix ();
+		//GL.LoadPixelMatrix ();
+
+		GL.LoadPixelMatrix (0, Camera.current.pixelWidth, Camera.current.pixelHeight, 0);
+
+
+		Graphics.DrawTexture (new Rect (position.x, position.y, diplayBackground.width, diplayBackground.height), diplayBackground);
+
+		if (oscilloscope != null) {
+			Vector2 size = new Vector2 (halfXCellCount * 2 * cellSize, halfYCellCount * 2 * cellSize);
+			CreateLineMaterial();
+			//func
+			lineMaterial.SetPass(0);
+			GL.Begin(GL.LINES);
+			lineMaterial.SetPass(0);
+			float verticalOffset = halfYCellCount * cellSize;
+			
+			foreach (var channel in oscilloscope.channels) {
+				if(channel == null || !channel.draw){
+					continue;
+				}
+				GL.Color(channel.color);
+				
+				var timeBuffer = channel.SampleTimeBuffer;
+				var valueBuffer = channel.SampleValueBuffer;
+				var bufferPos = channel.SampleBufferPosition;
+				
+				if(timeBuffer == null || timeBuffer.Length == 0 || valueBuffer == null || valueBuffer.Length == 0){
+					
+					continue;
+				}
+				
+				float lastTime = timeBuffer[bufferPos];
+				float lastX = size.x;
+				float lastY = size.y - ((valueBuffer[bufferPos] / channel.cellValue) * cellSize + verticalOffset);
+				
+				for(int i = 1; i < timeBuffer.Length; ++i){
+					if(bufferPos == 0){
+						bufferPos = valueBuffer.Length - 1;
+					}else{
+						bufferPos = bufferPos - 1;
+					}
+					
+					float timeOffset = timeBuffer[bufferPos] - lastTime;
+					float curX = size.x + (timeOffset / channel.cellTime) * cellSize;
+					float curY = size.y - ((valueBuffer[bufferPos] / channel.cellValue) * cellSize + verticalOffset);
+					
+					Vector2 curVector = Vector3.zero;
+					Vector2 lastVector = Vector3.zero;
+					
+					if(ClipLine(lastX, lastY, curX, curY, ref curVector, ref lastVector, size)){
+						GL.Vertex(new Vector3(curVector.x + position.x, curVector.y + position.y, 0));
+						GL.Vertex(new Vector3(lastVector.x + position.x, lastVector.y + position.y, 0));
+					}
+					
+					
+					lastY = curY;
+					lastX = curX;
+				}
+			}
+			
+			GL.End();
+		}
+		GL.PopMatrix ();
+
+	}
+
+	bool ClipLine(float lastX, float lastY, float curX, float curY, ref Vector2 L, ref Vector2 R, Vector2 size){
+		
+		if (lastX < 0) {
+			return false;
+		}
+		L.x = curX;
+		L.y = curY;
+		
+		R.x = lastX;
+		R.y = lastY;
+		
+		if(R.x < 0){
+			float kx = (L.x / (L.x - R.x));
+			L.y = L.y + (R.y - L.y) * kx;
+			L.x = 0;
+		}
+		
+		if (L.y > R.y) {
+			var tmp = R;
+			R = L;
+			L = tmp;
+		}
+		
+		//L.y always smaller
+		if (R.y <= 0) {
+			return false;
+		}
+		if (L.y >= size.y) {
+			return false;
+		}
+		
+		if (L.y < 0) { //top clamp
+			float kx = L.y / (L.y - R.y);
+			L.x = L.x + (R.x - L.x) * kx;
+			L.y = 0;
+		}
+		if (R.y > size.y){ //bottom clamp
+			float kx = (R.y - size.y) / (R.y - L.y);
+			
+			R.x = R.x + (L.x - R.x) * kx;
+			R.y = size.y;
+		}
+		
+		if (R.y > size.y || L.y > size.y) {
+			Debug.Log("Fail");
+		}
+		return true;
+	}
+	// Update is called once per frame
+	void Update () {
+	
+	}
+}

Unity/Assets/Scripts/OscilloscopeDisplay.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 63f15ae69061bb24f95668f58a44f9fa
+timeCreated: 1426016623
+licenseType: Free
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/PID.cs

@@ -0,0 +1,59 @@
+using UnityEngine;
+using System.Collections;
+using System;
+
+[Serializable]
+public class PID {
+	public bool enabled = true;
+	public float kp = 1.0f;
+	public float ki = 1.0f;
+	public float kd = 1.0f;
+
+	public float outMin = -0.1f;
+	public float outMax = 0.1f;
+
+	private float _errorIntegral;
+	private float _lastError;
+	private float _setpoint;
+
+	public float LastError {get {return _lastError;}}
+
+	private float _p;
+	public float P {get {return _p;}}
+	private float _i;
+	public float I {get {return _i;}}
+	private float _d;
+	public float D {get {return _d;}}
+
+	public float Setpoint{
+		get{ return _setpoint; }
+		set{ _setpoint = value; }
+	}
+
+	public float Process(float dt, float pv){
+		if (!enabled) {
+			return 0;
+		}
+		float error = Setpoint - pv;
+		float derivative = (error - _lastError) / dt;
+
+		//derivative = Mathf.Clamp(derivative, outMin, outMax);
+
+		/*if (Mathf.Abs(integralChange) > 0.001f) {
+			integralChange = 0.001f * Mathf.Sign(integralChange);
+		}*/
+		_errorIntegral += error * dt;
+
+
+		//_errorIntegral = Mathf.Clamp(_errorIntegral, outMin, outMax);
+
+		_lastError = error;
+
+		_p = kp * error;
+		_i = ki * _errorIntegral;
+		_d = kd * derivative;
+
+		var output = _p + _i + _d;
+		return Mathf.Clamp(output, outMin, outMax);
+	}
+}

Unity/Assets/Scripts/PID.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: e7c34d969bf797248b749b29807d1aaf
+timeCreated: 1423566544
+licenseType: Pro
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/PropellerForce.cs

@@ -0,0 +1,58 @@
+using UnityEngine;
+using System.Collections;
+
+public class PropellerForce : MonoBehaviour {
+	public Motor motor;
+	public float maxForce = 20;
+	public float maxTorque = 20;
+	private Vector3 forceVector;
+	private Rigidbody rb;
+	private Vector3 torque;
+
+	private Vector3 torqueForce;
+	private Vector3 torqueOrtho;
+
+	// Use this for initialization
+	void Start () {
+		rb = gameObject.GetComponentInParent<Rigidbody> ();
+	}
+
+	void FixedUpdate(){
+		if (rb == null) {
+			return;
+		}
+		float motorSpeedNormalized = (motor.speed / motor.maxRpm);
+
+		float force = Mathf.Abs(motorSpeedNormalized * maxForce);
+		forceVector = motor.transform.up * force;
+		rb.AddForceAtPosition(forceVector, motor.transform.position);
+
+		float torqueMagnitude = maxTorque * motorSpeedNormalized;
+		torque = -torqueMagnitude * motor.transform.up;
+
+		rb.AddTorqueAtPosition (torque, motor.transform.position, ref torqueForce, ref torqueOrtho);
+	}
+
+	void OnDrawGizmos(){
+		//return;
+		Gizmos.DrawLine (motor.transform.position, motor.transform.position + forceVector);
+		Gizmos.color = Color.green;
+
+		var pa = motor.transform.position;
+		var pb = motor.transform.position + torqueOrtho;
+		var pc = motor.transform.position + torqueOrtho + torqueForce;
+		var pd = motor.transform.position - torqueOrtho;
+		var pe = motor.transform.position - torqueOrtho - torqueForce;
+
+		Gizmos.DrawLine (pa, pb);
+		Gizmos.DrawLine (pb, pc);
+
+		Gizmos.DrawLine (pa, pd);
+		Gizmos.DrawLine (pd, pe);
+	}
+
+	// Update is called once per frame
+	void Update () {
+	
+	}
+}

Unity/Assets/Scripts/PropellerForce.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 2c51ebb8487fe6844bb469c4bc187c35
+timeCreated: 1423340351
+licenseType: Pro
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/QuadcopterEditorOritentationController.cs

@@ -0,0 +1,34 @@
+using UnityEngine;
+using System.Collections;
+
+[ExecuteInEditMode]
+public class QuadcopterEditorOritentationController : MonoBehaviour {
+
+	public FlightController target;
+
+	// Use this for initialization
+	void Start () {
+	
+	}
+	
+	// Update is called once per frame
+	void Update () {
+	
+	}
+
+	void OnDrawGizmos(){
+		Gizmos.color = Color.red;
+		Gizmos.DrawLine (transform.position, transform.position + transform.right);
+		Gizmos.color = Color.green;
+		Gizmos.DrawLine (transform.position, transform.position + transform.up);
+		Gizmos.color = Color.blue;
+		Gizmos.DrawLine (transform.position, transform.position + transform.forward);
+
+
+		if (enabled) {
+			if(target != null){
+				target.targetOrientation = transform.rotation;
+			}
+		}
+	}
+}

Unity/Assets/Scripts/QuadcopterEditorOritentationController.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 04a4bb44dcf9e8842903213ab500e4f5
+timeCreated: 1425907563
+licenseType: Free
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/QuadcopterFlightController.cs

@@ -0,0 +1,246 @@
+using UnityEngine;
+using System.Collections;
+
+
+
+public class QuadcopterFlightController : FlightController {
+	public ElectronicSpeedController[] esc;
+
+	private Vector2 tiltDirection;
+	private float tiltAmt;
+	private float yawError;
+
+	public PID yawPid;
+	public PID altitudePid;
+	public PID tiltPid;
+	private PID[] tiltPids;
+
+	public BiquadFilter[] motorTiltErrorFilters = new BiquadFilter[4];
+
+	public float targetAltitiude;
+
+	public Oscilloscope oscilloscope;
+
+	public class EngineStabilizationParameters{
+		public float tiltError;
+	}
+
+	private EngineStabilizationParameters[] stabilizationParameters = new EngineStabilizationParameters[4];
+
+	// Use this for initialization
+	void Start () {
+		stabilizationParameters = new EngineStabilizationParameters[esc.Length];
+		for (int i = 0; i < esc.Length; ++i) {
+			stabilizationParameters[i] = new EngineStabilizationParameters();
+		}
+
+		if (oscilloscope != null) {
+			oscilloscope.channels[0].sampler = SamplerP;
+			oscilloscope.channels[1].sampler = SamplerI;
+			oscilloscope.channels[2].sampler = SamplerD;
+			oscilloscope.channels[3].sampler = SamplerErr;
+		}
+
+		tiltPids = new PID[esc.Length];
+
+		for(int i = 0; i < tiltPids.Length; ++i){
+			tiltPids[i] = new PID();
+			tiltPids[i].Setpoint = 0;
+		}
+	}
+
+	float SamplerP(){
+		return tiltPids[0].P;
+	}
+
+	float SamplerI(){
+		return tiltPids[0].I;
+	}
+
+	float SamplerD(){
+		return tiltPids[0].D;
+	}
+
+	float SamplerErr(){
+		return tiltPids[0].LastError;
+	}
+
+	void UpdateTiltDirection(bool updateErrors){
+		if (stabilizationParameters == null) {
+			stabilizationParameters = new EngineStabilizationParameters[esc.Length];
+		}
+
+		for (int i = 0; i < esc.Length; ++i) {
+			if(stabilizationParameters[i] == null){
+				stabilizationParameters[i] = new EngineStabilizationParameters();
+			}
+		}
+
+		var qBody = orientationSensor.orientation;
+		var qDesired = targetOrientation;
+		
+		var qBodyInv = Quaternion.Inverse (qBody);
+		var qError = qBodyInv * qDesired;
+		
+		//decompose error
+		Vector3 qErrAxis;
+		float qErrAngle;
+		qError.ToAngleAxis (out qErrAngle, out qErrAxis);
+		var re = qErrAxis;
+		var rb = qBody * (qBodyInv * re);
+		var invQb = Quaternion.AngleAxis (qErrAngle, rb);
+
+		float q0 = invQb.w;
+		float q1 = invQb.x;
+		float q2 = invQb.z;
+		float q3 = invQb.y;
+		
+		float aH = Mathf.Acos (1 - 2.0f * (q1*q1 + q2*q2));
+		float phi = 2.0f * Mathf.Atan2 (q3, q0);
+
+		yawError = phi;
+		if (yawError > Mathf.PI) {
+			yawError = yawError - Mathf.PI * 2.0f;
+		}else if(yawError < -Mathf.PI) {
+			yawError = yawError + Mathf.PI * 2.0f;
+		}
+
+		tiltAmt = Mathf.Abs(aH);
+
+
+		//Debug.LogFormat ("X{0}Y{1}Z{2}W{3} {4}", qBody.x, qBody.y, qBody.z, qBody.w, tiltAmt);
+		Vector2 tiltLocalVector;
+		if (tiltAmt <= 0.00001f) {
+			tiltDirection = Vector3.up;
+			tiltLocalVector = Vector2.zero;
+		} else {
+
+			float rx = (Mathf.Cos (phi / 2) * q1 - Mathf.Sin (phi / 2) * q2) / Mathf.Sin (aH / 2);
+			float ry = (Mathf.Sin (phi / 2) * q1 + Mathf.Cos (phi / 2) * q2) / Mathf.Sin (aH / 2);
+			
+			float bH = Mathf.Atan2 (ry, rx);
+			float gH = Mathf.Atan2 (rx, -ry);
+			tiltDirection = new Vector2 (rx, ry);
+			tiltLocalVector = tiltDirection.RotateRadians(-phi);
+			tiltDirection = tiltLocalVector;
+		}
+
+		if (updateErrors) {
+			for(int i = 0; i < esc.Length; ++i) {
+				var c = esc[i];
+				var motorPos = c.motor.transform.position;
+				motorPos = transform.InverseTransformPoint(motorPos);
+				var motorDir = motorPos.normalized;
+				motorDir.y = 0;
+				var motorK = Vector3.Dot (motorDir, new Vector3(tiltLocalVector.y, 0, -tiltLocalVector.x));
+				
+				float motorError = motorK * (tiltAmt / (float)esc.Length);
+				motorError = (float)motorTiltErrorFilters[i].Process((double)motorError);
+				//tiltAmt = orientationLopassFilter.DoFilter (tiltAmt, Time.fixedDeltaTime);
+				
+				stabilizationParameters[i].tiltError = motorError;
+			}
+		}
+
+	}
+
+		
+
+	void Stabilize(){
+
+	}
+
+	void FixedUpdate(){
+		UpdateTiltDirection (true);
+
+		foreach (var p in tiltPids) {
+			p.kp = tiltPid.kp;
+			p.kd = tiltPid.kd;
+			p.ki = tiltPid.ki;
+
+			p.outMin = tiltPid.outMin;
+			p.outMax = tiltPid.outMax;
+		}
+
+		yawPid.Setpoint = 0;
+		var yawK = yawPid.Process (Time.fixedDeltaTime, yawError);
+		altitudePid.Setpoint = targetAltitiude;
+		var altK = altitudePid.Process (Time.fixedDeltaTime, transform.position.y);
+
+		for(int i = 0; i < esc.Length; ++i) {
+			float tiltK = tiltPids[i].Process (Time.fixedDeltaTime,  stabilizationParameters[i].tiltError);
+
+			var e = esc[i];
+			float sign = e.inverseDirection ? -1 : 1;
+
+			float yawPart =  (yawK / esc.Length) * sign;
+			//Debug.LogFormat ("yawPart: {0}", yawPart);
+			e.inputSpeed = Mathf.Clamp01(targetThrolle)  - tiltK +altK + yawPart;
+			/*if(i == 0){
+				Debug.Log ("Engine 0 speed: " + e.inputSpeed + "Err = " + stabilizationParameters[i].tiltError);
+			}else if(i == 3){
+				Debug.Log ("Engine 3 speed: " + e.inputSpeed + "Err = " + stabilizationParameters[i].tiltError);
+			}*/
+		}
+
+
+
+		//Debug.Log("D = " + tiltPids[0].D + "E=" + tiltPids[0].LastError);
+	}
+
+	// Update is called once per frame
+	void Update () {
+
+	}
+
+	void OnDrawGizmos(){
+		if (!Application.isPlaying) {
+
+			orientationSensor.FixedUpdate();
+			UpdateTiltDirection (true);
+		}
+
+
+		var up = targetOrientation * Vector3.up;
+		var right = targetOrientation * Vector3.right;
+		var forward = targetOrientation * Vector3.forward;
+
+		var p = transform.position;
+
+		//draw target plane
+		Gizmos.color = Color.red;
+		Gizmos.DrawLine (p, p + right);
+		Gizmos.color = Color.green;
+		Gizmos.DrawLine (p, p + up);
+		Gizmos.color = Color.blue;
+		Gizmos.DrawLine (p, p + forward);
+
+		//draw current frame
+
+		float cfOpacity = 0.2f;
+		Gizmos.color = new Color (1, 0, 0, cfOpacity);
+
+		Gizmos.DrawLine (p, p + orientationSensor.right);
+		Gizmos.color = new Color (0, 1, 0, cfOpacity);
+		Gizmos.DrawLine (p, p + orientationSensor.up);
+		Gizmos.color = new Color (0, 0, 1, cfOpacity);
+		Gizmos.DrawLine (p, p + orientationSensor.forward);
+
+		//draw tilt
+		Gizmos.color = Color.yellow;
+		Gizmos.DrawLine (p, p + orientationSensor.orientation * new Vector3(-tiltDirection.y, 0, tiltDirection.x));
+
+		if (stabilizationParameters != null) {
+			for(int i = 0; i < esc.Length; ++i) {
+				var c = esc[i];
+				var motorPos = c.motor.transform.position;
+				Gizmos.DrawLine (motorPos, motorPos + c.motor.transform.up * stabilizationParameters[i].tiltError * 50);
+			}
+		}
+
+	}
+
+	void OnGUI(){
+
+	}
+}

Unity/Assets/Scripts/QuadcopterFlightController.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 86c32bb98f442ac478f3981bcd7b5011
+timeCreated: 1423552181
+licenseType: Pro
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/RemoteController.cs

@@ -0,0 +1,48 @@
+using UnityEngine;
+using System.Collections;
+
+public class RemoteController : MonoBehaviour {
+	public Joystick leftStick;
+	public Joystick rightStick;
+
+	public Vector4 yprtRate = new Vector4(35,35,35, 1);
+	public float yaw;
+	public float throlle = 0;
+
+	public Transform testTransform;
+	public Quaternion targetOrientation;
+
+	public FlightController targetController;
+
+	// Use this for initialization
+	void Start () {
+	
+	}
+	
+	// Update is called once per frame
+	void Update () {
+		throlle = Mathf.Clamp01 (leftStick.State.y);
+		yaw += leftStick.State.x * yprtRate.x * Time.deltaTime;
+
+		Vector2 tiltAxis = rightStick.State;
+
+		Quaternion rotation = Quaternion.identity;
+		if (tiltAxis.magnitude > 0.01f) {
+			Vector3 rotationAxis = Vector3.Cross(Vector3.up, new Vector3(tiltAxis.x * yprtRate.y, 0, tiltAxis.y * yprtRate.z));
+			rotation = Quaternion.AngleAxis(rotationAxis.magnitude, rotationAxis.normalized);
+		}
+
+		//add yaw
+		targetOrientation = Quaternion.AngleAxis (yaw, Vector3.up) * rotation;
+
+		if (testTransform != null) {
+			testTransform.rotation = targetOrientation;
+		}
+
+		if (targetController != null) {
+			targetController.targetOrientation = targetOrientation;
+			targetController.targetThrolle = Mathf.Pow (throlle, 1.0f/4.0f) * 0.4f ;
+		}
+
+	}
+}

Unity/Assets/Scripts/RemoteController.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 248b197a229f6b3479293f47fa5c40da
+timeCreated: 1423935269
+licenseType: Pro
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/SegmentDisplay.cs

@@ -0,0 +1,90 @@
+using UnityEngine;
+using System.Collections;
+using System.Collections.Generic;
+
+public class SegmentDisplay : MonoBehaviour{
+    public byte segments;
+    private Material segmentMaterial;
+    private int _mask;
+
+    private Dictionary<char, byte> char_map = new Dictionary<char, byte>() {
+        //digits
+        {'0', 0x3f },
+        {'1', 0x06 },
+        {'2', 0x5b },
+        {'3', 0x4f },
+        {'4', 0x66 },
+        {'5', 0x6d },
+        {'6', 0x7d },
+        {'7', 0x07 },
+        {'8', 0x7f },
+        {'9', 0x6f },
+    };
+
+    public void SetChar(char c)
+    {
+        byte val = char_map.ContainsKey(c) ? char_map[c] : (byte)0;
+        _mask = (_mask & (1 << 7)) | val;
+        UpdateShader();
+    }
+
+    Material mat
+    {
+        get
+        {
+            if (segmentMaterial == null)
+            {
+                segmentMaterial = gameObject.GetComponent<Renderer>().materials[1]; ;
+            }
+            return segmentMaterial;
+        }
+    }
+
+    private void UpdateShader()
+    {
+        mat.SetInt("segments", _mask);
+    }
+
+    public bool ShowDot
+    {
+        get { return (_mask & (1 << 7)) != 0; }
+        set {
+            _mask = (_mask & ~(1 << 7));
+            if (value){
+                _mask |= 1 << 7;
+            }
+            UpdateShader();
+        }
+    }
+
+	void Start ()
+	{
+	    
+	   // StartCoroutine(UpdateDigit());
+	}
+	
+	void Update (){
+	 
+	}
+
+    private int d = 0;
+
+    /*IEnumerator UpdateDigit() {
+        while (true) {
+            yield return new WaitForSeconds(1);
+
+            if (d >= 9)
+            {
+                d = 0;
+            }
+            else
+            {
+                d++;
+            }
+
+
+
+            
+        }
+    }*/
+}

Unity/Assets/Scripts/SegmentDisplay.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 9937900fd7a6f6a49a5680226a2d99e6
+timeCreated: 1452349197
+licenseType: Free
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/SegmentDisplayString.cs

@@ -0,0 +1,35 @@
+using UnityEngine;
+using System.Collections;
+
+public class SegmentDisplayString : MonoBehaviour {
+
+    public SegmentDisplay[] segments;
+
+    public void SetString(string val) {
+        int segmentId = 0;
+        for (int i = 0; i < val.Length; ++i) {
+            char v = val[i];
+            if (v == '.') {
+                segments[segmentId == 0 ? 0 : segmentId - 1].ShowDot = true;
+            } else {
+                segments[segmentId].ShowDot = false;
+                segments[segmentId].SetChar(v);
+                segmentId++;
+                if (segmentId >= segments.Length) {
+                    break;
+                }
+            }
+        }
+
+    }
+
+    // Use this for initialization
+    void Start () {
+	
+	}
+	
+	// Update is called once per frame
+	void Update () {
+	
+	}
+}

Unity/Assets/Scripts/SegmentDisplayString.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: bee6484a74d2d5d49b80abdf2640d594
+timeCreated: 1452362664
+licenseType: Free
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/SimpleLowPassFilter.cs

@@ -0,0 +1,17 @@
+using UnityEngine;
+using System.Collections;
+using System;
+
+[Serializable]
+public class SimpleLowPassFilter {
+	public float old = 0.0f;
+	public float RC = 0.5f;
+	public float dtScale = 1.0f;
+
+	public float DoFilter(float val, float dt){
+		float a = dt / (RC + dt * dtScale);
+		float result = a * val + (1.0f - a) * old;
+		old = val;
+		return result;
+	}
+}

Unity/Assets/Scripts/SimpleLowPassFilter.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 3d4441c1960b611449ce5b9a95ea640e
+timeCreated: 1425815930
+licenseType: Free
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/TestQuadcopterController.cs

@@ -0,0 +1,15 @@
+using UnityEngine;
+using System.Collections;
+
+public class TestQuadcopterController : QuadcopterFlightController {
+
+	// Use this for initialization
+	void Start () {
+	
+	}
+	
+	// Update is called once per frame
+	void Update () {
+	
+	}
+}

Unity/Assets/Scripts/TestQuadcopterController.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 65d8c0a0067702f4f99aca30e9ebdea1
+timeCreated: 1423553200
+licenseType: Pro
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Unity/Assets/Scripts/VectorEx.cs

@@ -0,0 +1,14 @@
+using UnityEngine;
+using System.Collections;
+
+public static class VectorEx {
+	public static Vector2 Rotate(this Vector2 v, float degrees){
+		return v.RotateRadians(degrees * Mathf.Deg2Rad);
+	}
+	
+	public static Vector2 RotateRadians(this Vector2 v, float radians){
+		var ca = Mathf.Cos(radians);
+		var sa = Mathf.Sin(radians);
+		return new Vector2(ca*v.x - sa*v.y, sa*v.x + ca*v.y);
+	}
+}

Unity/Assets/Scripts/VectorEx.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: d5ed7d4c9ef09044ebce072f7fb8f05d
+timeCreated: 1425893756
+licenseType: Free
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: