Добрый день.
Делаю AR приложение на юнити и понадобилось по компасу выравнивать камеру.
Но выяснилось что стандартное значение не точная вещь и как минимум его необходимо калибровать.
Так как функции калибровки самого компаса нет(Как например на виндовсфонах), ее необходимо написать самостоятельно, а для этого необходимо реализовать алгорим преобразования магнитных векторов на пару с векторами акселерометра в градус отклонения от севера.
Всеми правда и не правда реализовал это дело.
Но работает оно убого, очень убого.
Калибровка вообще через раз работает.
Было предположение что проблема еще и в апаратной части.
Но загрузив с маркета десяток приложений компасов выяснили что вроде все нормально.
Может подскажет кто где можно взять исходники компасов приложений нормальных?
Основные статьи на которые я опирался:
https://cache.freescale.com/files/sensors/doc/app_...
robotclass.ru/articles/magnetometer-and-compass
https://www.phidgets.com/docs/Magnetometer_Primer
Из последней статьи код был взят за основу.
Сам кодКласс компасаusing System;
using System.Collections.Generic;
using UnityEngine;
public class CorrectCompass
{
//This finds a magnetic north bearing, correcting for board tilt and roll as measured by the accelerometer
//This doesn't account for dynamic acceleration - ie accelerations other than gravity will throw off the calculation
private double[] lastAngles = { 0, 0, 0 };
private List<double[]> compassBearingFilter;
private int compassBearingFilterSize;
private Compass compass;
private double pitchAngle;
private double rollAngle;
public double get_pitchAngle() { return pitchAngle; }
public double get_rollAngle() { return rollAngle; }
private float tmpcompassBearing;
private CorrectAcceleration acCor = new CorrectAcceleration();
public CorrectCompass(int compassBearingFilterSize = 10)
{
this.compassBearingFilterSize = compassBearingFilterSize;
compassBearingFilter = new List<double[]>();
Input.compass.enabled=true;
}
public double Update()
{
Vector3 ac = acCor.Update();//MoveCamera.corAc;
double[] gravity = {
ac.x,
ac.y,
ac.z};
Vector3 cmp = Sensor.magneticField;//Input.compass.rawVector;//
double[] magField = {
((cmp.x-(CalibrationSens.mag_hard.x))*CalibrationSens.mag_soft.x),//*CalibrationSens.mag_soft.x,
((cmp.y-(CalibrationSens.mag_hard.y))*CalibrationSens.mag_soft.y),//*CalibrationSens.mag_soft.y,
((cmp.z-(CalibrationSens.mag_hard.z)))*CalibrationSens.mag_soft.z };//*CalibrationSens.mag_soft.z};
//Roll Angle - about axis 0
// tan(roll angle) = gy/gz
// Use Atan2 so we have an output os (-180 - 180) degrees
rollAngle = Math.Atan2(gravity[1], gravity[2]);
//Pitch Angle - about axis 1
// tan(pitch angle) = -gx / (gy * sin(roll angle) * gz * cos(roll angle))
// Pitch angle range is (-90 - 90) degrees
pitchAngle = Math.Atan(-gravity[0] / (gravity[1] * Math.Sin(rollAngle) + gravity[2] * Math.Cos(rollAngle)));
//Yaw Angle - about axis 2
// tan(yaw angle) = (mz * sin(roll) – my * cos(roll)) /
// (mx * cos(pitch) + my * sin(pitch) * sin(roll) + mz * sin(pitch) * cos(roll))
// Use Atan2 to get our range in (-180 - 180)
//
// Yaw angle == 0 degrees when axis 0 is pointing at magnetic north
double yawAngle = Math.Atan2(magField[2] * Math.Sin(rollAngle) - magField[1] * Math.Cos(rollAngle),
magField[0] * Math.Cos(pitchAngle) + magField[1] * Math.Sin(pitchAngle) * Math.Sin(rollAngle) + magField[2] * Math.Sin(pitchAngle) * Math.Cos(rollAngle));
//double yawAngle = Math.Atan2(-(magField[1] * Math.Cos(rollAngle) - magField[2] * Math.Sin(rollAngle)),
// magField[0] * Math.Cos(pitchAngle) + magField[1] * Math.Sin(rollAngle) * Math.Sin(pitchAngle) + magField[2] * Math.Cos(rollAngle) * Math.Sin(pitchAngle));
double[] angles = { rollAngle, pitchAngle, yawAngle };
//we low-pass filter the angle data so that it looks nicer on-screen
//make sure the filter buffer doesn't have values passing the -180<->180 mark
//Only for Roll and Yaw - Pitch will never have a sudden switch like that
for (int i = 0; i < 3; i += 2)
{
if (Math.Abs(angles[i] - lastAngles[i]) > 3)
{
foreach (double[] value in compassBearingFilter)
{
if (angles[i] > lastAngles[i])
{
value[i] += 360 * Math.PI / 180.0;
}
else
{
value[i] -= 360 * Math.PI / 180.0;
}
}
}
}
lastAngles = (double[])angles.Clone();
compassBearingFilter.Add((double[])angles.Clone());
if (compassBearingFilter.Count > compassBearingFilterSize)
compassBearingFilter.RemoveAt(0);
yawAngle = pitchAngle = rollAngle = 0;
foreach (double[] stuff in compassBearingFilter)
{
rollAngle += stuff[0];
pitchAngle += stuff[1];
yawAngle += stuff[2];
}
yawAngle /= compassBearingFilter.Count;
pitchAngle /= compassBearingFilter.Count;
rollAngle /= compassBearingFilter.Count;
double compassBearing = yawAngle * (180.0 / Math.PI);
if (compassBearing < 0) compassBearing = 180 + (180 + compassBearing);
return compassBearing;
}
}
Класс акселерометраusing UnityEngine;
public class CorrectAcceleration
{
private float AccelerometerUpdateInterval = 1.0f / 60.0f;
private float LowPassFilterFactor;
private Vector3 lowPassValue = Vector3.zero;
public CorrectAcceleration(float LowPassKernelWidthInSeconds = 1.0f)
{
LowPassFilterFactor = AccelerometerUpdateInterval / LowPassKernelWidthInSeconds;
lowPassValue = Average();
}
//Высчитываем среднее значение акселерометра за кадр
private Vector3 Average()
{
float period = 0f;
Vector3 acc = Vector3.zero;
foreach (AccelerationEvent evnt in Input.accelerationEvents)
{
acc += evnt.acceleration * evnt.deltaTime;
period += evnt.deltaTime;
}
if (period > 0)
acc *= 1.0f / period;
return acc;
}
//Фильтруем шумы
public Vector3 Update()
{
lowPassValue = Vector3.Lerp(lowPassValue, Average(), LowPassFilterFactor);
return lowPassValue;
}
}
Метод калибровки (одна из версий, так как варианта нашел два)public void Compass()
{
/* Get a new sensor event */
magEvent = Sensor.magneticField;
if (magEvent.x < mag_min.x) mag_min.x = magEvent.x;
if (magEvent.x > mag_max.x) mag_max.x = magEvent.x;
if (magEvent.y < mag_min.y) mag_min.y = magEvent.y;
if (magEvent.y > mag_max.y) mag_max.y = magEvent.y;
if (magEvent.z < mag_min.z) mag_min.z = magEvent.z;
if (magEvent.z > mag_max.z) mag_max.z = magEvent.z;
mag_hard.x = (mag_max.x + mag_min.x) / 2; // get average x mag bias in counts
mag_hard.y = (mag_max.y + mag_min.y) / 2; // get average y mag bias in counts
mag_hard.z = (mag_max.z + mag_min.z) / 2; // get average z mag bias in counts
// Get soft iron correction estimate
mag_soft_temp.x = (mag_max.x - mag_min.x) / 2; // get average x axis max chord length in counts
mag_soft_temp.y = (mag_max.y - mag_min.y) / 2; // get average y axis max chord length in counts
mag_soft_temp.z = (mag_max.z - mag_min.z) / 2; // get average z axis max chord length in counts
float avg_rad = mag_soft_temp.x + mag_soft_temp.y + mag_soft_temp.z;
avg_rad /= 3.0f;
mag_soft.x = avg_rad / mag_soft_temp.x;
mag_soft.y = avg_rad / mag_soft_temp.y;
mag_soft.z = avg_rad / mag_soft_temp.z;
}
Второй вариантpublic void CalibrationSens()
{
cmp = Input.compass.rawVector;
if (cmp.x < cmp_min.x) cmp_min.x = cmp.x;
if (cmp.x > cmp_max.x) cmp_max.x = cmp.x;
if (cmp.y < cmp_min.y) cmp_min.y = cmp.y;
if (cmp.y > cmp_max.y) cmp_max.y = cmp.y;
if (cmp.z < cmp_min.z) cmp_min.z = cmp.z;
if (cmp.z > cmp_max.z) cmp_max.z = cmp.z;
cmp_hard.x = (cmp_min.x - cmp_max.x) / 2 - cmp_min.x;
cmp_hard.y = (cmp_min.y - cmp_max.y) / 2 - cmp_min.y;
cmp_hard.z = (cmp_min.z - cmp_max.z) / 2 - cmp_min.z;
cmp_tmp = cmp_max - cmp_min;
if (cmp_tmp.x > cmp_tmp.y && cmp_tmp.x > cmp_tmp.z)
{
cmp_soft.x = 0;
cmp_soft.y = cmp_tmp.x / cmp_tmp.y;
cmp_soft.z = cmp_tmp.x / cmp_tmp.z;
}
else if (cmp_tmp.y > cmp_tmp.z && cmp_tmp.y > cmp_tmp.x)
{
cmp_soft.x = cmp_tmp.y / cmp_tmp.x;
cmp_soft.y = 0;
cmp_soft.z = cmp_tmp.y / cmp_tmp.z;
}
else
{
cmp_soft.x = cmp_tmp.z / cmp_tmp.x;
cmp_soft.y = cmp_tmp.z / cmp_tmp.y;
cmp_soft.z = 0;
}
}
Сложный
