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