public override string ToString()
{
//mid:64;x:0;y:0;z:0;mpry:0,0,0;pitch:0;roll:0;yaw:0;vgx:0;vgy:0;vgz:-7;templ:60;temph:63;tof:20;h:10;bat:89;baro:-67.44;time:0;agx:14.00;agy:-12.00;agz:-1094.00;
var builder = new StringBuilder();
builder.Append($"mid:{MissionPadId};");
builder.Append($"x:{MissionPadX};");
builder.Append($"y:{MissionPadY};");
builder.Append($"z:{MissionPadZ};");
builder.Append($"mpry:{MissionPadPitch},{MissionPadRoll},{MissionPadYaw};");
builder.Append($"pitch:{Pitch};");
builder.Append($"roll:{Roll};");
builder.Append($"yaw:{Yaw};");
builder.Append($"vgx:{SpeedX};");
builder.Append($"vgy:{SpeedY};");
builder.Append($"vgz:{SpeedZ};");
builder.Append($"templ:{TemperatureLowC};");
builder.Append($"temph:{TemperatureHighC};");
builder.Append($"tof:{DistanceTraversedInCm};");
builder.Append($"h:{HeightInCm};");
builder.Append($"bat:{BatteryPercent};");
builder.Append($"baro:{BarometerInCm.ToString("F2")};");
builder.Append($"time:{MotorTimeInSeconds};");
builder.Append($"agx:{AccelerationX.ToString("F2")};");
builder.Append($"agy:{AccelerationY.ToString("F2")};");
builder.Append($"agz:{AccelerationZ.ToString("F2")};");
return(builder.ToString());
}
/* ---------- Public methods ----------------------------------------------------------/**/
/// <summary>
/// Returns a hash code for this instance.
/// </summary>
/// <returns>
/// A hash code for this instance, suitable for use in hashing algorithms and data
/// structures like a hash table.
/// </returns>
/// <remarks>
/// The hash code generation process ignores properties <see cref="Checksum1" />,
/// <see cref="Checksum2" /> and <see cref="NcomPacket.Checksum3" />, since they are not
/// used when testing for equality.
/// </remarks>
public override int GetHashCode()
{
int hash = 13;
int mul = 7;
hash = (hash * mul) + base.GetHashCode();
hash = (hash * mul) + Time.GetHashCode();
hash = (hash * mul) + AccelerationX.GetHashCode();
hash = (hash * mul) + AccelerationY.GetHashCode();
hash = (hash * mul) + AccelerationZ.GetHashCode();
hash = (hash * mul) + AngularRateX.GetHashCode();
hash = (hash * mul) + AngularRateY.GetHashCode();
hash = (hash * mul) + AngularRateZ.GetHashCode();
hash = (hash * mul) + Latitude.GetHashCode();
hash = (hash * mul) + Longitude.GetHashCode();
hash = (hash * mul) + Altitude.GetHashCode();
hash = (hash * mul) + NorthVelocity.GetHashCode();
hash = (hash * mul) + EastVelocity.GetHashCode();
hash = (hash * mul) + DownVelocity.GetHashCode();
hash = (hash * mul) + Heading.GetHashCode();
hash = (hash * mul) + Pitch.GetHashCode();
hash = (hash * mul) + Roll.GetHashCode();
hash = (hash * mul) + (StatusChannel != null ? StatusChannel.GetHashCode() : 0);
return(hash);
}
private void ReadI2CAccel()
{
const string FLOAT_FORMAT = "F3"; /* Specify that we want 3 decimal points for floating point numbers */
const int ACCEL_RES = 1024; /* The ADXL345 has 10 bit resolution giving 1024 unique values */
const int ACCEL_DYN_RANGE_G = 8; /* The ADXL345 had a total dynamic range of 8G, since we're configuring it to +-4G */
const int UNITS_PER_G = ACCEL_RES / ACCEL_DYN_RANGE_G; /* Ratio of raw int values to G units */
Int16 AccelerationRawX, AccelerationRawY, AccelerationRawZ; /* Raw readings from the accelerometer */
double AccelerationX, AccelerationY, AccelerationZ; /* Readings converted to G units */
byte[] RegAddrBuf = new byte[] { ACCEL_REG_X }; /* Register address we want to read from */
byte[] ReadBuf = new byte[6]; /* We read 6 bytes sequentially to get all 3 two-byte axes registers in one read */
try
{
/*
* Read from the accelerometer
* We call WriteRead() so we first write the address of the X-Axis I2C register, then read all 3 axes
*/
I2CAccel.WriteRead(RegAddrBuf, ReadBuf);
}
catch (Exception e)
{
/* If WriteRead() fails, display error messages */
Text_X_Axis.Text = "X Axis: Error";
Text_Y_Axis.Text = "Y Axis: Error";
Text_Z_Axis.Text = "Z Axis: Error";
Text_Status.Text = "Exception: " + e.Message;
return;
}
/*
* In order to get the raw 16-bit data values, we need to concatenate two 8-bit bytes from the I2C read for each axis.
* We accomplish this by using bit shift and logical OR operations
*/
AccelerationRawX = (Int16)(ReadBuf[0] | ReadBuf[1] << 8);
AccelerationRawY = (Int16)(ReadBuf[2] | ReadBuf[3] << 8);
AccelerationRawZ = (Int16)(ReadBuf[4] | ReadBuf[5] << 8);
/* Convert raw values to G's */
AccelerationX = (double)AccelerationRawX / UNITS_PER_G;
AccelerationY = (double)AccelerationRawY / UNITS_PER_G;
AccelerationZ = (double)AccelerationRawZ / UNITS_PER_G;
/* Display the values */
Text_X_Axis.Text = "X Axis: " + AccelerationX.ToString(FLOAT_FORMAT) + "G";
Text_Y_Axis.Text = "Y Axis: " + AccelerationY.ToString(FLOAT_FORMAT) + "G";
Text_Z_Axis.Text = "Z Axis: " + AccelerationZ.ToString(FLOAT_FORMAT) + "G";
Text_Status.Text = "Status: Running";
}
public void OnSensorChanged(SensorEvent e)
{
lock (_syncLock)
{
double alpha = 0.8;
double newAccelerationX, newAccelerationY, newAccelerationZ;
AccelerationX.Add(e.Values[0]);
AccelerationX.RemoveAt(0);
AccelerationY.Add(e.Values[1]);
AccelerationY.RemoveAt(0);
AccelerationZ.Add(e.Values[2]);
AccelerationZ.RemoveAt(0);
newAccelerationX = 0.0;
newAccelerationY = 0.0;
newAccelerationZ = 0.0;
for (int i = 0; i < numberOfSamplesTaken; i++)
{
newAccelerationX += weights[i] * AccelerationX[i];
newAccelerationY += weights[i] * AccelerationY[i];
newAccelerationZ += weights[i] * AccelerationZ[i];
}
GravityX = alpha * GravityX + (1 - alpha) * newAccelerationX;
GravityY = alpha * GravityY + (1 - alpha) * newAccelerationY;
GravityZ = alpha * GravityZ + (1 - alpha) * newAccelerationZ;
filteredAccelerationX = newAccelerationX - GravityX;
filteredAccelerationY = newAccelerationY - GravityY;
filteredAccelerationZ = newAccelerationZ - GravityZ;
if (lastTimeUpdated == 0)
{
lastTimeUpdated = e.Timestamp;
}
sensorChanged(new OnSensorChangedArgs(filteredAccelerationX, filteredAccelerationY, filteredAccelerationZ,
GravityX, GravityY, GravityZ, e.Timestamp - lastTimeUpdated));
lastTimeUpdated = e.Timestamp;
}
}
public static AccelerationY LerpUnclamped(AccelerationY a, AccelerationY b, float t)
{
return(new AccelerationY(Mathf.LerpUnclamped(a.Float, b.Float, t)));
}
public static AccelerationY Clamp(AccelerationY value, AccelerationY min, AccelerationY max)
{
return(new AccelerationY(Mathf.Clamp(value.Float, min.Float, max.Float)));
}
public static AccelerationY Max(AccelerationY a, AccelerationY b)
{
return(new AccelerationY(Mathf.Max(a.Float, b.Float)));
}
public static AccelerationY Clamp01(AccelerationY value)
{
return(new AccelerationY(Mathf.Clamp01(value.Float)));
}
public override string ToString()
{
return($"pitch:{Pitch};roll:{Roll};yaw:{Yaw};vgx:{XSpeed};vgy:{YSpeed};vgz:{ZSpeed};templ:{TemperatureLow};temph:{TemperatureHigh};tof:{TimeOfFlight};h:{Height};bat:{BatteryPercentage};baro:{BarometricPressure.ToString("F2")};time:{MotorTime};agx:{AccelerationX.ToString("F2")};agy:{AccelerationY.ToString("F2")};agz:{AccelerationZ.ToString("F2")};\r\n");
}
