sfwa / TRICAL

Straightforward UKF-based scale and bias calibration for magnetometers (and other tri-axial field sensors).
http://au.tono.my/log/20131213-trical-magnetometer-calibration.html
MIT License
36 stars 17 forks source link

Usage in a mobile app #7

Open Lelelo1 opened 4 years ago

Lelelo1 commented 4 years ago

I have manage to interact with the library to some degree from C#.

The raw magnetometer on iphone11 on the left and the calibrated_reading float array values to the right as System.Numerics.Vector3:

<-83.00697, 16.52205, -389.2053> ---> <-80.01025, 17.02175, -388.8991> 50.01 <-83.33554, 16.6873, -389.5024> ---> <NaN, NaN, NaN> 50.02 Thread started: #19 <-83.27498, 16.83936, -388.3975> ---> <NaN, NaN, NaN> 50.02999 <-82.71381, 16.61551, -389.6036> ---> <NaN, NaN, NaN> 50.03999

I can set and get norm just fine. The first app update I get values back, but then there is only NaN values.

Here is the update code in the C# wrapper library:

public float[] Update(float[] raw)
{
    TRICALWrapper.TRICAL_estimate_update(handle, raw, ExpectedField);

    float[] calibrated_reading = new float[3];
    TRICALWrapper.TRICAL_measurement_calibrate(handle, raw, calibrated_reading);
    return calibrated_reading;
}

The handle is some sort of pointer to the TRICAL_instance.t, being of TRICALSafeHandle : SafeHandleZeroOrMinusOneIsInvalid in C#

// added two wrapper methods in TRICAL.cpp
TRICAL_instance_t* CreateTRICAL()
{
    return new TRICAL_instance_t();
}

void DisposeTRICAL(TRICAL_instance_t* ptr)
{
    if (ptr != nullptr)
    {
        delete ptr;
        ptr = nullptr;
    }
}

Also I had to change c -> cpp and remove retsrict keyword to remove error. The dll building guide seem to require cpp.

So is this problem, communicating between C# <---> C++, or what could be the problem?

Lelelo1 commented 4 years ago

Making a translation, the calibrated data that comes out is the same as raw input data

Lelelo1 commented 4 years ago

When running from a c console program I also get NaN, just as I got when running from C#. So there is probably something wrong in the library? expected_field - I assume is earth's magnetic field from world magnetic model? Sensor data is in ENU coordinate system, can a mismatch with wmm (which i think is in NED coordinate system) - causing NaN after first update?