void Start()
{
/* Set sample time (T) */
Time.fixedDeltaTime = sampleTimeSystem;
/* Initialise system */
system = new SemiQuadDynamics(massMotor, massRod, lengthRod, frictionCoefficient, sampleTimeSystem);
system.Initialise(0.0f, 0.0f); /* Set initial state */
/* Initialise motor(s) */
motorLeft = new MotorDynamics(motorConversionCoefficient, 0.1f, sampleTimeSystem);
motorRight = new MotorDynamics(motorConversionCoefficient, 0.1f, sampleTimeSystem);
/* Initialise controller */
controller = new PID(1.0f, 0.0f, 0.0f, 0.01f, -50.0f, 50.0f, 0.01f);
/* Initialise sensor filter */
sensorFilter = new SensorFilter(0.01f, 0.01f);
/* Clear timer */
timerController = 0.0f;
/* Initial setpoint */
setpoint = 0.0f;
/* Initialise GUI */
InitGUI();
}
public SensorFusion(RiftHeadsetDevice sensor = null)
{
Stage = 0;
RunningTime = 0;
DeltaT = 0.001f;
Gain = 0.05f;
EnableGravity = true;
EnablePrediction = true;
PredictionDT = 0.03f;
PredictionTimeIncrement = 0.001f;
FRawMag = new SensorFilter(10);
FAngV = new SensorFilter(20);
GyroOffset = new Vector3f();
TiltAngleFilter = new SensorFilterBase_float(1000);
EnableYawCorrection = false;
MagCalibrated = false;
MagNumReferences = 0;
MagRefIdx = -1;
MagRefScore = 0;
MotionTrackingEnabled = true;
if (sensor != null)
AttachToSensor(sensor);
MagCalibrationMatrix = new Matrix4f();
MagCalibrationMatrix.SetIdentity();
}
//BME280 allows various modes. See chapter 3.5 "Recommended modes of operation" in the datasheet
public async Task SetSampling(SensorMode mode, SensorSampling tempSampling, SensorSampling pressSampling,
SensorSampling humSampling, SensorFilter filter, StandbyDuration duration)
{
Debug.WriteLine("BME280: Set sampling");
_measReg.mode = (uint)mode;
_measReg.osrs_t = (uint)tempSampling;
_measReg.osrs_p = (uint)pressSampling;
_humReg.osrs_h = (uint)humSampling;
_configReg.filter = (uint)filter;
_configReg.t_sb = (uint)duration;
// you must make sure to also set REGISTER_CONTROL after setting the
// CONTROLHUMID register, otherwise the values won't be applied (see DS 5.4.3)
byte[] WriteBuffer1 = new byte[] {
(byte)eRegisters.BME280_REGISTER_CONTROLHUMID, (byte)_humReg.Get()
};
byte[] WriteBuffer2 = new byte[] {
(byte)eRegisters.BME280_REGISTER_CONFIG, (byte)_configReg.Get()
};
byte[] WriteBuffer3 = new byte[] {
(byte)eRegisters.BME280_REGISTER_CONTROL, (byte)_measReg.Get()
};
bme280.Write(WriteBuffer1);
bme280.Write(WriteBuffer2);
bme280.Write(WriteBuffer3);
await Task.Delay(1);
}
public Settings()
{
if (!String.IsNullOrEmpty(SensorFilter))
{
var names = SensorFilter.Split(',');
foreach (var n in names)
{
if (!SensorFilterShortNames.Contains(n))
{
SensorFilterShortNames.Add(n);
}
}
}
}
public override void OnInspectorGUI()
{
serializedObject.Update();
// Draw inspector for each filter
for (int i = 0; i < Filters.arraySize; i++)
{
SerializedProperty currentf = Filters.GetArrayElementAtIndex(i);
GUILayout.BeginHorizontal();
currentf.isExpanded = EditorGUILayout.Foldout(currentf.isExpanded, new GUIContent(ObjectNames.NicifyVariableName(currentf.objectReferenceValue.GetType().Name)));
GUILayout.FlexibleSpace();
if (GUILayout.Button("Remove", EditorStyles.miniButton))
{
DestroyImmediate(currentf.objectReferenceValue, true);
AssetDatabase.SaveAssets();
Filters.DeleteArrayElementAtIndex(i);
Filters.DeleteArrayElementAtIndex(i);
serializedObject.ApplyModifiedProperties();
continue;
}
GUILayout.EndHorizontal();
if (currentf.isExpanded)
{
Editor m_editor = CreateEditor(currentf.objectReferenceValue);
GUILayout.Space(5);
m_editor.OnInspectorGUI();
}
MoonGUILayout.Separator();
}
GUILayout.Space(5);
// add buttun filter
if (GUILayout.Button("Add Filter", GUILayout.Height(20)))
{
GenericMenu Menu = new GenericMenu();
// Gets all aviable filters via reflection
List <System.Type> m_types = MoonReflection.GetAllDerivedTypes(typeof(SensorFilter)).ToList();
for (int i = 0; i < m_types.Count; i++)
{
System.Type currt = m_types[i];
if (currt != null)
{
Menu.AddItem(new GUIContent(currt.Name), false, new GenericMenu.MenuFunction(() =>
{
int index = Filters.arraySize;
SensorFilter filter = CreateInstance(currt) as SensorFilter;
filter.hideFlags = HideFlags.HideInHierarchy | HideFlags.HideInInspector;
AssetDatabase.AddObjectToAsset(filter, target);
AssetDatabase.SaveAssets();
Filters.InsertArrayElementAtIndex(index);
SerializedProperty curr = Filters.GetArrayElementAtIndex(index);
curr.objectReferenceValue = filter;
serializedObject.ApplyModifiedProperties();
}));
}
}
Menu.ShowAsContext();
}
serializedObject.ApplyModifiedProperties();
}
