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

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:
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 }