Skip to content

Commit

Permalink
MData2 callback data structure includes tangent-plane coordinate system
Browse files Browse the repository at this point in the history
Provides end-users with ability to check if a rotation was ENU, NED, NWU.
See #10
  • Loading branch information
Scottapotamas committed Oct 18, 2022
1 parent 5ab9dde commit d7d8e0f
Show file tree
Hide file tree
Showing 3 changed files with 21 additions and 1 deletion.
10 changes: 9 additions & 1 deletion src/xsens_constants.h
Original file line number Diff line number Diff line change
Expand Up @@ -82,10 +82,19 @@ typedef enum
XSENS_EVT_TYPE_DOUBLE9,
} XsensEventDataType_t;

// Enum describes possible types sent to user in the event data union
typedef enum
{
XSENS_EVT_ENU = 0x0,
XSENS_EVT_NED = 0x4,
XSENS_EVT_NWU = 0x8,
} XsensEventTangentPlaneFormat_t;

// Unionised data sent to user callback
typedef struct
{
XsensEventDataType_t type;
XsensEventTangentPlaneFormat_t coord_ref;
union
{
uint8_t u1;
Expand Down Expand Up @@ -329,7 +338,6 @@ enum XSENS_COORDINATE_SYSTEM
XSENS_COORD_NWU = 0x8,
};

// TODO: consider enum cleanup, and supporting these in the callback union?
enum XSENS_FLOAT_TYPE
{
XSENS_FLOAT_SINGLE = 0x0,
Expand Down
3 changes: 3 additions & 0 deletions src/xsens_mdata2.c
Original file line number Diff line number Diff line change
Expand Up @@ -158,6 +158,9 @@ void xsens_mdata2_decode_field( mdata2_packet_t *output, callback_event_t evt_cb
}
}

// Provide the local tangent plane coordinate scheme in the callback
value.coord_ref = coordinate_system;

// Convert BE data to LE, put it in the right union field
switch( value.type )
{
Expand Down
9 changes: 9 additions & 0 deletions test/test_xsens_mti_parse_messages.c
Original file line number Diff line number Diff line change
Expand Up @@ -218,6 +218,7 @@ void test_parse_mdata2_quat_minimal( void )
// Quaternion
TEST_ASSERT_EQUAL_INT( XSENS_EVT_QUATERNION, cb_evt_flag_cache[2] );
TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_FLOAT4, cb_evt_data_cache[2].type );
TEST_ASSERT_EQUAL_INT( XSENS_EVT_ENU, cb_evt_data_cache[2].coord_ref );
float golden_quat[4] = { 0.94455600, -0.32308814, 0.01374718, -0.05691256 };
TEST_ASSERT_EQUAL_FLOAT_ARRAY( golden_quat, cb_evt_data_cache[2].data.f4x4, 4);

Expand Down Expand Up @@ -276,6 +277,7 @@ void test_parse_mdata2_shake_clipping( void )
// Quaternion
TEST_ASSERT_EQUAL_INT( XSENS_EVT_QUATERNION, cb_evt_flag_cache[2] );
TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_FLOAT4, cb_evt_data_cache[2].type );
TEST_ASSERT_EQUAL_INT( XSENS_EVT_ENU, cb_evt_data_cache[2].coord_ref );
float golden_quat[4] = { 0.66437358, -0.42175028, 0.02720882, 0.61643654 };
TEST_ASSERT_EQUAL_FLOAT_ARRAY( golden_quat, cb_evt_data_cache[2].data.f4x4, 4);

Expand Down Expand Up @@ -390,6 +392,7 @@ void test_parse_mdata2_full( void )
// Quaternion
TEST_ASSERT_EQUAL_INT( XSENS_EVT_QUATERNION, cb_evt_flag_cache[2] );
TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_FLOAT4, cb_evt_data_cache[2].type );
TEST_ASSERT_EQUAL_INT( XSENS_EVT_ENU, cb_evt_data_cache[2].coord_ref );
float golden_quat[4] = { 0.71045315, 0.69453555, -0.07777759, -0.08262789 };
TEST_ASSERT_EQUAL_FLOAT_ARRAY( golden_quat, cb_evt_data_cache[2].data.f4x4, 4);

Expand Down Expand Up @@ -489,6 +492,7 @@ void test_parse_mdata2_number_formats( void )

// EulerAngles
TEST_ASSERT_EQUAL_INT( XSENS_EVT_EULER, cb_evt_flag_cache[1] );
TEST_ASSERT_EQUAL_INT( XSENS_EVT_ENU, cb_evt_data_cache[1].coord_ref );
TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_1220FP3, cb_evt_data_cache[1].type );
float golden_pry[3] = { -0.9826155, -0.1385441, 115.7006302 };

Expand Down Expand Up @@ -560,6 +564,7 @@ void test_parse_mdata2_number_formats_2( void )
// Quaternion
TEST_ASSERT_EQUAL_INT( XSENS_EVT_QUATERNION, cb_evt_flag_cache[1] );
TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_FLOAT4, cb_evt_data_cache[1].type );
TEST_ASSERT_EQUAL_INT( XSENS_EVT_ENU, cb_evt_data_cache[1].coord_ref );
float golden_quat[4] = { 0.6447144, -0.00622723, -0.00247884, 0.7643942 };
TEST_ASSERT_EQUAL_FLOAT_ARRAY( golden_quat, cb_evt_data_cache[1].data.f4x4, 4);

Expand Down Expand Up @@ -610,6 +615,7 @@ void test_parse_mdata2_rotmatrix_float( void )
xsens_mti_parse_buffer( &test_imu, test_packet, sizeof(test_packet));

TEST_ASSERT_EQUAL_INT( XSENS_EVT_ROT_MATRIX, cb_evt_flag_cache[0] );
TEST_ASSERT_EQUAL_INT( XSENS_EVT_ENU, cb_evt_data_cache[0].coord_ref );
TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_FLOAT9, cb_evt_data_cache[0].type );
float golden_rot[9] = { -0.21940619, -0.97555816, 0.01214410,
0.97537732, -0.21961689, -0.02019581,
Expand All @@ -635,6 +641,7 @@ void test_parse_mdata2_rotmatrix_fp1220( void )
xsens_mti_parse_buffer( &test_imu, test_packet, sizeof(test_packet));

TEST_ASSERT_EQUAL_INT( XSENS_EVT_ROT_MATRIX, cb_evt_flag_cache[0] );
TEST_ASSERT_EQUAL_INT( XSENS_EVT_ENU, cb_evt_data_cache[0].coord_ref );
TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_1220FP9, cb_evt_data_cache[0].type );
float golden_rot[9] = { -0.23230076, -0.97257423, 0.01154137,
0.97240829, -0.23248863, -0.01924133,
Expand Down Expand Up @@ -669,6 +676,7 @@ void test_parse_mdata2_rotmatrix_fp1632( void )
xsens_mti_parse_buffer( &test_imu, test_packet, sizeof(test_packet));

TEST_ASSERT_EQUAL_INT( XSENS_EVT_ROT_MATRIX, cb_evt_flag_cache[0] );
TEST_ASSERT_EQUAL_INT( XSENS_EVT_ENU, cb_evt_data_cache[0].coord_ref );
TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_1632FP9, cb_evt_data_cache[0].type );
float golden_rot[9] = { -0.23666316, -0.97153002, 0.01097167,
0.97137016, -0.23683536, -0.01869209,
Expand Down Expand Up @@ -703,6 +711,7 @@ void test_parse_mdata2_rotmatrix_double( void )
xsens_mti_parse_buffer( &test_imu, test_packet, sizeof(test_packet));

TEST_ASSERT_EQUAL_INT( XSENS_EVT_ROT_MATRIX, cb_evt_flag_cache[0] );
TEST_ASSERT_EQUAL_INT( XSENS_EVT_ENU, cb_evt_data_cache[0].coord_ref );
TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_DOUBLE9, cb_evt_data_cache[0].type );
double golden_rot[9] = { -0.23455584, -0.97204965, 0.01015384,
0.97191745, -0.23470223, -0.01706769,
Expand Down

0 comments on commit d7d8e0f

Please sign in to comment.