Xsens / Mbed 2 deprecated MTi-1_example

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 Alex Young

xbus/xbusmessage.c@48:69d35430fe8b, 2015-05-21 (annotated)

Committer:
Alex Young
Date:
Thu May 21 16:45:07 2015 +0200
Revision:
48:69d35430fe8b
Parent:
28:ae74baf7e5ab
Child:
49:38ecfbff5391
Add documentation for xbusmessage

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Alex Young 18:2073072bad51 1 /*!
Alex Young 18:2073072bad51 2 * \file
Alex Young 18:2073072bad51 3 * \copyright
Alex Young 18:2073072bad51 4 * Copyright (C) Xsens Technologies B.V., 2015. All rights reserved.
Alex Young 18:2073072bad51 5 *
Alex Young 18:2073072bad51 6 * This source code is intended for use only by Xsens Technologies BV and
Alex Young 18:2073072bad51 7 * those that have explicit written permission to use it from
Alex Young 18:2073072bad51 8 * Xsens Technologies BV.
Alex Young 18:2073072bad51 9 *
Alex Young 18:2073072bad51 10 * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
Alex Young 18:2073072bad51 11 * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
Alex Young 18:2073072bad51 12 * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
Alex Young 18:2073072bad51 13 * PARTICULAR PURPOSE.
Alex Young 18:2073072bad51 14 */
Alex Young 18:2073072bad51 15
Alex Young 18:2073072bad51 16 #include "xbusmessage.h"
Alex Young 18:2073072bad51 17 #include "xbusdef.h"
Alex Young 18:2073072bad51 18 #include "xbusutility.h"
Alex Young 18:2073072bad51 19
Alex Young 48:69d35430fe8b 20 /*!
Alex Young 48:69d35430fe8b 21 * \brief Calculate the number of bytes needed for \a message payload.
Alex Young 48:69d35430fe8b 22 */
Alex Young 28:ae74baf7e5ab 23 static uint16_t messageLength(struct XbusMessage const* message)
Alex Young 28:ae74baf7e5ab 24 {
Alex Young 28:ae74baf7e5ab 25 switch (message->mid)
Alex Young 28:ae74baf7e5ab 26 {
Alex Young 28:ae74baf7e5ab 27 case XMID_SetOutputConfig:
Alex Young 28:ae74baf7e5ab 28 return message->length * 2 * sizeof(uint16_t);
Alex Young 28:ae74baf7e5ab 29
Alex Young 28:ae74baf7e5ab 30 default:
Alex Young 28:ae74baf7e5ab 31 return message->length;
Alex Young 28:ae74baf7e5ab 32 }
Alex Young 28:ae74baf7e5ab 33 }
Alex Young 28:ae74baf7e5ab 34
Alex Young 48:69d35430fe8b 35 /*!
Alex Young 48:69d35430fe8b 36 * \brief Format a message with a pointer to an array of OutputConfiguration elements.
Alex Young 48:69d35430fe8b 37 */
Alex Young 28:ae74baf7e5ab 38 static void formatOutputConfig(uint8_t* raw, struct XbusMessage const* message)
Alex Young 28:ae74baf7e5ab 39 {
Alex Young 28:ae74baf7e5ab 40 struct OutputConfiguration* conf = message->data;
Alex Young 28:ae74baf7e5ab 41 for (int i = 0; i < message->length; ++i)
Alex Young 28:ae74baf7e5ab 42 {
Alex Young 28:ae74baf7e5ab 43 raw = XbusUtility_writeU16(raw, conf->dtype);
Alex Young 28:ae74baf7e5ab 44 raw = XbusUtility_writeU16(raw, conf->freq);
Alex Young 28:ae74baf7e5ab 45 ++conf;
Alex Young 28:ae74baf7e5ab 46 }
Alex Young 28:ae74baf7e5ab 47 }
Alex Young 28:ae74baf7e5ab 48
Alex Young 48:69d35430fe8b 49 /*!
Alex Young 48:69d35430fe8b 50 * \brief Format the payload of a message from a native data type to
Alex Young 48:69d35430fe8b 51 * raw bytes.
Alex Young 48:69d35430fe8b 52 */
Alex Young 28:ae74baf7e5ab 53 static void formatPayload(uint8_t* raw, struct XbusMessage const* message)
Alex Young 28:ae74baf7e5ab 54 {
Alex Young 28:ae74baf7e5ab 55 switch (message->mid)
Alex Young 28:ae74baf7e5ab 56 {
Alex Young 28:ae74baf7e5ab 57 case XMID_SetOutputConfig:
Alex Young 28:ae74baf7e5ab 58 formatOutputConfig(raw, message);
Alex Young 28:ae74baf7e5ab 59 break;
Alex Young 28:ae74baf7e5ab 60
Alex Young 28:ae74baf7e5ab 61 default:
Alex Young 28:ae74baf7e5ab 62 for (int i = 0; i < message->length; ++i)
Alex Young 28:ae74baf7e5ab 63 {
Alex Young 28:ae74baf7e5ab 64 *raw++ = ((uint8_t*)message->data)[i];
Alex Young 28:ae74baf7e5ab 65 }
Alex Young 28:ae74baf7e5ab 66 break;
Alex Young 28:ae74baf7e5ab 67 }
Alex Young 28:ae74baf7e5ab 68 }
Alex Young 28:ae74baf7e5ab 69
Alex Young 48:69d35430fe8b 70 /*!
Alex Young 48:69d35430fe8b 71 * \brief Format a message into the raw Xbus format ready for transmission to
Alex Young 48:69d35430fe8b 72 * a motion tracker.
Alex Young 48:69d35430fe8b 73 */
Alex Young 18:2073072bad51 74 size_t XbusMessage_format(uint8_t* raw, struct XbusMessage const* message)
Alex Young 18:2073072bad51 75 {
Alex Young 18:2073072bad51 76 uint8_t* dptr = raw;
Alex Young 18:2073072bad51 77 *dptr++ = XBUS_PREAMBLE;
Alex Young 18:2073072bad51 78
Alex Young 18:2073072bad51 79 *dptr = XBUS_BUS_ID_STANDALONE;
Alex Young 18:2073072bad51 80 uint8_t checksum = 0;
Alex Young 18:2073072bad51 81 checksum -= *dptr++;
Alex Young 18:2073072bad51 82
Alex Young 18:2073072bad51 83 *dptr = message->mid;
Alex Young 18:2073072bad51 84 checksum -= *dptr++;
Alex Young 18:2073072bad51 85
Alex Young 28:ae74baf7e5ab 86 uint16_t length = messageLength(message);
Alex Young 28:ae74baf7e5ab 87
Alex Young 28:ae74baf7e5ab 88 if (length < XBUS_EXTENDED_LENGTH)
Alex Young 18:2073072bad51 89 {
Alex Young 28:ae74baf7e5ab 90 *dptr = length;
Alex Young 18:2073072bad51 91 checksum -= *dptr++;
Alex Young 18:2073072bad51 92 }
Alex Young 18:2073072bad51 93 else
Alex Young 18:2073072bad51 94 {
Alex Young 18:2073072bad51 95 *dptr = XBUS_EXTENDED_LENGTH;
Alex Young 18:2073072bad51 96 checksum -= *dptr++;
Alex Young 28:ae74baf7e5ab 97 *dptr = length >> 8;
Alex Young 18:2073072bad51 98 checksum -= *dptr++;
Alex Young 28:ae74baf7e5ab 99 *dptr = length & 0xFF;
Alex Young 18:2073072bad51 100 checksum -= *dptr++;
Alex Young 18:2073072bad51 101 }
Alex Young 18:2073072bad51 102
Alex Young 28:ae74baf7e5ab 103 formatPayload(dptr, message);
Alex Young 28:ae74baf7e5ab 104 for (int i = 0; i < length; ++i)
Alex Young 18:2073072bad51 105 {
Alex Young 18:2073072bad51 106 checksum -= *dptr++;
Alex Young 18:2073072bad51 107 }
Alex Young 18:2073072bad51 108 *dptr++ = checksum;
Alex Young 18:2073072bad51 109
Alex Young 18:2073072bad51 110 return dptr - raw;
Alex Young 18:2073072bad51 111 }
Alex Young 18:2073072bad51 112
Alex Young 48:69d35430fe8b 113 /*!
Alex Young 48:69d35430fe8b 114 * \brief Get a pointer to the data correspondind to \a id.
Alex Young 48:69d35430fe8b 115 * \param id The data identifier to find in the message.
Alex Young 48:69d35430fe8b 116 * \param data Pointer to the raw message payload.
Alex Young 48:69d35430fe8b 117 * \param dataLength The length of the payload in bytes.
Alex Young 48:69d35430fe8b 118 * \returns Pointer to data item, or NULL if the identifier is not present in
Alex Young 48:69d35430fe8b 119 * the message.
Alex Young 48:69d35430fe8b 120 */
Alex Young 18:2073072bad51 121 static uint8_t const* getPointerToData(enum XsDataIdentifier id, uint8_t const* data, uint16_t dataLength)
Alex Young 18:2073072bad51 122 {
Alex Young 18:2073072bad51 123 uint8_t const* dptr = data;
Alex Young 18:2073072bad51 124 while (dptr < data + dataLength)
Alex Young 18:2073072bad51 125 {
Alex Young 18:2073072bad51 126 uint16_t itemId;
Alex Young 18:2073072bad51 127 uint8_t itemSize;
Alex Young 18:2073072bad51 128 dptr = XbusUtility_readU16(&itemId, dptr);
Alex Young 18:2073072bad51 129 dptr = XbusUtility_readU8(&itemSize, dptr);
Alex Young 18:2073072bad51 130
Alex Young 18:2073072bad51 131 if (id == itemId)
Alex Young 18:2073072bad51 132 return dptr;
Alex Young 18:2073072bad51 133
Alex Young 18:2073072bad51 134 dptr += itemSize;
Alex Young 18:2073072bad51 135 }
Alex Young 18:2073072bad51 136 return NULL;
Alex Young 18:2073072bad51 137 }
Alex Young 18:2073072bad51 138
Alex Young 48:69d35430fe8b 139 /*!
Alex Young 48:69d35430fe8b 140 * \brief Read a number of floats from a message payload.
Alex Young 48:69d35430fe8b 141 * \param out Pointer to where to output data.
Alex Young 48:69d35430fe8b 142 * \param raw Pointer to the start of the raw float data.
Alex Young 48:69d35430fe8b 143 * \param floats The number of floats to read.
Alex Young 48:69d35430fe8b 144 */
Alex Young 18:2073072bad51 145 static void readFloats(float* out, uint8_t const* raw, uint8_t floats)
Alex Young 18:2073072bad51 146 {
Alex Young 18:2073072bad51 147 for (int i = 0; i < floats; ++i)
Alex Young 18:2073072bad51 148 {
Alex Young 18:2073072bad51 149 raw = XbusUtility_readU32((uint32_t*)&out[i], raw);
Alex Young 18:2073072bad51 150 }
Alex Young 18:2073072bad51 151 }
Alex Young 18:2073072bad51 152
Alex Young 48:69d35430fe8b 153 /*!
Alex Young 48:69d35430fe8b 154 * \brief Get a data item from a XMID_MtData2 Xbus message.
Alex Young 48:69d35430fe8b 155 * \param item Pointer to where to store the data.
Alex Young 48:69d35430fe8b 156 * \param id The data identifier to get.
Alex Young 48:69d35430fe8b 157 * \param message The message to read the data item from.
Alex Young 48:69d35430fe8b 158 * \returns true if the data item is found in the message, else false.
Alex Young 48:69d35430fe8b 159 */
Alex Young 18:2073072bad51 160 bool XbusMessage_getDataItem(void* item, enum XsDataIdentifier id, struct XbusMessage const* message)
Alex Young 18:2073072bad51 161 {
Alex Young 18:2073072bad51 162 uint8_t const* raw = getPointerToData(id, message->data, message->length);
Alex Young 18:2073072bad51 163 if (raw)
Alex Young 18:2073072bad51 164 {
Alex Young 18:2073072bad51 165 switch (id)
Alex Young 18:2073072bad51 166 {
Alex Young 18:2073072bad51 167 case XDI_PacketCounter:
Alex Young 18:2073072bad51 168 raw = XbusUtility_readU16(item, raw);
Alex Young 18:2073072bad51 169 break;
Alex Young 18:2073072bad51 170
Alex Young 18:2073072bad51 171 case XDI_SampleTimeFine:
Alex Young 18:2073072bad51 172 case XDI_StatusWord:
Alex Young 18:2073072bad51 173 raw = XbusUtility_readU32(item, raw);
Alex Young 18:2073072bad51 174 break;
Alex Young 18:2073072bad51 175
Alex Young 18:2073072bad51 176 case XDI_Quaternion:
Alex Young 18:2073072bad51 177 case XDI_DeltaQ:
Alex Young 18:2073072bad51 178 readFloats(item, raw, 4);
Alex Young 18:2073072bad51 179 break;
Alex Young 18:2073072bad51 180
Alex Young 18:2073072bad51 181 case XDI_DeltaV:
Alex Young 18:2073072bad51 182 case XDI_Acceleration:
Alex Young 18:2073072bad51 183 case XDI_RateOfTurn:
Alex Young 18:2073072bad51 184 case XDI_MagneticField:
Alex Young 18:2073072bad51 185 readFloats(item, raw, 3);
Alex Young 18:2073072bad51 186 break;
Alex Young 18:2073072bad51 187
Alex Young 18:2073072bad51 188 default:
Alex Young 18:2073072bad51 189 return false;
Alex Young 18:2073072bad51 190 }
Alex Young 18:2073072bad51 191 return true;
Alex Young 18:2073072bad51 192 }
Alex Young 18:2073072bad51 193 else
Alex Young 18:2073072bad51 194 {
Alex Young 18:2073072bad51 195 return false;
Alex Young 18:2073072bad51 196 }
Alex Young 18:2073072bad51 197 }
Alex Young 18:2073072bad51 198
Alex Young 48:69d35430fe8b 199 /*!
Alex Young 48:69d35430fe8b 200 * \brief Get a string descriptions for the passed data identifier.
Alex Young 48:69d35430fe8b 201 */
Alex Young 21:6015b8be3a00 202 char const* XbusMessage_dataDescription(enum XsDataIdentifier id)
Alex Young 21:6015b8be3a00 203 {
Alex Young 21:6015b8be3a00 204 switch (id)
Alex Young 21:6015b8be3a00 205 {
Alex Young 21:6015b8be3a00 206 case XDI_PacketCounter:
Alex Young 21:6015b8be3a00 207 return "Packet counter";
Alex Young 21:6015b8be3a00 208
Alex Young 21:6015b8be3a00 209 case XDI_SampleTimeFine:
Alex Young 21:6015b8be3a00 210 return "Sample time fine";
Alex Young 21:6015b8be3a00 211
Alex Young 21:6015b8be3a00 212 case XDI_Quaternion:
Alex Young 21:6015b8be3a00 213 return "Quaternion";
Alex Young 21:6015b8be3a00 214
Alex Young 21:6015b8be3a00 215 case XDI_DeltaV:
Alex Young 21:6015b8be3a00 216 return "Velocity increment";
Alex Young 21:6015b8be3a00 217
Alex Young 21:6015b8be3a00 218 case XDI_Acceleration:
Alex Young 21:6015b8be3a00 219 return "Acceleration";
Alex Young 21:6015b8be3a00 220
Alex Young 21:6015b8be3a00 221 case XDI_RateOfTurn:
Alex Young 21:6015b8be3a00 222 return "Rate of turn";
Alex Young 21:6015b8be3a00 223
Alex Young 21:6015b8be3a00 224 case XDI_DeltaQ:
Alex Young 21:6015b8be3a00 225 return "Orientation increment";
Alex Young 21:6015b8be3a00 226
Alex Young 21:6015b8be3a00 227 case XDI_MagneticField:
Alex Young 21:6015b8be3a00 228 return "Magnetic field";
Alex Young 21:6015b8be3a00 229
Alex Young 21:6015b8be3a00 230 case XDI_StatusWord:
Alex Young 21:6015b8be3a00 231 return "Status word";
Alex Young 21:6015b8be3a00 232
Alex Young 21:6015b8be3a00 233 default:
Alex Young 21:6015b8be3a00 234 return "Unknown data type";
Alex Young 21:6015b8be3a00 235 }
Alex Young 21:6015b8be3a00 236 }