public GyroUpdater(Bno055 sensor) {
//assign our sensor and create our data. This only happens once.
_sensor = sensor;
_gyroData = new GyroData();
//create our work item!
_workItem = new WorkItem(UpdateGyro, true, FlightEventType.Gyro, _gyroData);
}
public GyroUpdater(Bno055 sensor)
{
//assign our sensor and create our data. This only happens once.
_sensor = sensor;
_gyroData = new GyroData();
//create our work item!
_workItem = new WorkItem(UpdateGyro, true, FlightEventType.Gyro, _gyroData);
}
private void ReadSimulationSensorData(SensorData sensorDataObj, string [] row, int startIndex)
{
Vector3 accData = new Vector3();
accData.X = Convert.ToSingle(row[startIndex]);
accData.Y = Convert.ToSingle(row[startIndex + 1]);
accData.Z = Convert.ToSingle(row[startIndex + 2]);
sensorDataObj.AccDataObject.DataVector = accData;
GyroData gyroObj = sensorDataObj.GyroDataObject;
gyroObj.Pitch = Convert.ToSingle(row[startIndex + 3]);
gyroObj.Yaw = Convert.ToSingle(row[startIndex + 4]);
gyroObj.Roll = Convert.ToSingle(row[startIndex + 5]);
Quaternion quatObj = sensorDataObj.QuaternionObject;
quatObj.X = Convert.ToSingle(row[startIndex + 6]);
quatObj.Y = Convert.ToSingle(row[startIndex + 7]);
quatObj.Z = Convert.ToSingle(row[startIndex + 8]);
quatObj.W = Convert.ToSingle(row[startIndex + 9]);
}
public void Clear()
{
myOrigin = new clsPoint3d();
myEndXAxis = new clsPoint3d();
myEndYAxis = new clsPoint3d();
myPoint = new clsPoint3d();
VX = new clsPoint3d();
VY = new clsPoint3d();
VZ = new clsPoint3d();
mySeenFromCameraPoints.Clear();
myPts1.Clear();
myPts2.Clear();
myPts3.Clear();
myCameraPoints.Clear();
GyroData.Clear();
LastGyroData.Clear();
AccelData.Clear();
LastAccelData.Clear();
myHistory.Clear();
myLastTime = null;
myLastVector = null;
FirstPointRemoved = false;
mySeenFromMarkerIDs.Clear();
}
/// <summary>
/// Initializes a new instance of the <see cref="AngularVelocity"/> struct.
/// </summary>
/// <param name="gyroData">The gyro data.</param>
internal AngularVelocity(GyroData gyroData)
{
X = gyroData.X;
Y = gyroData.Y;
Z = gyroData.Z;
}
//must set syncdData in derivation so base methods have something to sync
void Start () {syncdData = new GyroData (); }
public void Load(System.IO.StreamReader sr)
{
clsPoint3d p1;
//var version = myPGLoadedVersion.Split('.');
//var nLoadedVersionMajor = version.Count() >= 1 ? Convert.ToInt32(version[0]) : 0;
//var nLoadedVersionMinor = version.Count() >= 2 ? Convert.ToInt32(version[1]) : 0;
while (sr.EndOfStream == false)
{
var myLine = sr.ReadLine();
if (myLine == "END_MARKER_POINT_SETTINGS")
{
break;
}
if (myLine.IndexOf(",") > -1)
{
var mySplit = myLine.Split(',');
if (mySplit.GetUpperBound(0) == 1)
{
if (mySplit[0] == "MarkerID")
{
myMarkerID = Convert.ToInt32(mySplit[1]);
}
if (mySplit[0] == "SeenFromMarkerID")
{
mySeenFromMarkerID = Convert.ToInt32(mySplit[1]);
}
if (mySplit[0] == "SeenFromMarkerIDs")
{
mySeenFromMarkerIDs.Add(Convert.ToInt32(mySplit[1]));
}
if (mySplit[0] == "ActualMarkerID")
{
myActualMarkerID = Convert.ToInt32(mySplit[1]);
}
if (mySplit[0] == "VerticalVectorX")
{
if (myVerticalVect == null)
{
myVerticalVect = new clsPoint3d();
}
myVerticalVect.X = Convert.ToDouble(mySplit[1]);
}
if (mySplit[0] == "VerticalVectorY")
{
if (myVerticalVect == null)
{
myVerticalVect = new clsPoint3d();
}
myVerticalVect.Y = Convert.ToDouble(mySplit[1]);
}
if (mySplit[0] == "VerticalVectorZ")
{
if (myVerticalVect == null)
{
myVerticalVect = new clsPoint3d();
}
myVerticalVect.Z = Convert.ToDouble(mySplit[1]);
}
if (mySplit[0] == "BulkheadHeight")
{
BulkheadHeight = Convert.ToDouble(mySplit[1]);
}
if (mySplit[0] == "Confirmed")
{
_confirmed = (mySplit[1] == "1");
}
}
}
}
if (mySeenFromMarkerIDs.Contains(mySeenFromMarkerID) == false)
{
mySeenFromMarkerIDs.Add(mySeenFromMarkerID);
}
myOrigin.Load(sr);
myEndXAxis.Load(sr);
myEndYAxis.Load(sr);
myPoint.Load(sr);
VX.Load(sr);
VY.Load(sr);
VZ.Load(sr);
var n = Convert.ToInt32(sr.ReadLine());
for (var i = 1; i <= n; i++)
{
p1 = new clsPoint3d();
p1.Load(sr);
myCameraPoints.Add(p1);
}
n = Convert.ToInt32(sr.ReadLine());
for (var i = 1; i <= n; i++)
{
p1 = new clsPoint3d();
p1.Load(sr);
mySeenFromCameraPoints.Add(p1);
}
n = Convert.ToInt32(sr.ReadLine());
for (var i = 1; i <= n; i++)
{
p1 = new clsPoint3d();
p1.Load(sr);
myPts1.Add(p1);
}
n = Convert.ToInt32(sr.ReadLine());
for (var i = 1; i <= n; i++)
{
p1 = new clsPoint3d();
p1.Load(sr);
myPts2.Add(p1);
}
n = Convert.ToInt32(sr.ReadLine());
for (var i = 1; i <= n; i++)
{
p1 = new clsPoint3d();
p1.Load(sr);
myPts3.Add(p1);
}
n = Convert.ToInt32(sr.ReadLine());
for (var i = 1; i <= n; i++)
{
p1 = new clsPoint3d();
p1.Load(sr);
GyroData.Add(p1);
}
n = Convert.ToInt32(sr.ReadLine());
for (var i = 1; i <= n; i++)
{
p1 = new clsPoint3d();
p1.Load(sr);
LastGyroData.Add(p1);
}
n = Convert.ToInt32(sr.ReadLine());
for (var i = 1; i <= n; i++)
{
p1 = new clsPoint3d();
p1.Load(sr);
AccelData.Add(p1);
}
n = Convert.ToInt32(sr.ReadLine());
for (var i = 1; i <= n; i++)
{
p1 = new clsPoint3d();
p1.Load(sr);
LastAccelData.Add(p1);
}
n = Convert.ToInt32(sr.ReadLine());
for (var i = 1; i <= n; i++)
{
var myHistoricPoint = new clsMarkerPoint();
myHistoricPoint.Load(sr);
myHistory.Add(myHistoricPoint);
}
}
