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

Dependencies:   mbed-rtos mbed Xbus

Fork of MTi-1_example by Alex Young

Important Information

This example 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

Overview

The example program demonstrates connecting to an MTi-1 series device, restoring communications settings to default if necessary, and configuring the MTi to send data. For an MTi-1 the device is configured to send inertial sensor data, while MTi-2 and MTi-3 devices are configured to output orientation data using the onboard XKF3i filter.

Communication with the MTi-1 series device is implemented using a either a full-duplex UART, I2C or SPI bus. A reset line is used to reset the MTi during initialization. Data is output to a host PC terminal using a second UART.

For more information on the MTi-1 series communication protocol please refer to the datasheet: https://www.xsens.com/download/pdf/documentation/mti-1/mti-1-series_datasheet.pdf

Supported Platforms

The program has been tested on the following mbed platforms:

Using the Example

  1. To use the example program connect one of the supported mbed boards to the host PC and download the application from the mbed online compiler to the target device.
  2. With the mbed board unpowered (USB disconnected) wire the mbed board to the MTi-1 development board. The following connections are required:
    • In all cases:
      • 5V (or 3V3) main supply to VDD (P300-1)
      • MCU IO voltage (IORef) to VDDIO (P300-2)
      • GND to GND (P300-3)
      • MT_NRESET to nRST (P300-5)
    • For I2C communication:
      • MT_SCL to I2C_SCL (P300-9)
      • MT_SDA to I2C_SDA (P300-11)
      • MT_DRDY to DRDY (P300-15)
      • MT_ADD0 to ADD0 (P300-17)
      • MT_ADD1 to ADD1 (P300-19)
      • MT_ADD2 to ADD2 (P300-21)
    • For SPI communication:
      • MT_DRDY to DRDY (P300-15)
      • MT_SCLK to SPI_SCK (P300-17)
      • MT_MISO to SPI_MISO (P300-19)
      • MT_MOSI to SPI_MOSI (P300-21)
      • MT_nCS to SPI_nCS (P300-23)
    • For UART communication:
      • MT_RX to UART_TX (P300-9)
      • MT_TX to UART_RX (P300-11)

For more information on the MTi-1 development board please refer to the MTi-1 series user manual: https://www.xsens.com/download/pdf/documentation/mti-1/mti-1-series_dk_user_manual.pdf

Information

Check the defines at the top of main.cpp to determine which IO pins are used for the MT_xxx connections on each mbed platform.

Information

The active peripheral (I2C, SPI or UART) is selected on the MTi-1 development board through the PSEL0 and PSEL1 switches. Look on the bottom of the development board for the correct settings.

  1. Connect to the target using a serial terminal. The application is configured for:
    • Baudrate = 921600
    • Stop bits = 1
    • No parity bits
    • No flow control
  2. Reset the mbed board.
  3. You should be presented with a simple user interface as shown below:
MTi-1 series embedded example firmware.
Device ready for operation.
Found device with ID: 03880011.
Device is an MTi-3: Attitude Heading Reference System.
Output configuration set to:
        Packet counter: 65535 Hz
        Sample time fine: 65535 Hz
        Quaternion: 100 Hz
        Status word: 65535 Hz

Press 'm' to start measuring and 'c' to return to config mode.
Committer:
Alex Young
Date:
Thu May 21 12:19:50 2015 +0200
Revision:
34:3d7a6519a256
Parent:
32:fafe0f42d82b
Child:
35:7e519b88c610
Split out sending message from a full transaction.

The wakeup ack message will not have a response from the MT.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Alex Young 4:98f063b2e6da 1 #include "mbed.h"
Alex Young 25:01356fb59467 2 #include "rtos.h"
Alex Young 4:98f063b2e6da 3 #include "xbusparser.h"
Alex Young 11:8593ba137917 4 #include "xbusmessage.h"
Alex Young 4:98f063b2e6da 5
Alex Young 25:01356fb59467 6 #define MEMORY_POOL_SIZE (4)
Alex Young 26:665d3624f9ab 7 #define RESPONSE_QUEUE_SIZE (1)
Alex Young 25:01356fb59467 8 #define MAX_XBUS_DATA_SIZE (128)
Alex Young 25:01356fb59467 9
Alex Young 4:98f063b2e6da 10 static Serial pc(PA_2, PA_3);
Alex Young 4:98f063b2e6da 11 static Serial mt(PB_9, PB_8);
Alex Young 4:98f063b2e6da 12 static XbusParser* xbusParser;
Alex Young 25:01356fb59467 13
Alex Young 25:01356fb59467 14 MemoryPool<XbusMessage, MEMORY_POOL_SIZE> g_messagePool;
Alex Young 25:01356fb59467 15 MemoryPool<uint8_t[MAX_XBUS_DATA_SIZE], MEMORY_POOL_SIZE> g_messageDataPool;
Alex Young 26:665d3624f9ab 16 Queue<XbusMessage, RESPONSE_QUEUE_SIZE> g_responseQueue;
Alex Young 4:98f063b2e6da 17
Alex Young 25:01356fb59467 18 static void* allocateMessageData(size_t bufSize)
Alex Young 4:98f063b2e6da 19 {
Alex Young 25:01356fb59467 20 return bufSize < MAX_XBUS_DATA_SIZE ? g_messageDataPool.alloc() : NULL;
Alex Young 25:01356fb59467 21 }
Alex Young 25:01356fb59467 22
Alex Young 25:01356fb59467 23 static void deallocateMessageData(void const* buffer)
Alex Young 25:01356fb59467 24 {
Alex Young 25:01356fb59467 25 g_messageDataPool.free((uint8_t(*)[MAX_XBUS_DATA_SIZE])buffer);
Alex Young 4:98f063b2e6da 26 }
Alex Young 4:98f063b2e6da 27
Alex Young 4:98f063b2e6da 28 static void mtLowLevelHandler(void)
Alex Young 4:98f063b2e6da 29 {
Alex Young 4:98f063b2e6da 30 while (mt.readable())
Alex Young 4:98f063b2e6da 31 {
Alex Young 4:98f063b2e6da 32 XbusParser_parseByte(xbusParser, mt.getc());
Alex Young 4:98f063b2e6da 33 }
Alex Young 4:98f063b2e6da 34 }
Alex Young 4:98f063b2e6da 35
Alex Young 34:3d7a6519a256 36 static void sendMessage(XbusMessage const* m)
Alex Young 11:8593ba137917 37 {
Alex Young 26:665d3624f9ab 38 uint8_t buf[64];
Alex Young 26:665d3624f9ab 39 size_t rawLength = XbusMessage_format(buf, m);
Alex Young 11:8593ba137917 40 for (size_t i = 0; i < rawLength; ++i)
Alex Young 11:8593ba137917 41 {
Alex Young 11:8593ba137917 42 mt.putc(buf[i]);
Alex Young 11:8593ba137917 43 }
Alex Young 34:3d7a6519a256 44 }
Alex Young 34:3d7a6519a256 45
Alex Young 34:3d7a6519a256 46 static XbusMessage const* doTransaction(XbusMessage const* m)
Alex Young 34:3d7a6519a256 47 {
Alex Young 34:3d7a6519a256 48 sendMessage(m);
Alex Young 26:665d3624f9ab 49
Alex Young 26:665d3624f9ab 50 osEvent ev = g_responseQueue.get(500);
Alex Young 26:665d3624f9ab 51 return ev.status == osEventMessage ? (XbusMessage*)ev.value.p : NULL;
Alex Young 26:665d3624f9ab 52 }
Alex Young 26:665d3624f9ab 53
Alex Young 31:ce1ea9ae861e 54 /*!
Alex Young 31:ce1ea9ae861e 55 * \brief RAII object to manage message memory deallocation.
Alex Young 31:ce1ea9ae861e 56 *
Alex Young 31:ce1ea9ae861e 57 * Will automatically free the memory used by a XbusMessage when going out
Alex Young 31:ce1ea9ae861e 58 * of scope.
Alex Young 31:ce1ea9ae861e 59 */
Alex Young 31:ce1ea9ae861e 60 class XbusMessageMemoryManager
Alex Young 26:665d3624f9ab 61 {
Alex Young 31:ce1ea9ae861e 62 public:
Alex Young 31:ce1ea9ae861e 63 XbusMessageMemoryManager(XbusMessage const* message)
Alex Young 31:ce1ea9ae861e 64 : m_message(message)
Alex Young 31:ce1ea9ae861e 65 {
Alex Young 31:ce1ea9ae861e 66 }
Alex Young 31:ce1ea9ae861e 67
Alex Young 31:ce1ea9ae861e 68 ~XbusMessageMemoryManager()
Alex Young 31:ce1ea9ae861e 69 {
Alex Young 31:ce1ea9ae861e 70 if (m_message)
Alex Young 31:ce1ea9ae861e 71 {
Alex Young 31:ce1ea9ae861e 72 if (m_message->data)
Alex Young 31:ce1ea9ae861e 73 deallocateMessageData(m_message->data);
Alex Young 31:ce1ea9ae861e 74 g_messagePool.free(const_cast<XbusMessage*>(m_message));
Alex Young 31:ce1ea9ae861e 75 }
Alex Young 31:ce1ea9ae861e 76 }
Alex Young 31:ce1ea9ae861e 77
Alex Young 31:ce1ea9ae861e 78 private:
Alex Young 31:ce1ea9ae861e 79 XbusMessage const* m_message;
Alex Young 31:ce1ea9ae861e 80 };
Alex Young 26:665d3624f9ab 81
Alex Young 29:d9310e7b58b5 82 static void dumpResponse(XbusMessage const* response)
Alex Young 29:d9310e7b58b5 83 {
Alex Young 29:d9310e7b58b5 84 switch (response->mid)
Alex Young 29:d9310e7b58b5 85 {
Alex Young 29:d9310e7b58b5 86 case XMID_GotoConfigAck:
Alex Young 29:d9310e7b58b5 87 pc.printf("Device went to config mode\n");
Alex Young 29:d9310e7b58b5 88 break;
Alex Young 29:d9310e7b58b5 89
Alex Young 29:d9310e7b58b5 90 case XMID_DeviceId:
Alex Young 29:d9310e7b58b5 91 pc.printf("Device ID: %08X\n", *(uint32_t*)response->data);
Alex Young 29:d9310e7b58b5 92 break;
Alex Young 29:d9310e7b58b5 93
Alex Young 29:d9310e7b58b5 94 case XMID_OutputConfig:
Alex Young 29:d9310e7b58b5 95 {
Alex Young 29:d9310e7b58b5 96 pc.printf("Output configuration\n");
Alex Young 29:d9310e7b58b5 97 OutputConfiguration* conf = (OutputConfiguration*)response->data;
Alex Young 29:d9310e7b58b5 98 for (int i = 0; i < response->length; ++i)
Alex Young 29:d9310e7b58b5 99 {
Alex Young 29:d9310e7b58b5 100 pc.printf("\t%s: %d Hz\n", XbusMessage_dataDescription(conf->dtype), conf->freq);
Alex Young 29:d9310e7b58b5 101 ++conf;
Alex Young 29:d9310e7b58b5 102 }
Alex Young 29:d9310e7b58b5 103 }
Alex Young 29:d9310e7b58b5 104 break;
Alex Young 29:d9310e7b58b5 105
Alex Young 29:d9310e7b58b5 106 case XMID_Error:
Alex Young 29:d9310e7b58b5 107 pc.printf("Device error!");
Alex Young 29:d9310e7b58b5 108 break;
Alex Young 29:d9310e7b58b5 109
Alex Young 29:d9310e7b58b5 110 default:
Alex Young 29:d9310e7b58b5 111 pc.printf("Received response MID=%X, length=%d\n", response->mid, response->length);
Alex Young 29:d9310e7b58b5 112 break;
Alex Young 29:d9310e7b58b5 113 }
Alex Young 29:d9310e7b58b5 114 }
Alex Young 29:d9310e7b58b5 115
Alex Young 26:665d3624f9ab 116 static void sendCommand(XsMessageId cmdId)
Alex Young 26:665d3624f9ab 117 {
Alex Young 26:665d3624f9ab 118 XbusMessage m = {cmdId};
Alex Young 26:665d3624f9ab 119 XbusMessage const* response = doTransaction(&m);
Alex Young 31:ce1ea9ae861e 120 XbusMessageMemoryManager janitor(response);
Alex Young 26:665d3624f9ab 121
Alex Young 26:665d3624f9ab 122 if (response)
Alex Young 26:665d3624f9ab 123 {
Alex Young 29:d9310e7b58b5 124 dumpResponse(response);
Alex Young 26:665d3624f9ab 125 }
Alex Young 26:665d3624f9ab 126 else
Alex Young 26:665d3624f9ab 127 {
Alex Young 26:665d3624f9ab 128 pc.printf("Timeout waiting for response.\n");
Alex Young 26:665d3624f9ab 129 }
Alex Young 11:8593ba137917 130 }
Alex Young 11:8593ba137917 131
Alex Young 11:8593ba137917 132 static void handlePcCommand(char cmd)
Alex Young 11:8593ba137917 133 {
Alex Young 11:8593ba137917 134 switch (cmd)
Alex Young 11:8593ba137917 135 {
Alex Young 11:8593ba137917 136 case 'c':
Alex Young 11:8593ba137917 137 sendCommand(XMID_GotoConfig);
Alex Young 11:8593ba137917 138 break;
Alex Young 11:8593ba137917 139
Alex Young 11:8593ba137917 140 case 'm':
Alex Young 11:8593ba137917 141 sendCommand(XMID_GotoMeasurement);
Alex Young 11:8593ba137917 142 break;
Alex Young 20:38560fa3d2eb 143
Alex Young 20:38560fa3d2eb 144 case 'd':
Alex Young 20:38560fa3d2eb 145 sendCommand(XMID_ReqDid);
Alex Young 20:38560fa3d2eb 146 break;
Alex Young 22:3eab999c5076 147
Alex Young 22:3eab999c5076 148 case 'o':
Alex Young 22:3eab999c5076 149 sendCommand(XMID_ReqOutputConfig);
Alex Young 22:3eab999c5076 150 break;
Alex Young 11:8593ba137917 151 }
Alex Young 11:8593ba137917 152 }
Alex Young 11:8593ba137917 153
Alex Young 24:2cc49dc854e3 154 static void handleDataMessage(struct XbusMessage const* message)
Alex Young 24:2cc49dc854e3 155 {
Alex Young 24:2cc49dc854e3 156 pc.printf("MTData2:");
Alex Young 24:2cc49dc854e3 157 uint16_t counter;
Alex Young 24:2cc49dc854e3 158 if (XbusMessage_getDataItem(&counter, XDI_PacketCounter, message))
Alex Young 24:2cc49dc854e3 159 {
Alex Young 24:2cc49dc854e3 160 pc.printf(" Packet counter: %5d", counter);
Alex Young 24:2cc49dc854e3 161 }
Alex Young 24:2cc49dc854e3 162 float ori[4];
Alex Young 24:2cc49dc854e3 163 if (XbusMessage_getDataItem(ori, XDI_Quaternion, message))
Alex Young 24:2cc49dc854e3 164 {
Alex Young 24:2cc49dc854e3 165 pc.printf(" Orientation: (% .3f, % .3f, % .3f, % .3f)", ori[0], ori[1],
Alex Young 24:2cc49dc854e3 166 ori[2], ori[3]);
Alex Young 24:2cc49dc854e3 167 }
Alex Young 24:2cc49dc854e3 168 uint32_t status;
Alex Young 24:2cc49dc854e3 169 if (XbusMessage_getDataItem(&status, XDI_StatusWord, message))
Alex Young 24:2cc49dc854e3 170 {
Alex Young 24:2cc49dc854e3 171 pc.printf(" Status:%X", status);
Alex Young 24:2cc49dc854e3 172 }
Alex Young 24:2cc49dc854e3 173 pc.printf("\n");
Alex Young 26:665d3624f9ab 174 deallocateMessageData(message->data);
Alex Young 24:2cc49dc854e3 175 }
Alex Young 24:2cc49dc854e3 176
Alex Young 24:2cc49dc854e3 177 static void mtMessageHandler(struct XbusMessage const* message)
Alex Young 4:98f063b2e6da 178 {
Alex Young 15:558d279addd9 179 if (message->mid == XMID_MtData2)
Alex Young 7:c913a7cd5231 180 {
Alex Young 24:2cc49dc854e3 181 handleDataMessage(message);
Alex Young 7:c913a7cd5231 182 }
Alex Young 7:c913a7cd5231 183 else
Alex Young 7:c913a7cd5231 184 {
Alex Young 26:665d3624f9ab 185 XbusMessage* m = g_messagePool.alloc();
Alex Young 26:665d3624f9ab 186 memcpy(m, message, sizeof(XbusMessage));
Alex Young 26:665d3624f9ab 187 g_responseQueue.put(m);
Alex Young 25:01356fb59467 188 }
Alex Young 4:98f063b2e6da 189 }
Alex Young 4:98f063b2e6da 190
Alex Young 4:98f063b2e6da 191 static void configureSerialPorts(void)
Alex Young 4:98f063b2e6da 192 {
Alex Young 4:98f063b2e6da 193 pc.baud(921600);
Alex Young 4:98f063b2e6da 194 pc.format(8, Serial::None, 2);
Alex Young 4:98f063b2e6da 195
Alex Young 4:98f063b2e6da 196 mt.baud(921600);
Alex Young 4:98f063b2e6da 197 mt.format(8, Serial::None, 2);
Alex Young 4:98f063b2e6da 198 mt.attach(mtLowLevelHandler, Serial::RxIrq);
Alex Young 4:98f063b2e6da 199 }
Alex Young 4:98f063b2e6da 200
Alex Young 29:d9310e7b58b5 201 static uint32_t readDeviceId(void)
Alex Young 29:d9310e7b58b5 202 {
Alex Young 29:d9310e7b58b5 203 XbusMessage reqDid = {XMID_ReqDid};
Alex Young 29:d9310e7b58b5 204 XbusMessage const* didRsp = doTransaction(&reqDid);
Alex Young 31:ce1ea9ae861e 205 XbusMessageMemoryManager janitor(didRsp);
Alex Young 29:d9310e7b58b5 206 uint32_t deviceId = 0;
Alex Young 29:d9310e7b58b5 207 if (didRsp)
Alex Young 29:d9310e7b58b5 208 {
Alex Young 29:d9310e7b58b5 209 if (didRsp->mid == XMID_DeviceId)
Alex Young 29:d9310e7b58b5 210 {
Alex Young 29:d9310e7b58b5 211 deviceId = *(uint32_t*)didRsp->data;
Alex Young 29:d9310e7b58b5 212 }
Alex Young 29:d9310e7b58b5 213 }
Alex Young 29:d9310e7b58b5 214 return deviceId;
Alex Young 29:d9310e7b58b5 215 }
Alex Young 29:d9310e7b58b5 216
Alex Young 32:fafe0f42d82b 217 static bool setOutputConfiguration(OutputConfiguration const* conf, uint8_t elements)
Alex Young 29:d9310e7b58b5 218 {
Alex Young 32:fafe0f42d82b 219 XbusMessage outputConfMsg = {XMID_SetOutputConfig, elements, (void*)conf};
Alex Young 32:fafe0f42d82b 220 XbusMessage const* outputConfRsp = doTransaction(&outputConfMsg);
Alex Young 32:fafe0f42d82b 221 XbusMessageMemoryManager janitor(outputConfRsp);
Alex Young 32:fafe0f42d82b 222 if (outputConfRsp)
Alex Young 29:d9310e7b58b5 223 {
Alex Young 32:fafe0f42d82b 224 if (outputConfRsp->mid == XMID_OutputConfig)
Alex Young 29:d9310e7b58b5 225 {
Alex Young 32:fafe0f42d82b 226 pc.printf("Output configuration set to:\n");
Alex Young 32:fafe0f42d82b 227 OutputConfiguration* conf = (OutputConfiguration*)outputConfRsp->data;
Alex Young 32:fafe0f42d82b 228 for (int i = 0; i < outputConfRsp->length; ++i)
Alex Young 32:fafe0f42d82b 229 {
Alex Young 32:fafe0f42d82b 230 pc.printf("\t%s: %d Hz\n", XbusMessage_dataDescription(conf->dtype), conf->freq);
Alex Young 32:fafe0f42d82b 231 ++conf;
Alex Young 32:fafe0f42d82b 232 }
Alex Young 32:fafe0f42d82b 233 return true;
Alex Young 29:d9310e7b58b5 234 }
Alex Young 29:d9310e7b58b5 235 else
Alex Young 29:d9310e7b58b5 236 {
Alex Young 32:fafe0f42d82b 237 dumpResponse(outputConfRsp);
Alex Young 29:d9310e7b58b5 238 }
Alex Young 32:fafe0f42d82b 239 }
Alex Young 32:fafe0f42d82b 240 else
Alex Young 32:fafe0f42d82b 241 {
Alex Young 32:fafe0f42d82b 242 pc.printf("Failed to set output configuration.\n");
Alex Young 32:fafe0f42d82b 243 }
Alex Young 32:fafe0f42d82b 244 return false;
Alex Young 32:fafe0f42d82b 245 }
Alex Young 29:d9310e7b58b5 246
Alex Young 32:fafe0f42d82b 247 static bool configureMotionTracker(void)
Alex Young 32:fafe0f42d82b 248 {
Alex Young 32:fafe0f42d82b 249 uint32_t deviceId = readDeviceId();
Alex Young 32:fafe0f42d82b 250
Alex Young 32:fafe0f42d82b 251 if (deviceId)
Alex Young 32:fafe0f42d82b 252 {
Alex Young 32:fafe0f42d82b 253 uint8_t deviceType = (deviceId >> 24) & 0x0F;
Alex Young 32:fafe0f42d82b 254 pc.printf("Found MTi-%d\n", deviceType);
Alex Young 32:fafe0f42d82b 255
Alex Young 32:fafe0f42d82b 256 if (deviceType == 1)
Alex Young 29:d9310e7b58b5 257 {
Alex Young 32:fafe0f42d82b 258 OutputConfiguration conf[] = {
Alex Young 32:fafe0f42d82b 259 {XDI_PacketCounter, 65535},
Alex Young 32:fafe0f42d82b 260 {XDI_SampleTimeFine, 65535},
Alex Young 32:fafe0f42d82b 261 {XDI_Acceleration, 100},
Alex Young 32:fafe0f42d82b 262 {XDI_RateOfTurn, 100},
Alex Young 32:fafe0f42d82b 263 {XDI_MagneticField, 100}
Alex Young 32:fafe0f42d82b 264 };
Alex Young 32:fafe0f42d82b 265 return setOutputConfiguration(conf,
Alex Young 32:fafe0f42d82b 266 sizeof(conf) / sizeof(OutputConfiguration));
Alex Young 29:d9310e7b58b5 267 }
Alex Young 29:d9310e7b58b5 268 else
Alex Young 29:d9310e7b58b5 269 {
Alex Young 32:fafe0f42d82b 270 OutputConfiguration conf[] = {
Alex Young 32:fafe0f42d82b 271 {XDI_PacketCounter, 65535},
Alex Young 32:fafe0f42d82b 272 {XDI_SampleTimeFine, 65535},
Alex Young 32:fafe0f42d82b 273 {XDI_Quaternion, 100},
Alex Young 32:fafe0f42d82b 274 {XDI_StatusWord, 65535}
Alex Young 32:fafe0f42d82b 275 };
Alex Young 32:fafe0f42d82b 276 return setOutputConfiguration(conf,
Alex Young 32:fafe0f42d82b 277 sizeof(conf) / sizeof(OutputConfiguration));
Alex Young 29:d9310e7b58b5 278 }
Alex Young 29:d9310e7b58b5 279 }
Alex Young 32:fafe0f42d82b 280
Alex Young 32:fafe0f42d82b 281 return false;
Alex Young 29:d9310e7b58b5 282 }
Alex Young 29:d9310e7b58b5 283
Alex Young 2:b3e402dc11ca 284 int main(void)
Alex Young 2:b3e402dc11ca 285 {
Alex Young 4:98f063b2e6da 286 XbusParserCallback xbusCallback = {};
Alex Young 25:01356fb59467 287 xbusCallback.allocateBuffer = allocateMessageData;
Alex Young 25:01356fb59467 288 xbusCallback.deallocateBuffer = deallocateMessageData;
Alex Young 24:2cc49dc854e3 289 xbusCallback.handleMessage = mtMessageHandler;
Alex Young 4:98f063b2e6da 290
Alex Young 4:98f063b2e6da 291 xbusParser = XbusParser_create(&xbusCallback);
Alex Young 4:98f063b2e6da 292 configureSerialPorts();
Alex Young 29:d9310e7b58b5 293 if (configureMotionTracker())
Alex Young 5:abc52dd88be2 294 {
Alex Young 29:d9310e7b58b5 295 for (;;)
Alex Young 26:665d3624f9ab 296 {
Alex Young 29:d9310e7b58b5 297 while (pc.readable())
Alex Young 29:d9310e7b58b5 298 {
Alex Young 29:d9310e7b58b5 299 handlePcCommand(pc.getc());
Alex Young 29:d9310e7b58b5 300 }
Alex Young 26:665d3624f9ab 301 }
Alex Young 5:abc52dd88be2 302 }
Alex Young 29:d9310e7b58b5 303 else
Alex Young 29:d9310e7b58b5 304 {
Alex Young 29:d9310e7b58b5 305 pc.printf("Failed to configure motion tracker.\n");
Alex Young 29:d9310e7b58b5 306 return -1;
Alex Young 29:d9310e7b58b5 307 }
Alex Young 4:98f063b2e6da 308 }