David Khosravi / Mbed 2 deprecated MTi-1_example_LPC1768

Add LPC1768

Dependencies:   mbed-rtos mbed Xbus

Fork of MTi-1_example by Xsens

main.cpp@32:fafe0f42d82b, 2015-05-21 (annotated)

Committer:
Alex Young
Date:
Thu May 21 11:47:29 2015 +0200
Revision:
32:fafe0f42d82b
Parent:
31:ce1ea9ae861e
Child:
34:3d7a6519a256
Extract function for setting output configuration.

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