Skip to content

Commit

Permalink
Rough parser support for fixed-point and double precision MData2 packets
Browse files Browse the repository at this point in the history 
As part of #10.
  • Loading branch information
@Scottapotamas
Scottapotamas committed Oct 12, 2022
1 parent a5c409f commit 0c56fba
Show file tree
Hide file tree
Showing 3 changed files with 99 additions and 17 deletions.
41 changes: 40 additions & 1 deletion src/xsens_constants.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,11 +60,26 @@ typedef enum
XSENS_EVT_TYPE_U8,
XSENS_EVT_TYPE_U16,
XSENS_EVT_TYPE_U32,
XSENS_EVT_TYPE_FLOAT,
XSENS_EVT_TYPE_FLOAT = 10,
XSENS_EVT_TYPE_FLOAT2,
XSENS_EVT_TYPE_FLOAT3,
XSENS_EVT_TYPE_FLOAT4,
XSENS_EVT_TYPE_FLOAT9,
XSENS_EVT_TYPE_1220FP = 20,
XSENS_EVT_TYPE_1220FP2,
XSENS_EVT_TYPE_1220FP3,
XSENS_EVT_TYPE_1220FP4,
XSENS_EVT_TYPE_1220FP9,
XSENS_EVT_TYPE_1632FP = 30,
XSENS_EVT_TYPE_1632FP2,
XSENS_EVT_TYPE_1632FP3,
XSENS_EVT_TYPE_1632FP4,
XSENS_EVT_TYPE_1632FP9,
XSENS_EVT_TYPE_DOUBLE = 40,
XSENS_EVT_TYPE_DOUBLE2,
XSENS_EVT_TYPE_DOUBLE3,
XSENS_EVT_TYPE_DOUBLE4,
XSENS_EVT_TYPE_DOUBLE9,
} XsensEventDataType_t;

// Unionised data sent to user callback
Expand All @@ -76,11 +91,35 @@ typedef struct
uint8_t u1;
uint16_t u2;
uint32_t u4;

// Single precision
float f4;
float f4x2[2];
float f4x3[3];
float f4x4[4];
float f4x9[9];

// Fixed-point 12.20
int32_t fp1220;
int32_t fp1220x2[2];
int32_t fp1220x3[3];
int32_t fp1220x4[4];
int32_t fp1220x9[9];

// Fixed-point 16.32
int64_t fp1632;
int64_t fp1632x2[2];
int64_t fp1632x3[3];
int64_t fp1632x4[4];
int64_t fp1632x9[9];

// Double precision
double f8;
double f8x2[2];
double f8x3[3];
double f8x4[4];
double f8x9[9];

} data;
} XsensEventData_t;

Expand Down
51 changes: 48 additions & 3 deletions src/xsens_mdata2.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,11 +120,15 @@ void xsens_mdata2_decode_field( mdata2_packet_t *output, callback_event_t evt_cb
XsensEventData_t value = { 0 };
const mdata2_decode_rules_t *decode_rule = 0;

enum XSENS_FLOAT_TYPE number_precision = XSENS_IDENTIFIER_FORMAT_GET_PRECISION( output->id );
enum XSENS_COORDINATE_SYSTEM coordinate_system = XSENS_IDENTIFIER_FORMAT_GET_COORD_SYSTEM( output->id );
uint16_t id_simplifed = XSENS_IDENTIFIER_FORMAT_SIMPLIFY( output->id );

// Find the matching XID in the table
uint8_t table_length = sizeof( xid_decode_table ) / sizeof( mdata2_decode_rules_t );
for( uint8_t i = 0; i < table_length; i++ )
{
if( xid_decode_table[i].xid == output->id )
if( xid_decode_table[i].xid == id_simplifed )
{
decode_rule = &xid_decode_table[i];
break;
Expand All @@ -135,10 +139,12 @@ void xsens_mdata2_decode_field( mdata2_packet_t *output, callback_event_t evt_cb
if( decode_rule )
{
// The structure describes the union type
value.type = decode_rule->type;
// - for situations where non-single precision is used, apply an offset to the enum
// to correctly describe the type as fixed-precision or double, etc
value.type = decode_rule->type + ( number_precision * 10 );

// Convert BE data to LE, put it in the right union field
switch( decode_rule->type )
switch( value.type )
{
case XSENS_EVT_TYPE_U8:
value.data.u1 = output->payload[0];
Expand All @@ -153,28 +159,33 @@ void xsens_mdata2_decode_field( mdata2_packet_t *output, callback_event_t evt_cb
break;

case XSENS_EVT_TYPE_FLOAT:
case XSENS_EVT_TYPE_1220FP:
value.data.f4 = xsens_coalesce_32BE_F32LE( &output->payload[0] );
break;

case XSENS_EVT_TYPE_FLOAT2:
case XSENS_EVT_TYPE_1220FP2:
value.data.f4x2[0] = xsens_coalesce_32BE_F32LE( &output->payload[0] );
value.data.f4x2[1] = xsens_coalesce_32BE_F32LE( &output->payload[4] );
break;

case XSENS_EVT_TYPE_FLOAT3:
case XSENS_EVT_TYPE_1220FP3:
value.data.f4x3[0] = xsens_coalesce_32BE_F32LE( &output->payload[0] );
value.data.f4x3[1] = xsens_coalesce_32BE_F32LE( &output->payload[4] );
value.data.f4x3[2] = xsens_coalesce_32BE_F32LE( &output->payload[8] );
break;

case XSENS_EVT_TYPE_FLOAT4:
case XSENS_EVT_TYPE_1220FP4:
value.data.f4x4[0] = xsens_coalesce_32BE_F32LE( &output->payload[0] );
value.data.f4x4[1] = xsens_coalesce_32BE_F32LE( &output->payload[4] );
value.data.f4x4[2] = xsens_coalesce_32BE_F32LE( &output->payload[8] );
value.data.f4x4[3] = xsens_coalesce_32BE_F32LE( &output->payload[12] );
break;

case XSENS_EVT_TYPE_FLOAT9:
case XSENS_EVT_TYPE_1220FP9:
value.data.f4x9[0] = xsens_coalesce_32BE_F32LE( &output->payload[0] );
value.data.f4x9[1] = xsens_coalesce_32BE_F32LE( &output->payload[4] );
value.data.f4x9[2] = xsens_coalesce_32BE_F32LE( &output->payload[8] );
Expand All @@ -186,6 +197,40 @@ void xsens_mdata2_decode_field( mdata2_packet_t *output, callback_event_t evt_cb
value.data.f4x9[8] = xsens_coalesce_32BE_F32LE( &output->payload[32] );
break;

case XSENS_EVT_TYPE_DOUBLE:
xsens_swap_endian_u64( &value.data.f8, &output->payload[0] );
break;

case XSENS_EVT_TYPE_DOUBLE2:
xsens_swap_endian_u64( &value.data.f8x2[0], &output->payload[0] );
xsens_swap_endian_u64( &value.data.f8x2[1], &output->payload[8] );
break;

case XSENS_EVT_TYPE_DOUBLE3:
xsens_swap_endian_u64( &value.data.f8x3[0], &output->payload[0] );
xsens_swap_endian_u64( &value.data.f8x3[1], &output->payload[8] );
xsens_swap_endian_u64( &value.data.f8x3[2], &output->payload[16] );
break;

case XSENS_EVT_TYPE_DOUBLE4:
xsens_swap_endian_u64( &value.data.f8x4[0], &output->payload[0] );
xsens_swap_endian_u64( &value.data.f8x4[1], &output->payload[8] );
xsens_swap_endian_u64( &value.data.f8x4[2], &output->payload[16] );
xsens_swap_endian_u64( &value.data.f8x4[3], &output->payload[24] );
break;

case XSENS_EVT_TYPE_DOUBLE9:
xsens_swap_endian_u64( &value.data.f8x9[0], &output->payload[0] );
xsens_swap_endian_u64( &value.data.f8x9[1], &output->payload[8] );
xsens_swap_endian_u64( &value.data.f8x9[2], &output->payload[16] );
xsens_swap_endian_u64( &value.data.f8x9[3], &output->payload[24] );
xsens_swap_endian_u64( &value.data.f8x9[4], &output->payload[32] );
xsens_swap_endian_u64( &value.data.f8x9[5], &output->payload[40] );
xsens_swap_endian_u64( &value.data.f8x9[6], &output->payload[48] );
xsens_swap_endian_u64( &value.data.f8x9[7], &output->payload[56] );
xsens_swap_endian_u64( &value.data.f8x9[8], &output->payload[64] );
break;

default:
// There's an error or not supported, return a 'null' type?
value.type = XSENS_EVT_TYPE_NONE;
Expand Down
24 changes: 11 additions & 13 deletions test/test_xsens_mti_parse_messages.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -482,25 +482,23 @@ void test_parse_mdata2_number_formats( void )

xsens_mti_parse_buffer( &test_imu, test_packet, sizeof(test_packet));

// TODO: mdata2 parser needs to support other typed MID's.
printf("cache_usage: %i\n",cache_usage);

for(uint8_t i = 0; i <= cache_usage; i++)
{
printf(" - evt flg: %i\n",cb_evt_flag_cache[i]);

}

// PacketCounter
TEST_ASSERT_EQUAL_INT( XSENS_EVT_PACKET_COUNT, cb_evt_flag_cache[0] );
TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_U16, cb_evt_data_cache[0].type );
TEST_ASSERT_EQUAL_INT( 15325, cb_evt_data_cache[0].data.u2);

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

// Convert the fixed-point 1220 roll/pitch/yaw array into floats
float result_pry[3] = { 0 };
result_pry[0] = xsens_fp1220_to_f32( cb_evt_data_cache[1].data.fp1220x3[0] );
result_pry[1] = xsens_fp1220_to_f32( cb_evt_data_cache[1].data.fp1220x3[1] );
result_pry[2] = xsens_fp1220_to_f32( cb_evt_data_cache[1].data.fp1220x3[2] );
TEST_ASSERT_EQUAL_FLOAT_ARRAY( golden_pry, result_pry, 3);


// DeltaQ
TEST_ASSERT_EQUAL_INT( XSENS_EVT_DELTA_Q, cb_evt_flag_cache[7] );
Expand All @@ -510,9 +508,9 @@ void test_parse_mdata2_number_formats( void )

// MagneticField
TEST_ASSERT_EQUAL_INT( XSENS_EVT_MAGNETIC, cb_evt_flag_cache[3] );
TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_FLOAT3, cb_evt_data_cache[3].type );
TEST_ASSERT_EQUAL_INT( XSENS_EVT_TYPE_DOUBLE3, cb_evt_data_cache[3].type );
double golden_mag[3] = { 0.833747744560242, -0.434182971715927, 1.617681622505188 };
TEST_ASSERT_EQUAL_FLOAT_ARRAY( golden_mag, cb_evt_data_cache[3].data.f4x3, 3);
TEST_ASSERT_EQUAL_DOUBLE_ARRAY( golden_mag, cb_evt_data_cache[3].data.f4x3, 3);

// Temperature
TEST_ASSERT_EQUAL_INT( XSENS_EVT_TEMPERATURE, cb_evt_flag_cache[4] );
Expand Down

0 comments on commit 0c56fba

Please sign in to comment.