Xsens / Xbus

Basic implementation of Xbus message parsing and generation for embedded processors. The code has no dependencies and should also work for other MCU architectures than ARM provided a C99 compiler is available.

Dependents:   MTi-1_example_LPC1768 MTi-1_rikbeun MTi-1_example MTi-1_example ... more

Important Information

This library is deprecated and no longer maintained. There are new embedded examples available in the MT SDK folder of the MT Software Suite. For more information please visit: https://xsenstechnologies.force.com/knowledgebase/s/article/Introduction-to-the-MT-SDK-programming-examples-for-MTi-devices

For an example of using the Xbus library to communicate with an MTi-1 series device using a full-duplex UART see:

Import programMTi-1_example

Example of using Xbus library to communicate with an MTi-1 series device using a full-duplex UART connection.

Last commit 03 May 2018 by Xsens

xbusmessage.c@0:eb25b1785ee4, 2015-06-16 (annotated)

Committer:
alexandery
Date:
Tue Jun 16 07:54:23 2015 +0000
Revision:
0:eb25b1785ee4
Child:
1:c24f69a2eff4
Make Xbus code into a library;

Who changed what in which revision?

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