public void OnSensorChanged(SensorEvent e) { LoadNewSensorData(e); xView.Text = (accelData[0]).ToString(); yView.Text = (accelData[1]).ToString(); zView.Text = (accelData[2]).ToString(); vx.Text = (v[0]).ToString(); vy.Text = (v[1]).ToString(); vz.Text = (v[2]).ToString(); drx.Text = (dr[0]).ToString(); dry.Text = (dr[1]).ToString(); drz.Text = (dr[2]).ToString(); if (giroscopeData != null) { girox.Text = (giroscopeData[0]).ToString(); giroy.Text = (giroscopeData[1]).ToString(); giroz.Text = (giroscopeData[2]).ToString(); } if (giroscopeData != null && accelData != null) { // MadgwickAHRS madgwick = new MadgwickAHRS(1f / 256f,1); AHRS.Update(deg2rad(giroscopeData[0]), deg2rad(giroscopeData[1]), deg2rad(giroscopeData[2]), accelData[0], accelData[1], accelData[2]); //выводим в текстовое поле значение кватерниона из свойства класса MadgwickAHRS {get;set} QuaterionField.Text = (AHRS.Quaternion).ToString();
// static void Main (string[] args) { // // var lsm9DS1 = new LSM9DS1 (false); // lsm9DS1.GyroChanged += (sender, vector3) => // { // Console.WriteLine($"Gyro X: {vector3.X} Y: {vector3.Y} Z: {vector3.Z}"); // }; // lsm9DS1.AccelerometerChanged += (sender, vector3) => { // Console.WriteLine ($"Accelerometer X: {vector3.X} Y: {vector3.Y} Z: {vector3.Z}"); // }; // // lsm9DS1.StartInternalPolling (); // // Thread.Sleep (Timeout.Infinite); // } // static void Main(string[] args) // { // var lsm9DS1 = new LSM9DS1(); // lsm9DS1.Begin(); // while (true) // { // var acc = lsm9DS1.ReadAccelerometer(); // var gyro = lsm9DS1.ReadGyroscope(); // var mag = lsm9DS1.ReadMagnetometer(); // Console.WriteLine($"Accelerometer x {acc.X:N3} y {acc.X:N3} z {acc.Z:N3}"); // Console.WriteLine($"Gyroscope x {gyro.X:N3} y {gyro.X:N3} z {gyro.Z:N3}"); // Console.WriteLine($"Magnetometer x {mag.X:N3} y {mag.X:N3} z {mag.Z:N3}"); // Console.WriteLine(); // Thread.Sleep(50); // } // Thread.Sleep(Timeout.Infinite); // } static async Task Main(string[] args) { // This switch must be set before creating the GrpcChannel/HttpClient. AppContext.SetSwitch( "System.Net.Http.SocketsHttpHandler.Http2UnencryptedSupport", true); var channel = GrpcChannel.ForAddress("http://192.168.0.40:5000"); var client = new Location.LocationClient(channel); var degreesToRadiansFactor = Math.PI / 180; var radiansToDegreesFactor = 180.0f / Math.PI; var samplePeriod = TimeSpan.FromMilliseconds(50); // var ahrs = new MahonyAHRS(samplePeriod, 5f); var ahrs = new MadgwickAHRS((float)samplePeriod.TotalSeconds, 0.1f); var lsm9DS1 = new LSM9DS1(); lsm9DS1.Begin(); while (true) { // read fresh sensor data var acc = lsm9DS1.ReadAccelerometer(); var gyro = lsm9DS1.ReadGyroscope(); var mag = lsm9DS1.ReadMagnetometer(); // convert gyro readings from dps to rad/s var gx = (float)(gyro.X * degreesToRadiansFactor); var gy = (float)(gyro.Y * degreesToRadiansFactor); var gz = (float)(gyro.Z * degreesToRadiansFactor); // update ahrs ahrs.Update(gx, gy, gz, acc.X, acc.Y, acc.Z, mag.X, mag.Y, mag.Z ); ahrs.ComputeAngles(); // output rol, pitch and yaw var rollDegrees = ahrs.Roll * radiansToDegreesFactor; var pitchDegrees = ahrs.Pitch * radiansToDegreesFactor; var yawDegrees = ahrs.Yaw * radiansToDegreesFactor; client.UpdateLocationAsync( new NewLocation { Roll = rollDegrees, Pitch = pitchDegrees, Yaw = yawDegrees } ); Console.WriteLine($"Roll {rollDegrees:N3} Pitch {pitchDegrees:N3} Yaw {yawDegrees:N3}"); Console.WriteLine(); Thread.Sleep(samplePeriod); } Thread.Sleep(Timeout.Infinite); }
public void AHRS_Test() { MadgwickAHRS AHRS = new MadgwickAHRS(0.00000001f); AHRS.Update(0f, 0f, 0f, 0f, 1f, 0f); List <float> q = AHRS.Quaternion.ToList(); Assert.AreEqual(1, Math.Sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3])); }
/// <summary> /// Update sensor readings using Madgwick AHRS algorithm. /// </summary> public void Update() { if (MagXAxis == 0 || MagYAxis == 0 || MagZAxis == 0) { _ahrs.Update( Deg2rad((float)GyroXAxis), Deg2rad((float)GyroYAxis), Deg2rad((float)GyroZAxis), (float)AccelXAxis, (float)AccelYAxis, (float)AccelZAxis ); } else { _ahrs.Update( Deg2rad((float)GyroXAxis), Deg2rad((float)GyroYAxis), Deg2rad((float)GyroZAxis), (float)AccelXAxis, (float)AccelYAxis, (float)AccelZAxis, (float)MagXAxis, (float)MagYAxis, (float)MagZAxis ); } QuaternionWAxis = _ahrs.Quaternion[0]; QuaternionXAxis = _ahrs.Quaternion[1]; QuaternionYAxis = _ahrs.Quaternion[2]; QuaternionZAxis = _ahrs.Quaternion[3]; EulerRoll = _ahrs.Euler[0]; EulerPitch = _ahrs.Euler[1]; EulerYaw = _ahrs.Euler[2]; }
private void UpdateAHRS() { _ahrs.Update( //GYROSCOPE Input.gyro.rotationRate.x, Input.gyro.rotationRate.y, Input.gyro.rotationRate.z, //ACCELEROMETER Input.acceleration.x, Input.acceleration.y, Input.acceleration.z, //MAGNETOMETER Input.compass.rawVector.x, Input.compass.rawVector.y, Input.compass.rawVector.z ); }
public float[] getEuler(float[][] matGyr, float[][] matAcc) { //float[] matOut={0,0,0}; float[] euler = new float[] { 0, 0, 0 }; //float seamplePeriod = 1 / 256f; float[, ][] R = new float[3, 3][]; for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { R [i, j] = new float[matGyr.Length]; for (int k = 0; k < matGyr.Length; k++) { R [i, j] [k] = 0f; } } } MadgwickAHRS ahrs = new MadgwickAHRS(1 / 256f); for (int i = 0; i < matGyr.Length; i++) { float imp = (float)3.14 / 180; //gyro in grade float gx = (matGyr [i] [0] * imp); float gy = matGyr [i] [1] * imp; float gz = matGyr [i] [2] * imp; float ax = matAcc [i] [0]; float ay = matAcc [i] [1]; float az = matAcc [i] [2]; ahrs.Update(gx, gy, gz, ax, ay, az); euler = ahrs.QuaternToEuler(); } return(euler); }
private void Form1_Paint(object sender, PaintEventArgs e) { textBox2.Clear(); textBox3.Clear(); textBox4.Clear(); textBox6.Clear(); textBox7.Clear(); textBox8.Clear(); textBox8.AppendText(updatevalues.Beta.ToString("0.00")); textBox2.AppendText("x del acelerometro: " + afx.ToString("0.0000") + "\r\n"); textBox2.AppendText("y del acelerometro: " + afy.ToString("0.0000") + "\r\n"); textBox2.AppendText("z del acelerometro: " + afz.ToString("0.0000") + "\r\n"); textBox3.AppendText("x del giroscopo" + gfx.ToString("0.0000") + "\r\n"); textBox3.AppendText("y del giroscopo" + gfy.ToString("0.0000") + "\r\n"); textBox3.AppendText("z del giroscopo" + gfz.ToString("0.0000") + "\r\n"); textBox4.AppendText("x del magenetometro" + mfx.ToString("0.0000") + "\r\n"); textBox4.AppendText("y del magenetometro" + mfy.ToString("0.0000") + "\r\n"); textBox4.AppendText("z del magenetometro" + mfz.ToString("0.0000") + "\r\n"); smooth[0] = (gfx * alpha + (1 - alpha) * smooth[0]); smooth[1] = (gfy * alpha + (1 - alpha) * smooth[1]); smooth[2] = (gfz * alpha + (1 - alpha) * smooth[2]); smooth[3] = (afx * alpha + (1 - alpha) * smooth[3]); smooth[4] = (afy * alpha + (1 - alpha) * smooth[4]); smooth[5] = (afz * alpha + (1 - alpha) * smooth[5]); smooth[6] = (mfx * alpha + (1 - alpha) * smooth[6]); smooth[7] = (mfy * alpha + (1 - alpha) * smooth[7]); smooth[8] = (mfz * alpha + (1 - alpha) * smooth[8]); gdfx = gfx * ((float)Math.PI / 180.0f); gdfy = gfx * ((float)Math.PI / 180.0f); gdfz = gfx * ((float)Math.PI / 180.0f); // updatevalues.Update(gdfx, gdfy, gdfz, smooth[3], smooth[4], smooth[5], smooth[6], smooth[7], smooth[8]);//con filtro updatevalues.Update(gdfx, gdfy, gdfz, afx, afy, afz, mfx, mfy, mfz);//Sin filtro de medias Quaterniones(q, updatevalues); textBox7.AppendText("q0:" + updatevalues.Quaternion[0] + "\r\n"); textBox7.AppendText("q1:" + updatevalues.Quaternion[1] + "\r\n"); textBox7.AppendText("q2:" + updatevalues.Quaternion[2] + "\r\n"); textBox7.AppendText("q3:" + updatevalues.Quaternion[3] + "\r\n"); /*textBox5.AppendText("timer: " + timervalue.ToString()+"us" + "\r\n");*/ textBox5.AppendText("timer: " + timervalue.ToString() + "us" + "\r\n"); getYawPitchRollRad(ypr, q); pitchb = -(float)Math.Atan(afx / ((afx * afx) + (afz * afz))); // ypr[1]; rollb = -(float)Math.Atan(afy / ((afz * afz) + (afy * afy))); //ypr[2]; yawb = ypr[0]; differencepitch = pitch - pitchb; differenceroll = roll - rollb; differenceyaw = yaw - yawb; textBox6.AppendText("pitch:" + pitch + "\r\n"); textBox6.AppendText("roll:" + roll + "\r\n"); textBox6.AppendText("yaw:" + yaw + "\r\n"); textBox6.AppendText("pitchb:" + pitchb + "\r\n"); textBox6.AppendText("rollb:" + rollb + "\r\n"); textBox6.AppendText("yawb:" + yawb + "\r\n"); textBox6.AppendText("Longitud" + longitud + "\r\n"); textBox6.AppendText("Latitud" + latitud + "\r\n"); //Datos aplicacion c# chart1.ChartAreas[0].AxisX.Minimum = nmin; chart1.ChartAreas[0].AxisX.Maximum = nmax; chart1.ChartAreas[0].AxisY.Maximum = 2; chart1.ChartAreas[0].AxisY.Minimum = -2; chart2.ChartAreas[0].AxisX.Minimum = nmin; chart2.ChartAreas[0].AxisX.Maximum = nmax; chart2.ChartAreas[0].AxisY.Maximum = 4; chart3.ChartAreas[0].AxisX.Minimum = nmin; chart3.ChartAreas[0].AxisX.Maximum = nmax; chart3.ChartAreas[0].AxisY.Minimum = -2; chart3.ChartAreas[0].AxisY.Maximum = 2; chart1.Series["Pitchb"].ChartType = SeriesChartType.FastLine; chart1.Series["Pitchb"].Color = Color.Black; chart1.Series["Pitchb"].Points.AddXY(i, pitchb); /* chart2.Series["Yawb"].ChartType = SeriesChartType.FastLine; * chart2.Series["Yawb"].Color = Color.Black; * chart2.Series["Yawb"].Points.AddXY(i, yawb);*/ chart3.Series["Rollb"].ChartType = SeriesChartType.FastLine; chart3.Series["Rollb"].Color = Color.Black; chart3.Series["Rollb"].Points.AddXY(i, rollb); //Datos Microcontrolador chart1.Series["Pitch"].ChartType = SeriesChartType.FastLine; chart1.Series["Pitch"].Color = Color.Blue; chart1.Series["Pitch"].Points.AddXY(i, pitch); chart2.Series["Yaw"].ChartType = SeriesChartType.FastLine; chart2.Series["Yaw"].Color = Color.Orange; chart2.Series["Yaw"].Points.AddXY(i, yaw); chart3.Series["Roll"].ChartType = SeriesChartType.FastLine; chart3.Series["Roll"].Color = Color.Green; chart3.Series["Roll"].Points.AddXY(i, roll); //Diferencia /* chart1.Series["Error"].ChartType = SeriesChartType.FastLine; * chart1.Series["Error"].Color = Color.Red; * chart1.Series["Error"].Points.AddXY(i, differencepitch); * * * * chart2.Series["Error"].ChartType = SeriesChartType.FastLine; * chart2.Series["Error"].Color = Color.Red; * chart2.Series["Error"].Points.AddXY(i, differenceyaw); * * chart3.Series["Error"].ChartType = SeriesChartType.FastLine; * chart3.Series["Error"].Color = Color.Red; * chart3.Series["Error"].Points.AddXY(i, differenceroll);*/ i++; if (i == nmax) { nmin = nmax / 2; nmax = (nmax + nmin); } }
public static async Task Main(string[] args) { //Load in data from sensor MLContext mlContext = new MLContext(); IDataView dataView = mlContext.Data.LoadFromTextFile <SensorData>(path: linAccOriginal, hasHeader: true, separatorChar: ','); var accelerations = mlContext.Data.CreateEnumerable <SensorData>(dataView, reuseRowObject: false).ToList(); var time = accelerations.Select(x => x.Time).ToList(); //load data from python export IDataView counts = mlContext.Data.LoadFromTextFile <linnaccpy>(path: counts_path, hasHeader: true); var acc_counts = mlContext.Data.CreateEnumerable <linnaccpy>(counts, reuseRowObject: false).ToList(); //load data from wheel export IDataView wheelView = mlContext.Data.LoadFromTextFile <WheelD>(path: wheel_path, hasHeader: true, separatorChar: ','); var wheeldata = mlContext.Data.CreateEnumerable <WheelD>(wheelView, reuseRowObject: false).ToList(); //Apply filter List <double> difference = new List <double>(); for (int k = 1; k < accelerations.Count; k++) { difference.Add(accelerations[k].Time - accelerations[k - 1].Time); } var average = difference.Sum(x => x) / difference.Count(); var filteredAcceleration = Butterworth(accelerations.Select(x => x.Acceleration_x).ToArray(), average, 0.5); var jerk = Derivative(filteredAcceleration.ToList(), average); List <CustomDouble> accelerationRoots = new List <CustomDouble>(); bisection(1, filteredAcceleration.ToList().Count - 1, filteredAcceleration.ToList()); accelerationRoots = foundRoots.ToList(); exportdata(filteredAcceleration.ToList(), "versnelling_filtered", "m/s^2", true, foundRoots.Select(x => x.countInArray).ToList()); List <CustomDouble> max = new List <CustomDouble>(); for (int k = 0; k < foundRoots.Count; k++) { //Find all max: //f'(t+h) < 0 and f'(t-h) > 0 int h = 5; int tminh = foundRoots[k].countInArray - h; int tplush = foundRoots[k].countInArray + h; if (foundRoots[k].countInArray - h < 0) { tminh = foundRoots[k].countInArray; } if (foundRoots[k].countInArray + h > foundRoots.Count - 1) { tplush = foundRoots[k].countInArray + h; } if (jerk[tminh] > 0 && jerk[tplush] < 0) { max.Add(foundRoots[k]); } } difference = new List <double>(); for (int k = 1; k < wheeldata.Count; k++) { difference.Add(wheeldata[k].Time - wheeldata[k - 1].Time); } var averageWheelPeriod = difference.Sum(x => x) / difference.Count(); MadgwickAHRS AHRS = new MadgwickAHRS((float)averageWheelPeriod, 1f); List <string> eulerAngles = new List <string>(); //Z,Y,X for (int i = 0; i < wheeldata.Count - 2; i++) { var k = wheeldata[i]; float[] a = { (float)k.Accy / (float)9.81, (float)k.Accz / (float)9.81, (float)k.Accx / (float)9.81 }; float[] g = { (float)k.GyroY, (float)k.GyorZ, (float)k.GyorX }; float[] B = { (float)k.By, (float)k.Bz, (float)k.Bx }; var e = new MagData(g, a, B); AHRS.Update((e.Gyroscope[0]), (e.Gyroscope[1]), (e.Gyroscope[2]), e.Accelerometer[0], e.Accelerometer[1], e.Accelerometer[2]); var z = (new QuaternionData(AHRS.Quaternion)).ConvertToConjugate().ConvertToEulerAnglesCSVstring(); eulerAngles.Add(z); } System.Text.StringBuilder sb = new System.Text.StringBuilder(); sb.AppendLine($"Z,Y,X"); foreach (var item in eulerAngles) { sb.AppendLine(item.ToString()); } string eulerAnglespath = Path.Combine("/D/", "acc_export", $"euler_angles.csv"); System.IO.File.WriteAllText( eulerAnglespath, sb.ToString()); //rotatie om y-as dus... //load data from wheel export IDataView eulerView = mlContext.Data.LoadFromTextFile <EulerData>(path: eulerAnglespath, hasHeader: true, separatorChar: ','); var AnglesData = mlContext.Data.CreateEnumerable <EulerData>(eulerView, reuseRowObject: false).ToList(); // in deg exportdata(AnglesData.Select(x => x.Z).ToList(), "z-theta", "deg", true); exportdata(AnglesData.Select(x => x.Y).ToList(), "y-theta", "deg", true); exportdata(AnglesData.Select(x => x.X).ToList(), "x-theta", "deg", true); //perform bisection var y2 = Butterworth(wheeldata.Select(x => x.GyroY).ToArray(), averageWheelPeriod, 0.5); List <double> angleTimes = new List <double>(); for (int k = 0; k < AnglesData.Count; k++) { angleTimes.Add(k * averageWheelPeriod); } bisection(1, y2.ToList().Count - 1, y2.ToList()); exportdata(y2.ToList(), "gyro", "rad/s", true, foundRoots.Select(x => x.countInArray).ToList()); bisection(1, jerk.ToList().Count - 1, jerk.ToList()); exportdata(jerk.ToList(), "jerk", "m/s^3", true); List <CustomBool> maxOrMins = new List <CustomBool>(); for (int j = 0; j < foundRoots.Count; j++) { //f'(t-h) >0 en f'(t+h) < 0 , waar h een klein getal is. (maxima) int h = 5; int k = foundRoots[j].countInArray; bool IsMax = false; int indexForPlus = k + h; int indexForMin = k - h; if (k + h > jerk.Count) { indexForPlus = k; } else if (k - h < 0) { indexForMin = k; } var jj = jerk[indexForPlus]; if (jerk[indexForMin] > 0 && jerk[indexForPlus] < 0) { IsMax = true; } maxOrMins.Add(new CustomBool { index = k, Max = IsMax }); } exportdata(filteredAcceleration.ToList(), "acc", "m/s^2", true, maxOrMins.Select(x => (int)x.index).ToList()); List <double> t = new List <double>(); for (int k = 0; k < accelerationRoots.Count - 1; k++) { double averageXroot = (accelerationRoots[k + 1].countInArray + accelerationRoots[k].countInArray) / 2.0; var nearestMaxima = maxOrMins.OrderBy(x => Math.Abs((int)x.index - (int)averageXroot)).First(); if (nearestMaxima.Max) { var dt = (accelerationRoots[k + 1].countInArray - accelerationRoots[k].countInArray) * average; t.Add(dt); } } var countsmin = 5413.7; // 7.242048 kilometers per hour/TO CHANGE WHEN WE GET WRIST ACCELERATIONS! double m = 80; // 80 kg double r = 0.35; // wheel radius 35 cm //https://www.nature.com/articles/sc201533.pdf double EE = (0.0022 * countsmin + 3.13) / 1000; // V02 (L min-1 kg-1) double EEm = EE * m * 5 * (t.Sum() / 60.0); Console.WriteLine("Number of revolutions: " + (int)(foundRoots.Count / 2.0)); Console.WriteLine("Distance traveled: " + (int)((foundRoots.Count / 2.0) * 2 * Math.PI * r) + " m"); Console.WriteLine("Burned kcal: " + EEm); }
// VALUES PARSING void ParseSensorData(IBuffer characteristicValue) { if (eventData.Length != characteristicValue.Length) { eventData = new byte[characteristicValue.Length]; } DataReader.FromBuffer(characteristicValue).ReadBytes(eventData); var buffer = characteristicValue.ToArray(); if (buffer.Length < 3) { return; } //Array.Reverse(buffer); var accelerometer = new List <float> { getAccelerometerFloatWithOffsetFromArrayBufferAtIndex(buffer, 4, 0), getAccelerometerFloatWithOffsetFromArrayBufferAtIndex(buffer, 6, 0), getAccelerometerFloatWithOffsetFromArrayBufferAtIndex(buffer, 8, 0) }.Select(x => x * ACCEL_FACTOR).ToList(); var gyro = new List <float> { getGyroscopeFloatWithOffsetFromArrayBufferAtIndex(buffer, 10, 0), getGyroscopeFloatWithOffsetFromArrayBufferAtIndex(buffer, 12, 0), getGyroscopeFloatWithOffsetFromArrayBufferAtIndex(buffer, 14, 0) }.Select(x => x * GYRO_FACTOR).ToList(); var mag = new List <float> { getMagnetometerFloatWithOffsetFromArrayBufferAtIndex(buffer, 0), getMagnetometerFloatWithOffsetFromArrayBufferAtIndex(buffer, 2), getMagnetometerFloatWithOffsetFromArrayBufferAtIndex(buffer, 4) }; _ahrs.Update( gyro[0], gyro[1], gyro[2], accelerometer[0], accelerometer[1], accelerometer[2] ); // change quaternion format to Unity one _latestTrackingData.quaternion = new float[] { -_ahrs.Quaternion[1], -_ahrs.Quaternion[3], -_ahrs.Quaternion[2], _ahrs.Quaternion[0] }; _latestTrackingData.touchpadX = ( ((eventData[54] & 0xF) << 6) + ((eventData[55] & 0xFC) >> 2) ) & 0x3FF; // Max observed value = 315 _latestTrackingData.touchpadY = ( ((eventData[55] & 0x3) << 8) + ((eventData[56] & 0xFF) >> 0) ) & 0x3FF; _latestTrackingData.triggerButton = 0 != (eventData[58] & (1 << 0)); _latestTrackingData.homeButton = 0 != (eventData[58] & (1 << 1)); _latestTrackingData.backButton = 0 != (eventData[58] & (1 << 2)); _latestTrackingData.touchpadButton = 0 != (eventData[58] & (1 << 3)); _latestTrackingData.volumeDownButton = 0 != (eventData[58] & (1 << 4)); _latestTrackingData.volumeUpButton = 0 != (eventData[58] & (1 << 5)); _latestTrackingData.touchpadPressed = _latestTrackingData.touchpadX != 0 && _latestTrackingData.touchpadY != 0; var temperature = eventData[57]; //Log($"Touchpad: ({trackingData.touchpadX}, {trackingData.touchpadY})"); OnSensorDataUpdated(this); //Log($"trigger: {triggerButton}, home: {homeButton}, back: {backButton}, Q: {string.Join(", ", _ahrs.Quaternion)}"); }