<phyphox version="1.7">
    <title>Attitude and inclination</title>
    <category>Tools</category>
    <description>
        Test experiment to compare attitude, inclination and raw accelerometer and gyroscope data.
    </description>
    <data-containers>
        <container size="0">win</container>
        <container size="0">xin</container>
        <container size="0">yin</container>
        <container size="0">zin</container>
        <container size="0">woff</container>
        <container size="0">xoff</container>
        <container size="0">yoff</container>
        <container size="0">zoff</container>
        <container size="0">w</container>
        <container size="0">x</container>
        <container size="0">y</container>
        <container size="0">z</container>
        <container size="0">t</container>
        <container size="0">direct</container>
        <container size="0">yaw</container>
        <container size="0">pitch</container>
        <container size="0">roll</container>
        <container size="1" init="1">w0</container>
        <container size="1" init="0">x0</container>
        <container size="1" init="0">y0</container>
        <container size="1" init="0">z0</container>
        <container size="1" init="1">wlast</container>
        <container size="1" init="0">xlast</container>
        <container size="1" init="0">ylast</container>
        <container size="1" init="0">zlast</container>
        <container size="1">tmax</container>

        <container size="1">count</container>
        <container size="1">count-1</container>

        <container size="0">accX</container>
        <container size="0">accY</container>
        <container size="0">accZ</container>
        <container size="0">accT</container>
        <container size="0">gyrX</container>
        <container size="0">gyrY</container>
        <container size="0">gyrZ</container>
        <container size="0">gyrT</container>

        <container size="0">remainder</container>
        <container size="1" init="0">copyindex</container>
        <container size="1">ncopy</container>
        <container size="1">tavg</container>
        <container size="0">filterAccX</container>
        <container size="0">filterAccY</container>
        <container size="0">filterAccZ</container>
        <container size="1">filterT0</container>
        <container size="1">filterTmax</container>

        <container size="1">res_filteredT</container>
        <container size="1">res_angle</container>
        <container size="1">res_anglePlane</container>

        <container size="0">filteredT</container>
        <container size="0">angle</container>
        <container size="0">anglePlane</container>

    </data-containers>
    <input>
        <sensor type="attitude">
            <output component="x">xin</output>
            <output component="y">yin</output>
            <output component="z">zin</output>
            <output component="abs">win</output>
            <output component="t">t</output>
        </sensor>
        <sensor type="accelerometer">
          <output component="x">accX</output>
          <output component="y">accY</output>
          <output component="z">accZ</output>
          <output component="t">accT</output>
        </sensor>
        <sensor type="gyroscope">
          <output component="x">gyrX</output>
          <output component="y">gyrY</output>
          <output component="z">gyrZ</output>
          <output component="t">gyrT</output>
        </sensor>
    </input>
    <views>
        <view label="Attitude">
            <button label="Zero">
                <input>wlast</input>
                <output>w0</output>
                <input>xlast</input>
                <output>x0</output>
                <input>ylast</input>
                <output>y0</output>
                <input>zlast</input>
                <output>z0</output>
            </button>
            <value label="Direct ⍺" unit="°" size="2" precision="1">
                <input>direct</input>
            </value>
            <value label="Yaw ψ" unit="°" size="2" precision="1">
                <input>yaw</input>
            </value>
            <value label="Pitch θ" unit="°" size="2" precision="1">
                <input>pitch</input>
            </value>
            <value label="Roll φ" unit="°" size="2" precision="1">
                <input>roll</input>
            </value>
        </view>
        <view label="Attitude Graph">
            <button label="Zero">
                <input>wlast</input>
                <output>w0</output>
                <input>xlast</input>
                <output>x0</output>
                <input>ylast</input>
                <output>y0</output>
                <input>zlast</input>
                <output>z0</output>
            </button>
            <graph label="Direct" labelX="t" unitX="s" labelY="⍺" unitY="°" partialUpdate="true">
                <input axis="x">t</input>
                <input axis="y">direct</input>
            </graph>
            <graph label="Yaw" labelX="t" unitX="s" labelY="ψ" unitY="°" partialUpdate="true">
                <input axis="x">t</input>
                <input axis="y">yaw</input>
            </graph>
            <graph label="Pitch" labelX="t" unitX="s" labelY="θ" unitY="°" partialUpdate="true">
                <input axis="x">t</input>
                <input axis="y">pitch</input>
            </graph>
            <graph label="Roll" labelX="t" unitX="s" labelY="φ" unitY="°" partialUpdate="true">
                <input axis="x">t</input>
                <input axis="y">roll</input>
            </graph>
        </view>
        <view label="Inclination">
          <graph label="Inclination" timeOnX="true" labelX="[[quantity_short_time]]" unitX="[[unit_short_second]]" labelY="Angle" unitY="[[unit_short_degree]]" partialUpdate="true" style="line">
            <input axis="x">filteredT</input>
            <input axis="y">angle</input>
          </graph>
          <value label="Inclination" size="2" precision="2" scientific="false" unit="[[unit_short_degree]]" factor="1">
            <input>angle</input>
          </value>
          <separator height="1"/>
          <graph label="Rotation" timeOnX="true" labelX="[[quantity_short_time]]" unitX="[[unit_short_second]]" labelY="Angle" unitY="[[unit_short_degree]]" partialUpdate="true" style="line">
            <input axis="x">filteredT</input>
            <input axis="y">anglePlane</input>
          </graph>
          <value label="Rotation" size="2" precision="2" scientific="false" unit="[[unit_short_degree]]" factor="1">
            <input>anglePlane</input>
          </value>
          <edit label="Average period" default="0.5" unit="[[unit_short_second]]">
            <output>tavg</output>
          </edit>
        </view>
    </views>
    <analysis>
        <!-- INCLINATION -->

        <subrange>
            <input clear="false">accT</input>
            <input clear="false" as="from">copyindex</input>
            <output clear="true">remainder</output>
        </subrange>

        <first>
            <input clear="false">remainder</input>
            <output clear="false">filterT0</output>
        </first>

        <add>
            <input clear="false">filterT0</input>
            <input clear="false">tavg</input>
            <output>filterTmax</output>
        </add>

        <count>
            <input clear="false">remainder</input>
            <output>count</output>
        </count>

        <subtract>
            <input clear="false">count</input>
            <input type="value">1</input>
            <output>count-1</output>
        </subtract>

        <threshold>
            <input clear="false" as="y">remainder</input>
            <input clear="false" as="threshold">filterTmax</input>
            <output>ncopy</output>
        </threshold>

        <if equal="true" greater="true">
            <input clear="false">ncopy</input>
            <input clear="false">count-1</input>
            <input type="value">0</input>
            <input clear="false">ncopy</input>
            <output clear="false">ncopy</output>
        </if>

        <subrange>
            <input clear="false">accX</input>
            <input clear="false" as="from">copyindex</input>
            <input clear="false" as="length">ncopy</input>
            <output clear="true">filterAccX</output>
        </subrange>
        <subrange>
            <input clear="false">accY</input>
            <input clear="false" as="from">copyindex</input>
            <input clear="false" as="length">ncopy</input>
            <output clear="true">filterAccY</output>
        </subrange>
        <subrange>
            <input clear="false">accZ</input>
            <input clear="false" as="from">copyindex</input>
            <input clear="false" as="length">ncopy</input>
            <output clear="true">filterAccZ</output>
        </subrange>

        <formula formula="([1]+[2])/2">
            <input clear="false">filterT0</input>
            <input clear="false">filterTmax</input>
            <output>res_filteredT</output>
        </formula>

        <formula formula="atan2(sqrt([1_]*[1_]+[2_]*[2_]),[3_])*57.29577951">
          <input clear="false">filterAccX</input>
          <input clear="false">filterAccY</input>
          <input clear="false">filterAccZ</input>
          <output clear="false">res_angle</output>
        </formula>

        <formula formula="atan2([1_],[2_])*57.29577951">
          <input clear="false">filterAccX</input>
          <input clear="false">filterAccY</input>
          <output clear="false">res_anglePlane</output>
        </formula>

        <if greater="true">
            <input clear="false">ncopy</input>
            <input type="value">0</input>
            <input>res_angle</input>
            <output clear="false">angle</output>
        </if>

        <if greater="true">
            <input clear="false">ncopy</input>
            <input type="value">0</input>
            <input>res_anglePlane</input>
            <output clear="false">anglePlane</output>
        </if>

        <if greater="true">
            <input clear="false">ncopy</input>
            <input type="value">0</input>
            <input>res_filteredT</input>
            <output clear="false">filteredT</output>
        </if>

        <add>
            <input clear="false">copyindex</input>
            <input clear="false">ncopy</input>
            <output clear="false">copyindex</output>
        </add>

        <!-- ATTITUDE -->

        <!-- Store last raw value, so we can use it for the zero button -->
        <append>
            <input clear="false">win</input>
            <output>wlast</output>
        </append>
        <append>
            <input clear="false">xin</input>
            <output>xlast</output>
        </append>
        <append>
            <input clear="false">yin</input>
            <output>ylast</output>
        </append>
        <append>
            <input clear="false">zin</input>
            <output>zlast</output>
        </append>

        <!-- Apply zero offset to new values -->
        <formula formula="abs([1]*[5_]+[2]*[6_]+[3]*[7_]+[4]*[8_])">
            <input clear="false">w0</input>
            <input clear="false">x0</input>
            <input clear="false">y0</input>
            <input clear="false">z0</input>
            <input clear="false">win</input>
            <input clear="false">xin</input>
            <input clear="false">yin</input>
            <input clear="false">zin</input>
            <output clear="true">woff</output>
        </formula>

        <formula formula="([1]*[6_]-[2]*[5_]-[3]*[8_]+[4]*[7_])*sign([1]*[5_]+[2]*[6_]+[3]*[7_]+[4]*[8_])">
            <input clear="false">w0</input>
            <input clear="false">x0</input>
            <input clear="false">y0</input>
            <input clear="false">z0</input>
            <input clear="false">win</input>
            <input clear="false">xin</input>
            <input clear="false">yin</input>
            <input clear="false">zin</input>
            <output clear="true">xoff</output>
        </formula>

        <formula formula="([1]*[7_]+[2]*[8_]-[3]*[5_]-[4]*[6_])*sign([1]*[5_]+[2]*[6_]+[3]*[7_]+[4]*[8_])">
            <input clear="false">w0</input>
            <input clear="false">x0</input>
            <input clear="false">y0</input>
            <input clear="false">z0</input>
            <input clear="false">win</input>
            <input clear="false">xin</input>
            <input clear="false">yin</input>
            <input clear="false">zin</input>
            <output clear="true">yoff</output>
        </formula>

        <formula formula="([1]*[8_]-[2]*[7_]+[3]*[6_]-[4]*[5_])*sign([1]*[5_]+[2]*[6_]+[3]*[7_]+[4]*[8_])">
            <input clear="false">w0</input>
            <input clear="false">x0</input>
            <input clear="false">y0</input>
            <input clear="false">z0</input>
            <input clear="true">win</input>
            <input clear="true">xin</input>
            <input clear="true">yin</input>
            <input clear="true">zin</input>
            <output clear="true">zoff</output>
        </formula>

        <!-- Calculate angles for new values -->

        <formula formula="2*acos([1_])*57.295779513">
            <input clear="false">woff</input>
            <input clear="false">xoff</input>
            <input clear="false">yoff</input>
            <input clear="false">zoff</input>
            <output clear="false">direct</output>
        </formula>

        <formula formula="atan2(2*([1_]*[2_]+[3_]*[4_]),1-2*([2_]*[2_]+[3_]*[3_]))*57.295779513">
            <input clear="false">woff</input>
            <input clear="false">xoff</input>
            <input clear="false">yoff</input>
            <input clear="false">zoff</input>
            <output clear="false">yaw</output>
        </formula>

        <formula formula="asin(2*([1_]*[3_]-[2_]*[4_]))*57.295779513">
            <input clear="false">woff</input>
            <input clear="false">xoff</input>
            <input clear="false">yoff</input>
            <input clear="false">zoff</input>
            <output clear="false">pitch</output>
        </formula>

        <formula formula="atan2(2*([1_]*[4_]+[2_]*[3_]),1-2*([3_]*[3_]+[4_]*[4_]))*57.295779513">
            <input clear="false">woff</input>
            <input clear="false">xoff</input>
            <input clear="false">yoff</input>
            <input clear="false">zoff</input>
            <output clear="false">roll</output>
        </formula>

        <!-- APpend new values (with offset removed) to results -->

        <append>
            <input clear="true">woff</input>
            <output clear="false">w</output>
        </append>
        <append>
            <input clear="true">xoff</input>
            <output clear="false">x</output>
        </append>
        <append>
            <input clear="true">yoff</input>
            <output clear="false">y</output>
        </append>
        <append>
            <input clear="true">zoff</input>
            <output clear="false">z</output>
        </append>

    </analysis>
    <export>
        <set name="Attitude">
            <data name="Time (s)">t</data>
            <data name="w">w</data>
            <data name="x">x</data>
            <data name="y">y</data>
            <data name="z">z</data>
            <data name="Direct (°)">direct</data>
            <data name="Yaw (°)">yaw</data>
            <data name="Pitch (°)">pitch</data>
            <data name="Roll (°)">roll</data>
        </set>
        <set name="Inclination">
          <data name="t (s)">filteredT</data>
          <data name="Inclination (deg)">angle</data>
          <data name="Rotation (deg)">anglePlane</data>
        </set>
        <set name="Acceleration">
          <data name="t (s)">accT</data>
          <data name="Acceleration x (m/s^2)">accX</data>
          <data name="Acceleration y (m/s^2)">accY</data>
          <data name="Acceleration z (m/s^2)">accZ</data>
        </set>
        <set name="Gyroscope">
          <data name="t (s)">accT</data>
          <data name="Gyroscope x (rad/s)">gyrX</data>
          <data name="Gyroscope y (rad/s)">gyrY</data>
          <data name="Gyroscope z (rad/s)">gyrZ</data>
        </set>
    </export>
</phyphox>
