V N / xbus

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.

xbusmessage.c@2:89c569689f22, 2022-02-16 (annotated)

Committer:
mbed01_vn
Date:
Wed Feb 16 09:54:20 2022 +0000
Revision:
2:89c569689f22
Parent:
1:c24f69a2eff4 
t

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 }