A good compass on a smartphone, how to do that?

A

Ark Tarusov2017-09-11 11:13:16

Android

Ark Tarusov, 2017-09-11 11:13:16

Good afternoon.
I am making an AR application on Unity and I needed to align the camera with a compass.
But it turned out that the standard value is not an exact thing and at least it needs to be calibrated.
Since there is no calibration function for the compass itself (Like, for example, on Windows phones), you need to write it yourself, and for this you need to implement an algorithm for converting magnetic vectors to a pair with accelerometer vectors into a degree of deviation from north.
All the truth and not the truth realized this business.
But it works poorly, very poorly.
Calibration generally works every other time.
There was an assumption that the problem is also in the hardware part.
But after downloading a dozen compass applications from the market, we found out that everything seems to be fine.
Can someone tell me where you can get the source code for compasses of normal applications?
The main articles I relied on:
https://cache.freescale.com/files/sensors/doc/app_...
robotclass.ru/articles/magnetometer-and-compass
https://www.phidgets.com/docs/ Magnetometer_Primer
From the last article, the code was taken as a basis.

The code itself

Класс компаса

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