How to use
===========

.. toctree::
   :maxdepth: 3
   :caption: Contents:

Platform support
----------------

This library does not contain any external dependency and can be used on any platform that supports C++11.
It is composed of a header and a static `.a` libary provided for the following platforms:

- `Linux`
- `Windows`
- `MacOs apple silicon`
- `Esp32s3`
- `Teensy4.X`

.. note::

    If you need a precompiled version for another platform, please contact us at `CONTACT <contact@sysrox.com>`_ . We will be happy to provide it in the next release!

The main function (:cpp:func:`SRX_MODEL_INS_10_DOF::step`) has to be called at **500Hz** by user application.

- Stack usage: 2.948 ko
- Global variables (tunable parameters): 1.44 ko

.. tip::

    The possibility to build the library on embedded device and on host computer allows for quick development and easy
    tuning of the parameters using replays of real data!

Convention
----------

For the library to work, datas have to be provided in the following convention:

- Accelerometer: accelerations in [g]
- Gyrometer: rotations speeds in [°/s]
- Magnetometer: magntic field intensities in [Gauss]
- Barometer: altitude (oriented in upward direction) in [m]

The orientation chosen is Front-Right-Down (FRD), which mean all sensors data have to be given relative
to this orientation to provide expected data. Depending on the sensors placements, mapping will have to be done.
For example:

`SRX-INS00-DEV`:

.. image:: images/SRX_INS00_DEV_orientation.png
   :width: 400
   :height: 350

Except for the barometer which is given in upward direction (altitude), all sensors data given following this xyz convention
will give orientation following the indicated arrows. If user wants different configuration, he will have to rotate this proposed
frame and give sensors data in the new frame.


Usage
---------

1. Create object and initialize it
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

.. important::
   API ref: :cpp:class:`SRX_MODEL_INS_10_DOF`

After the inclusion, create the stepable object:

.. code-block:: c++

   #include "libDM_SRX_10_DOF_interface.hpp"

   SRX_MODEL_INS_10_DOF ins;

In any function that is executed one time before loop (setp for arduino ...):

.. code-block:: c++

   ins.initialize();


2. Initialize inputs
~~~~~~~~~~~~~~~~~~~~~~~~~~~

.. important::
   API ref: :cpp:class:`SRX_MODEL_INS_10_DOF`, :cpp:class:`SRX_MODEL_INS_10_DOF::modelInputs`, :cpp:func:`SRX_MODEL_INS_10_DOF::setInputs`

Before the while loop, create a modelInputs object:

.. code-block:: c++
    :linenos:

    SRX_MODEL_INS_10_DOF::modelInputs insInputs = SRX_MODEL_INS_10_DOF::modelInputs();
    insInputs.reset                             = false;
    ins.setInputs(insInputs);


3. Feed the model and step it
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
.. important::
   API ref: :cpp:func:`SRX_MODEL_INS_10_DOF::setInputs`, :cpp:func:`SRX_MODEL_INS_10_DOF::step`

In the while loop, update sensors output, feed the model inputs it and step it at **500Hz**:

.. note::

   The mapping proposed is for the `SRX-INS00-DEV` board.

.. code-block:: c++
    :linenos:

    // Get sensors data
    icm.readFifo();
    resIMU = icm.getOutput();

    mmc.readMag();
    resMAG = mmc.getOutput();

    lps.readTemperaturePressureAltitude();
    resBARO = lps.getOutput();

    // Feed INS with sensors data
    insInputs.gyroDpsRate[0] = -resIMU.dpsGyroData[1];
    insInputs.gyroDpsRate[1] = -resIMU.dpsGyroData[0];
    insInputs.gyroDpsRate[2] = -resIMU.dpsGyroData[2];
    insInputs.acceleroG[0]   = -resIMU.gAccelData[1];
    insInputs.acceleroG[1]   = -resIMU.gAccelData[0];
    insInputs.acceleroG[2]   = -resIMU.gAccelData[2];
    insInputs.timestampUs    = myTimer.returnSystemTimestampUs64();

    insInputs.magGauss[0]    = -resMAG.GMagData[0];
    insInputs.magGauss[1]    = resMAG.GMagData[1];
    insInputs.magGauss[2]    = resMAG.GMagData[2];
    insInputs.magTimestampUs = resMAG.usMagTimestamp;

    insInputs.barometerAltitudeAmsl = resBARO.mAbsoluteAltitude;
    insInputs.barometerAltitudeRel  = resBARO.mRelativeAltitude;
    insInputs.barometerTimestampUs  = resBARO.usPressureTimestamp;

    // Set inputs and step model
    ins.setInputs(insInputs);
    ins.step(); // Every 2ms

.. important::

   Timestamp is not important for accelerometer and gyrometer as the model runs at the same polling frequency, but is important for magnetometer and barometer to specified their own timestamp as the fusion system will use it to determine if the data has been updated or not.

4. Get outputs
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

.. important::
   :cpp:class:`SRX_MODEL_INS_10_DOF::modelOutputs`, :cpp:func:`SRX_MODEL_INS_10_DOF::getOutputs`

After the step, outputs specified in :cpp:class:`SRX_MODEL_INS_10_DOF::modelOutputs` will be updated and can be retrieved
with copying the pointer content:

.. code-block:: c++
    :linenos:

    float roll = ins.getOutputs().yawPitchRollLocal[2];
    float pitch = ins.getOutputs().yawPitchRollLocal[1];
    float yawLocalOffsetted = *ins.getOutputs().yawLocalOffsetted;
    std::copy(ins.getOutputs().localToBodyQuat,
              ins.getOutputs().localToBodyQuat + 4,
              localQuaternion);

5. Building with your application
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

For this step, the only thing to do is to build `.a` file with your application. For example, with a simple main.cpp file which include
`libDM_SRX_10_DOF_interface.hpp`:

.. code-block:: bash

    g++ main.cpp -o libSrx10Dof_windows.a -o myApp.exe

If using platformio, in `platformio.ini` file add the following lines (for esp32s3):

.. code-block:: bash

        -L lib/libDM_SRX_INS_10_DOF/src/static_libs
	-l Srx10Dof_esp32s3.a

Parameters tuning
-----------------

The topic is detailed in the :ref:`Tuning <how-it-works-tuning>` section.

Inputs mapping
---------------

Examples of inputs mapping configurations for SYSROX `SRX-INS00-DEV` and `SRX-INS01-CAS` boards:


.. admonition:: SRX-INS00-DEV mapping:

    .. image:: images/SRX_INS00_DEV_orientation.png
        :align: center
        :width: 300
        :height: 270

    .. code-block:: c++

        SRX_MODEL_INS_10_DOF::modelInputs insInputs = SRX_MODEL_INS_10_DOF::modelInputs();

    GYROMETER:

    - insInputs.gyroDpsRate[0] = -imuGyroY;
    - insInputs.gyroDpsRate[1] = -imuGyroX;
    - insInputs.gyroDpsRate[2] = -imuGyroZ;

    ACCELEROMETER:

    - insInputs.acceleroG[0] = -imuAccY;
    - insInputs.acceleroG[1] = -imuAccX;
    - insInputs.acceleroG[2] = -imuAccZ;

    MAGNETOMETER:

    - insInputs.magGauss[0]    = -magFX;
    - insInputs.magGauss[1]    = magFY;
    - insInputs.magGauss[2]    = magFZ;

    BAROMETER:

    - insInputs.barometerAltitudeRel  = baroRelativeAltitude;


.. admonition:: SRX-INS00-DEV mapping:

    .. image:: images/SRX_INS01_CAS_orientation.png
        :align: center
        :width: 300
        :height: 212

    .. code-block:: c++

        SRX_MODEL_INS_10_DOF::modelInputs insInputs = SRX_MODEL_INS_10_DOF::modelInputs();

    GYROMETER:

    - insInputs.gyroDpsRate[0] = -imuGyroY;
    - insInputs.gyroDpsRate[1] = -imuGyroX;
    - insInputs.gyroDpsRate[2] = -imuGyroZ;

    ACCELEROMETER:

    - insInputs.acceleroG[0] = -imuAccY;
    - insInputs.acceleroG[1] = -imuAccX;
    - insInputs.acceleroG[2] = -imuAccZ;

    MAGNETOMETER:

    - insInputs.magGauss[0]    = magFY;
    - insInputs.magGauss[1]    = magFX;
    - insInputs.magGauss[2]    = magFZ;

    BAROMETER:

    - insInputs.barometerAltitudeRel  = baroRelativeAltitude;