kreo_OL
@kreo_OL
Медузко -_-

Хороший компас на смартфон, как сделать то?

Добрый день.
Делаю 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;
        }
    }
  • Вопрос задан
  • 580 просмотров
Пригласить эксперта
Ваш ответ на вопрос

Войдите, чтобы написать ответ

Войти через центр авторизации
Похожие вопросы