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.

Committer:
tjerkhofmeijer
Date:
Fri Oct 02 16:22:33 2015 +0200
Revision:
1:c24f69a2eff4
Parent:
0:eb25b1785ee4
Xbus library is updated to support MTi 1-series' I2C and SPI interfaces

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 */
tjerkhofmeijer 1:c24f69a2eff4 76 size_t XbusMessage_format(uint8_t* raw, struct XbusMessage const* message, enum XbusLowLevelFormat format)
alexandery 0:eb25b1785ee4 77 {
alexandery 0:eb25b1785ee4 78 uint8_t* dptr = raw;
tjerkhofmeijer 1:c24f69a2eff4 79 switch (format)
tjerkhofmeijer 1:c24f69a2eff4 80 {
tjerkhofmeijer 1:c24f69a2eff4 81 case XLLF_I2c:
tjerkhofmeijer 1:c24f69a2eff4 82 {
tjerkhofmeijer 1:c24f69a2eff4 83 *dptr++ = XBUS_CONTROL_PIPE;
tjerkhofmeijer 1:c24f69a2eff4 84 }
tjerkhofmeijer 1:c24f69a2eff4 85 break;
alexandery 0:eb25b1785ee4 86
tjerkhofmeijer 1:c24f69a2eff4 87 case XLLF_Spi:
tjerkhofmeijer 1:c24f69a2eff4 88 {
tjerkhofmeijer 1:c24f69a2eff4 89 *dptr++ = XBUS_CONTROL_PIPE;
tjerkhofmeijer 1:c24f69a2eff4 90 // Fill bytes required to allow MT to process data
tjerkhofmeijer 1:c24f69a2eff4 91 *dptr++ = 0;
tjerkhofmeijer 1:c24f69a2eff4 92 *dptr++ = 0;
tjerkhofmeijer 1:c24f69a2eff4 93 *dptr++ = 0;
tjerkhofmeijer 1:c24f69a2eff4 94 }
tjerkhofmeijer 1:c24f69a2eff4 95 break;
tjerkhofmeijer 1:c24f69a2eff4 96
tjerkhofmeijer 1:c24f69a2eff4 97 case XLLF_Uart:
tjerkhofmeijer 1:c24f69a2eff4 98 {
tjerkhofmeijer 1:c24f69a2eff4 99 *dptr++ = XBUS_PREAMBLE;
tjerkhofmeijer 1:c24f69a2eff4 100 *dptr++ = XBUS_MASTERDEVICE;
tjerkhofmeijer 1:c24f69a2eff4 101 }
tjerkhofmeijer 1:c24f69a2eff4 102 break;
tjerkhofmeijer 1:c24f69a2eff4 103 }
tjerkhofmeijer 1:c24f69a2eff4 104
tjerkhofmeijer 1:c24f69a2eff4 105 uint8_t checksum = (uint8_t)(-XBUS_MASTERDEVICE);
alexandery 0:eb25b1785ee4 106
alexandery 0:eb25b1785ee4 107 *dptr = message->mid;
alexandery 0:eb25b1785ee4 108 checksum -= *dptr++;
alexandery 0:eb25b1785ee4 109
alexandery 0:eb25b1785ee4 110 uint16_t length = messageLength(message);
alexandery 0:eb25b1785ee4 111
alexandery 0:eb25b1785ee4 112 if (length < XBUS_EXTENDED_LENGTH)
alexandery 0:eb25b1785ee4 113 {
alexandery 0:eb25b1785ee4 114 *dptr = length;
alexandery 0:eb25b1785ee4 115 checksum -= *dptr++;
alexandery 0:eb25b1785ee4 116 }
alexandery 0:eb25b1785ee4 117 else
alexandery 0:eb25b1785ee4 118 {
alexandery 0:eb25b1785ee4 119 *dptr = XBUS_EXTENDED_LENGTH;
alexandery 0:eb25b1785ee4 120 checksum -= *dptr++;
alexandery 0:eb25b1785ee4 121 *dptr = length >> 8;
alexandery 0:eb25b1785ee4 122 checksum -= *dptr++;
alexandery 0:eb25b1785ee4 123 *dptr = length & 0xFF;
alexandery 0:eb25b1785ee4 124 checksum -= *dptr++;
alexandery 0:eb25b1785ee4 125 }
alexandery 0:eb25b1785ee4 126
alexandery 0:eb25b1785ee4 127 formatPayload(dptr, message);
alexandery 0:eb25b1785ee4 128 for (int i = 0; i < length; ++i)
alexandery 0:eb25b1785ee4 129 {
alexandery 0:eb25b1785ee4 130 checksum -= *dptr++;
alexandery 0:eb25b1785ee4 131 }
alexandery 0:eb25b1785ee4 132 *dptr++ = checksum;
alexandery 0:eb25b1785ee4 133
alexandery 0:eb25b1785ee4 134 return dptr - raw;
alexandery 0:eb25b1785ee4 135 }
alexandery 0:eb25b1785ee4 136
alexandery 0:eb25b1785ee4 137 /*!
alexandery 0:eb25b1785ee4 138 * \brief Get a pointer to the data corresponding to \a id.
alexandery 0:eb25b1785ee4 139 * \param id The data identifier to find in the message.
alexandery 0:eb25b1785ee4 140 * \param data Pointer to the raw message payload.
alexandery 0:eb25b1785ee4 141 * \param dataLength The length of the payload in bytes.
alexandery 0:eb25b1785ee4 142 * \returns Pointer to data item, or NULL if the identifier is not present in
alexandery 0:eb25b1785ee4 143 * the message.
alexandery 0:eb25b1785ee4 144 */
alexandery 0:eb25b1785ee4 145 static uint8_t const* getPointerToData(enum XsDataIdentifier id, uint8_t const* data, uint16_t dataLength)
alexandery 0:eb25b1785ee4 146 {
alexandery 0:eb25b1785ee4 147 uint8_t const* dptr = data;
alexandery 0:eb25b1785ee4 148 while (dptr < data + dataLength)
alexandery 0:eb25b1785ee4 149 {
alexandery 0:eb25b1785ee4 150 uint16_t itemId;
alexandery 0:eb25b1785ee4 151 uint8_t itemSize;
alexandery 0:eb25b1785ee4 152 dptr = XbusUtility_readU16(&itemId, dptr);
alexandery 0:eb25b1785ee4 153 dptr = XbusUtility_readU8(&itemSize, dptr);
alexandery 0:eb25b1785ee4 154
alexandery 0:eb25b1785ee4 155 if (id == itemId)
alexandery 0:eb25b1785ee4 156 return dptr;
alexandery 0:eb25b1785ee4 157
alexandery 0:eb25b1785ee4 158 dptr += itemSize;
alexandery 0:eb25b1785ee4 159 }
alexandery 0:eb25b1785ee4 160 return NULL;
alexandery 0:eb25b1785ee4 161 }
alexandery 0:eb25b1785ee4 162
alexandery 0:eb25b1785ee4 163 /*!
alexandery 0:eb25b1785ee4 164 * \brief Read a number of floats from a message payload.
alexandery 0:eb25b1785ee4 165 * \param out Pointer to where to output data.
alexandery 0:eb25b1785ee4 166 * \param raw Pointer to the start of the raw float data.
alexandery 0:eb25b1785ee4 167 * \param floats The number of floats to read.
alexandery 0:eb25b1785ee4 168 */
alexandery 0:eb25b1785ee4 169 static void readFloats(float* out, uint8_t const* raw, uint8_t floats)
alexandery 0:eb25b1785ee4 170 {
alexandery 0:eb25b1785ee4 171 for (int i = 0; i < floats; ++i)
alexandery 0:eb25b1785ee4 172 {
alexandery 0:eb25b1785ee4 173 raw = XbusUtility_readU32((uint32_t*)&out[i], raw);
alexandery 0:eb25b1785ee4 174 }
alexandery 0:eb25b1785ee4 175 }
alexandery 0:eb25b1785ee4 176
alexandery 0:eb25b1785ee4 177 /*!
alexandery 0:eb25b1785ee4 178 * \brief Get a data item from an XMID_MtData2 Xbus message.
alexandery 0:eb25b1785ee4 179 * \param item Pointer to where to store the data.
alexandery 0:eb25b1785ee4 180 * \param id The data identifier to get.
alexandery 0:eb25b1785ee4 181 * \param message The message to read the data item from.
alexandery 0:eb25b1785ee4 182 * \returns true if the data item is found in the message, else false.
alexandery 0:eb25b1785ee4 183 */
alexandery 0:eb25b1785ee4 184 bool XbusMessage_getDataItem(void* item, enum XsDataIdentifier id, struct XbusMessage const* message)
alexandery 0:eb25b1785ee4 185 {
alexandery 0:eb25b1785ee4 186 uint8_t const* raw = getPointerToData(id, message->data, message->length);
alexandery 0:eb25b1785ee4 187 if (raw)
alexandery 0:eb25b1785ee4 188 {
alexandery 0:eb25b1785ee4 189 switch (id)
alexandery 0:eb25b1785ee4 190 {
alexandery 0:eb25b1785ee4 191 case XDI_PacketCounter:
alexandery 0:eb25b1785ee4 192 raw = XbusUtility_readU16(item, raw);
alexandery 0:eb25b1785ee4 193 break;
alexandery 0:eb25b1785ee4 194
alexandery 0:eb25b1785ee4 195 case XDI_SampleTimeFine:
alexandery 0:eb25b1785ee4 196 case XDI_StatusWord:
alexandery 0:eb25b1785ee4 197 raw = XbusUtility_readU32(item, raw);
alexandery 0:eb25b1785ee4 198 break;
alexandery 0:eb25b1785ee4 199
alexandery 0:eb25b1785ee4 200 case XDI_Quaternion:
alexandery 0:eb25b1785ee4 201 case XDI_DeltaQ:
alexandery 0:eb25b1785ee4 202 readFloats(item, raw, 4);
alexandery 0:eb25b1785ee4 203 break;
alexandery 0:eb25b1785ee4 204
alexandery 0:eb25b1785ee4 205 case XDI_DeltaV:
alexandery 0:eb25b1785ee4 206 case XDI_Acceleration:
alexandery 0:eb25b1785ee4 207 case XDI_RateOfTurn:
alexandery 0:eb25b1785ee4 208 case XDI_MagneticField:
alexandery 0:eb25b1785ee4 209 readFloats(item, raw, 3);
alexandery 0:eb25b1785ee4 210 break;
alexandery 0:eb25b1785ee4 211
alexandery 0:eb25b1785ee4 212 default:
alexandery 0:eb25b1785ee4 213 return false;
alexandery 0:eb25b1785ee4 214 }
alexandery 0:eb25b1785ee4 215 return true;
alexandery 0:eb25b1785ee4 216 }
alexandery 0:eb25b1785ee4 217 else
alexandery 0:eb25b1785ee4 218 {
alexandery 0:eb25b1785ee4 219 return false;
alexandery 0:eb25b1785ee4 220 }
alexandery 0:eb25b1785ee4 221 }
alexandery 0:eb25b1785ee4 222
alexandery 0:eb25b1785ee4 223 /*!
alexandery 0:eb25b1785ee4 224 * \brief Get a string description for the passed data identifier.
alexandery 0:eb25b1785ee4 225 */
alexandery 0:eb25b1785ee4 226 char const* XbusMessage_dataDescription(enum XsDataIdentifier id)
alexandery 0:eb25b1785ee4 227 {
alexandery 0:eb25b1785ee4 228 switch (id)
alexandery 0:eb25b1785ee4 229 {
alexandery 0:eb25b1785ee4 230 case XDI_PacketCounter:
alexandery 0:eb25b1785ee4 231 return "Packet counter";
alexandery 0:eb25b1785ee4 232
alexandery 0:eb25b1785ee4 233 case XDI_SampleTimeFine:
alexandery 0:eb25b1785ee4 234 return "Sample time fine";
alexandery 0:eb25b1785ee4 235
alexandery 0:eb25b1785ee4 236 case XDI_Quaternion:
alexandery 0:eb25b1785ee4 237 return "Quaternion";
alexandery 0:eb25b1785ee4 238
alexandery 0:eb25b1785ee4 239 case XDI_DeltaV:
alexandery 0:eb25b1785ee4 240 return "Velocity increment";
alexandery 0:eb25b1785ee4 241
alexandery 0:eb25b1785ee4 242 case XDI_Acceleration:
alexandery 0:eb25b1785ee4 243 return "Acceleration";
alexandery 0:eb25b1785ee4 244
alexandery 0:eb25b1785ee4 245 case XDI_RateOfTurn:
alexandery 0:eb25b1785ee4 246 return "Rate of turn";
alexandery 0:eb25b1785ee4 247
alexandery 0:eb25b1785ee4 248 case XDI_DeltaQ:
alexandery 0:eb25b1785ee4 249 return "Orientation increment";
alexandery 0:eb25b1785ee4 250
alexandery 0:eb25b1785ee4 251 case XDI_MagneticField:
alexandery 0:eb25b1785ee4 252 return "Magnetic field";
alexandery 0:eb25b1785ee4 253
alexandery 0:eb25b1785ee4 254 case XDI_StatusWord:
alexandery 0:eb25b1785ee4 255 return "Status word";
alexandery 0:eb25b1785ee4 256
alexandery 0:eb25b1785ee4 257 default:
alexandery 0:eb25b1785ee4 258 return "Unknown data type";
alexandery 0:eb25b1785ee4 259 }
alexandery 0:eb25b1785ee4 260 }