rik freije / Mbed 2 deprecated MTi-1_rikbeun

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed-rtos mbed Xbus

Fork of MTi-1_example by Xsens

xbus/mtdata2.c@6:28cdd6ee6b9d, 2015-05-13 (annotated)

Committer:
Alex Young
Date:
Wed May 13 14:51:24 2015 +0200
Revision:
6:28cdd6ee6b9d
Child:
15:558d279addd9
Basic implementation for extracting items from MTData2 packets

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Alex Young 6:28cdd6ee6b9d 1 /*!
Alex Young 6:28cdd6ee6b9d 2 * \file
Alex Young 6:28cdd6ee6b9d 3 * \copyright
Alex Young 6:28cdd6ee6b9d 4 * Copyright (C) Xsens Technologies B.V., 2015. All rights reserved.
Alex Young 6:28cdd6ee6b9d 5 *
Alex Young 6:28cdd6ee6b9d 6 * This source code is intended for use only by Xsens Technologies BV and
Alex Young 6:28cdd6ee6b9d 7 * those that have explicit written permission to use it from
Alex Young 6:28cdd6ee6b9d 8 * Xsens Technologies BV.
Alex Young 6:28cdd6ee6b9d 9 *
Alex Young 6:28cdd6ee6b9d 10 * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
Alex Young 6:28cdd6ee6b9d 11 * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
Alex Young 6:28cdd6ee6b9d 12 * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
Alex Young 6:28cdd6ee6b9d 13 * PARTICULAR PURPOSE.
Alex Young 6:28cdd6ee6b9d 14 */
Alex Young 6:28cdd6ee6b9d 15
Alex Young 6:28cdd6ee6b9d 16 #include "mtdata2.h"
Alex Young 6:28cdd6ee6b9d 17 #include <stddef.h>
Alex Young 6:28cdd6ee6b9d 18
Alex Young 6:28cdd6ee6b9d 19 static uint8_t const* getPointerToData(enum XsDataIdentifier id, uint8_t const* data, uint16_t dataLength)
Alex Young 6:28cdd6ee6b9d 20 {
Alex Young 6:28cdd6ee6b9d 21 uint8_t const* dptr = data;
Alex Young 6:28cdd6ee6b9d 22 while (dptr < data + dataLength)
Alex Young 6:28cdd6ee6b9d 23 {
Alex Young 6:28cdd6ee6b9d 24 const uint16_t itemId = (dptr[0] << 8) | dptr[1];
Alex Young 6:28cdd6ee6b9d 25 dptr += sizeof(itemId);
Alex Young 6:28cdd6ee6b9d 26 const uint8_t itemSize = *dptr++;
Alex Young 6:28cdd6ee6b9d 27
Alex Young 6:28cdd6ee6b9d 28 if (id == itemId)
Alex Young 6:28cdd6ee6b9d 29 return dptr;
Alex Young 6:28cdd6ee6b9d 30
Alex Young 6:28cdd6ee6b9d 31 dptr += itemSize;
Alex Young 6:28cdd6ee6b9d 32 }
Alex Young 6:28cdd6ee6b9d 33 return NULL;
Alex Young 6:28cdd6ee6b9d 34 }
Alex Young 6:28cdd6ee6b9d 35
Alex Young 6:28cdd6ee6b9d 36 static void readShort(uint16_t* out, uint8_t const* raw)
Alex Young 6:28cdd6ee6b9d 37 {
Alex Young 6:28cdd6ee6b9d 38 *out = (raw[0] << 8) | raw[1];
Alex Young 6:28cdd6ee6b9d 39 }
Alex Young 6:28cdd6ee6b9d 40
Alex Young 6:28cdd6ee6b9d 41 static void readInt(uint32_t* out, uint8_t const* raw)
Alex Young 6:28cdd6ee6b9d 42 {
Alex Young 6:28cdd6ee6b9d 43 *out = (raw[0] << 24) | (raw[1] << 16) | (raw[2] << 8) | raw[3];
Alex Young 6:28cdd6ee6b9d 44 }
Alex Young 6:28cdd6ee6b9d 45
Alex Young 6:28cdd6ee6b9d 46 static void readFloats(float* out, uint8_t const* raw, uint8_t floats)
Alex Young 6:28cdd6ee6b9d 47 {
Alex Young 6:28cdd6ee6b9d 48 for (int i = 0; i < floats; ++i)
Alex Young 6:28cdd6ee6b9d 49 {
Alex Young 6:28cdd6ee6b9d 50 readInt((uint32_t*)&out[i], &raw[i * sizeof(float)]);
Alex Young 6:28cdd6ee6b9d 51 }
Alex Young 6:28cdd6ee6b9d 52 }
Alex Young 6:28cdd6ee6b9d 53
Alex Young 6:28cdd6ee6b9d 54 bool MtData2_getItem(void* item, enum XsDataIdentifier id, uint8_t const* data, uint16_t dataLength)
Alex Young 6:28cdd6ee6b9d 55 {
Alex Young 6:28cdd6ee6b9d 56 uint8_t const* raw = getPointerToData(id, data, dataLength);
Alex Young 6:28cdd6ee6b9d 57 if (raw)
Alex Young 6:28cdd6ee6b9d 58 {
Alex Young 6:28cdd6ee6b9d 59 switch (id)
Alex Young 6:28cdd6ee6b9d 60 {
Alex Young 6:28cdd6ee6b9d 61 case XDI_PacketCounter:
Alex Young 6:28cdd6ee6b9d 62 readShort(item, raw);
Alex Young 6:28cdd6ee6b9d 63 break;
Alex Young 6:28cdd6ee6b9d 64
Alex Young 6:28cdd6ee6b9d 65 case XDI_SampleTimeFine:
Alex Young 6:28cdd6ee6b9d 66 case XDI_StatusWord:
Alex Young 6:28cdd6ee6b9d 67 readInt(item, raw);
Alex Young 6:28cdd6ee6b9d 68 break;
Alex Young 6:28cdd6ee6b9d 69
Alex Young 6:28cdd6ee6b9d 70 case XDI_Quaternion:
Alex Young 6:28cdd6ee6b9d 71 case XDI_DeltaQ:
Alex Young 6:28cdd6ee6b9d 72 readFloats(item, raw, 4);
Alex Young 6:28cdd6ee6b9d 73 break;
Alex Young 6:28cdd6ee6b9d 74
Alex Young 6:28cdd6ee6b9d 75 case XDI_DeltaV:
Alex Young 6:28cdd6ee6b9d 76 case XDI_Acceleration:
Alex Young 6:28cdd6ee6b9d 77 case XDI_RateOfTurn:
Alex Young 6:28cdd6ee6b9d 78 case XDI_MagneticField:
Alex Young 6:28cdd6ee6b9d 79 readFloats(item, raw, 3);
Alex Young 6:28cdd6ee6b9d 80 break;
Alex Young 6:28cdd6ee6b9d 81
Alex Young 6:28cdd6ee6b9d 82 default:
Alex Young 6:28cdd6ee6b9d 83 return false;
Alex Young 6:28cdd6ee6b9d 84 }
Alex Young 6:28cdd6ee6b9d 85 return true;
Alex Young 6:28cdd6ee6b9d 86 }
Alex Young 6:28cdd6ee6b9d 87 else
Alex Young 6:28cdd6ee6b9d 88 {
Alex Young 6:28cdd6ee6b9d 89 return false;
Alex Young 6:28cdd6ee6b9d 90 }
Alex Young 6:28cdd6ee6b9d 91 }
Alex Young 6:28cdd6ee6b9d 92