FRDM-KL25Zand Xsens MTi-3
Dependencies: mbed mbed-rtos Xbus
main.cpp@35:7e519b88c610, 2015-05-21 (annotated)
- Committer:
- Alex Young
- Date:
- Thu May 21 12:20:39 2015 +0200
- Revision:
- 35:7e519b88c610
- Parent:
- 34:3d7a6519a256
- Child:
- 36:21198d933917
Reset MT on startup.
Use the reset line to hold the MT in reset until we are ready to start.
Then release the reset line and wait for the wakeup message.
Who changed what in which revision?
User | Revision | Line number | New 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 |
35:7e519b88c610 | 12 | /*! |
Alex Young |
35:7e519b88c610 | 13 | * \brief MT reset line. |
Alex Young |
35:7e519b88c610 | 14 | * |
Alex Young |
35:7e519b88c610 | 15 | * MT is held in reset on startup. |
Alex Young |
35:7e519b88c610 | 16 | */ |
Alex Young |
35:7e519b88c610 | 17 | static DigitalOut mtReset(PA_10, 0); |
Alex Young |
4:98f063b2e6da | 18 | static XbusParser* xbusParser; |
Alex Young |
25:01356fb59467 | 19 | |
Alex Young |
25:01356fb59467 | 20 | MemoryPool<XbusMessage, MEMORY_POOL_SIZE> g_messagePool; |
Alex Young |
25:01356fb59467 | 21 | MemoryPool<uint8_t[MAX_XBUS_DATA_SIZE], MEMORY_POOL_SIZE> g_messageDataPool; |
Alex Young |
26:665d3624f9ab | 22 | Queue<XbusMessage, RESPONSE_QUEUE_SIZE> g_responseQueue; |
Alex Young |
4:98f063b2e6da | 23 | |
Alex Young |
25:01356fb59467 | 24 | static void* allocateMessageData(size_t bufSize) |
Alex Young |
4:98f063b2e6da | 25 | { |
Alex Young |
25:01356fb59467 | 26 | return bufSize < MAX_XBUS_DATA_SIZE ? g_messageDataPool.alloc() : NULL; |
Alex Young |
25:01356fb59467 | 27 | } |
Alex Young |
25:01356fb59467 | 28 | |
Alex Young |
25:01356fb59467 | 29 | static void deallocateMessageData(void const* buffer) |
Alex Young |
25:01356fb59467 | 30 | { |
Alex Young |
25:01356fb59467 | 31 | g_messageDataPool.free((uint8_t(*)[MAX_XBUS_DATA_SIZE])buffer); |
Alex Young |
4:98f063b2e6da | 32 | } |
Alex Young |
4:98f063b2e6da | 33 | |
Alex Young |
4:98f063b2e6da | 34 | static void mtLowLevelHandler(void) |
Alex Young |
4:98f063b2e6da | 35 | { |
Alex Young |
4:98f063b2e6da | 36 | while (mt.readable()) |
Alex Young |
4:98f063b2e6da | 37 | { |
Alex Young |
4:98f063b2e6da | 38 | XbusParser_parseByte(xbusParser, mt.getc()); |
Alex Young |
4:98f063b2e6da | 39 | } |
Alex Young |
4:98f063b2e6da | 40 | } |
Alex Young |
4:98f063b2e6da | 41 | |
Alex Young |
34:3d7a6519a256 | 42 | static void sendMessage(XbusMessage const* m) |
Alex Young |
11:8593ba137917 | 43 | { |
Alex Young |
26:665d3624f9ab | 44 | uint8_t buf[64]; |
Alex Young |
26:665d3624f9ab | 45 | size_t rawLength = XbusMessage_format(buf, m); |
Alex Young |
11:8593ba137917 | 46 | for (size_t i = 0; i < rawLength; ++i) |
Alex Young |
11:8593ba137917 | 47 | { |
Alex Young |
11:8593ba137917 | 48 | mt.putc(buf[i]); |
Alex Young |
11:8593ba137917 | 49 | } |
Alex Young |
34:3d7a6519a256 | 50 | } |
Alex Young |
34:3d7a6519a256 | 51 | |
Alex Young |
34:3d7a6519a256 | 52 | static XbusMessage const* doTransaction(XbusMessage const* m) |
Alex Young |
34:3d7a6519a256 | 53 | { |
Alex Young |
34:3d7a6519a256 | 54 | sendMessage(m); |
Alex Young |
26:665d3624f9ab | 55 | |
Alex Young |
26:665d3624f9ab | 56 | osEvent ev = g_responseQueue.get(500); |
Alex Young |
26:665d3624f9ab | 57 | return ev.status == osEventMessage ? (XbusMessage*)ev.value.p : NULL; |
Alex Young |
26:665d3624f9ab | 58 | } |
Alex Young |
26:665d3624f9ab | 59 | |
Alex Young |
31:ce1ea9ae861e | 60 | /*! |
Alex Young |
31:ce1ea9ae861e | 61 | * \brief RAII object to manage message memory deallocation. |
Alex Young |
31:ce1ea9ae861e | 62 | * |
Alex Young |
31:ce1ea9ae861e | 63 | * Will automatically free the memory used by a XbusMessage when going out |
Alex Young |
31:ce1ea9ae861e | 64 | * of scope. |
Alex Young |
31:ce1ea9ae861e | 65 | */ |
Alex Young |
31:ce1ea9ae861e | 66 | class XbusMessageMemoryManager |
Alex Young |
26:665d3624f9ab | 67 | { |
Alex Young |
31:ce1ea9ae861e | 68 | public: |
Alex Young |
31:ce1ea9ae861e | 69 | XbusMessageMemoryManager(XbusMessage const* message) |
Alex Young |
31:ce1ea9ae861e | 70 | : m_message(message) |
Alex Young |
31:ce1ea9ae861e | 71 | { |
Alex Young |
31:ce1ea9ae861e | 72 | } |
Alex Young |
31:ce1ea9ae861e | 73 | |
Alex Young |
31:ce1ea9ae861e | 74 | ~XbusMessageMemoryManager() |
Alex Young |
31:ce1ea9ae861e | 75 | { |
Alex Young |
31:ce1ea9ae861e | 76 | if (m_message) |
Alex Young |
31:ce1ea9ae861e | 77 | { |
Alex Young |
31:ce1ea9ae861e | 78 | if (m_message->data) |
Alex Young |
31:ce1ea9ae861e | 79 | deallocateMessageData(m_message->data); |
Alex Young |
31:ce1ea9ae861e | 80 | g_messagePool.free(const_cast<XbusMessage*>(m_message)); |
Alex Young |
31:ce1ea9ae861e | 81 | } |
Alex Young |
31:ce1ea9ae861e | 82 | } |
Alex Young |
31:ce1ea9ae861e | 83 | |
Alex Young |
31:ce1ea9ae861e | 84 | private: |
Alex Young |
31:ce1ea9ae861e | 85 | XbusMessage const* m_message; |
Alex Young |
31:ce1ea9ae861e | 86 | }; |
Alex Young |
26:665d3624f9ab | 87 | |
Alex Young |
29:d9310e7b58b5 | 88 | static void dumpResponse(XbusMessage const* response) |
Alex Young |
29:d9310e7b58b5 | 89 | { |
Alex Young |
29:d9310e7b58b5 | 90 | switch (response->mid) |
Alex Young |
29:d9310e7b58b5 | 91 | { |
Alex Young |
29:d9310e7b58b5 | 92 | case XMID_GotoConfigAck: |
Alex Young |
29:d9310e7b58b5 | 93 | pc.printf("Device went to config mode\n"); |
Alex Young |
29:d9310e7b58b5 | 94 | break; |
Alex Young |
29:d9310e7b58b5 | 95 | |
Alex Young |
29:d9310e7b58b5 | 96 | case XMID_DeviceId: |
Alex Young |
29:d9310e7b58b5 | 97 | pc.printf("Device ID: %08X\n", *(uint32_t*)response->data); |
Alex Young |
29:d9310e7b58b5 | 98 | break; |
Alex Young |
29:d9310e7b58b5 | 99 | |
Alex Young |
29:d9310e7b58b5 | 100 | case XMID_OutputConfig: |
Alex Young |
29:d9310e7b58b5 | 101 | { |
Alex Young |
29:d9310e7b58b5 | 102 | pc.printf("Output configuration\n"); |
Alex Young |
29:d9310e7b58b5 | 103 | OutputConfiguration* conf = (OutputConfiguration*)response->data; |
Alex Young |
29:d9310e7b58b5 | 104 | for (int i = 0; i < response->length; ++i) |
Alex Young |
29:d9310e7b58b5 | 105 | { |
Alex Young |
29:d9310e7b58b5 | 106 | pc.printf("\t%s: %d Hz\n", XbusMessage_dataDescription(conf->dtype), conf->freq); |
Alex Young |
29:d9310e7b58b5 | 107 | ++conf; |
Alex Young |
29:d9310e7b58b5 | 108 | } |
Alex Young |
29:d9310e7b58b5 | 109 | } |
Alex Young |
29:d9310e7b58b5 | 110 | break; |
Alex Young |
29:d9310e7b58b5 | 111 | |
Alex Young |
29:d9310e7b58b5 | 112 | case XMID_Error: |
Alex Young |
29:d9310e7b58b5 | 113 | pc.printf("Device error!"); |
Alex Young |
29:d9310e7b58b5 | 114 | break; |
Alex Young |
29:d9310e7b58b5 | 115 | |
Alex Young |
29:d9310e7b58b5 | 116 | default: |
Alex Young |
29:d9310e7b58b5 | 117 | pc.printf("Received response MID=%X, length=%d\n", response->mid, response->length); |
Alex Young |
29:d9310e7b58b5 | 118 | break; |
Alex Young |
29:d9310e7b58b5 | 119 | } |
Alex Young |
29:d9310e7b58b5 | 120 | } |
Alex Young |
29:d9310e7b58b5 | 121 | |
Alex Young |
26:665d3624f9ab | 122 | static void sendCommand(XsMessageId cmdId) |
Alex Young |
26:665d3624f9ab | 123 | { |
Alex Young |
26:665d3624f9ab | 124 | XbusMessage m = {cmdId}; |
Alex Young |
26:665d3624f9ab | 125 | XbusMessage const* response = doTransaction(&m); |
Alex Young |
31:ce1ea9ae861e | 126 | XbusMessageMemoryManager janitor(response); |
Alex Young |
26:665d3624f9ab | 127 | |
Alex Young |
26:665d3624f9ab | 128 | if (response) |
Alex Young |
26:665d3624f9ab | 129 | { |
Alex Young |
29:d9310e7b58b5 | 130 | dumpResponse(response); |
Alex Young |
26:665d3624f9ab | 131 | } |
Alex Young |
26:665d3624f9ab | 132 | else |
Alex Young |
26:665d3624f9ab | 133 | { |
Alex Young |
26:665d3624f9ab | 134 | pc.printf("Timeout waiting for response.\n"); |
Alex Young |
26:665d3624f9ab | 135 | } |
Alex Young |
11:8593ba137917 | 136 | } |
Alex Young |
11:8593ba137917 | 137 | |
Alex Young |
11:8593ba137917 | 138 | static void handlePcCommand(char cmd) |
Alex Young |
11:8593ba137917 | 139 | { |
Alex Young |
11:8593ba137917 | 140 | switch (cmd) |
Alex Young |
11:8593ba137917 | 141 | { |
Alex Young |
11:8593ba137917 | 142 | case 'c': |
Alex Young |
11:8593ba137917 | 143 | sendCommand(XMID_GotoConfig); |
Alex Young |
11:8593ba137917 | 144 | break; |
Alex Young |
11:8593ba137917 | 145 | |
Alex Young |
11:8593ba137917 | 146 | case 'm': |
Alex Young |
11:8593ba137917 | 147 | sendCommand(XMID_GotoMeasurement); |
Alex Young |
11:8593ba137917 | 148 | break; |
Alex Young |
20:38560fa3d2eb | 149 | |
Alex Young |
20:38560fa3d2eb | 150 | case 'd': |
Alex Young |
20:38560fa3d2eb | 151 | sendCommand(XMID_ReqDid); |
Alex Young |
20:38560fa3d2eb | 152 | break; |
Alex Young |
22:3eab999c5076 | 153 | |
Alex Young |
22:3eab999c5076 | 154 | case 'o': |
Alex Young |
22:3eab999c5076 | 155 | sendCommand(XMID_ReqOutputConfig); |
Alex Young |
22:3eab999c5076 | 156 | break; |
Alex Young |
11:8593ba137917 | 157 | } |
Alex Young |
11:8593ba137917 | 158 | } |
Alex Young |
11:8593ba137917 | 159 | |
Alex Young |
24:2cc49dc854e3 | 160 | static void handleDataMessage(struct XbusMessage const* message) |
Alex Young |
24:2cc49dc854e3 | 161 | { |
Alex Young |
24:2cc49dc854e3 | 162 | pc.printf("MTData2:"); |
Alex Young |
24:2cc49dc854e3 | 163 | uint16_t counter; |
Alex Young |
24:2cc49dc854e3 | 164 | if (XbusMessage_getDataItem(&counter, XDI_PacketCounter, message)) |
Alex Young |
24:2cc49dc854e3 | 165 | { |
Alex Young |
24:2cc49dc854e3 | 166 | pc.printf(" Packet counter: %5d", counter); |
Alex Young |
24:2cc49dc854e3 | 167 | } |
Alex Young |
24:2cc49dc854e3 | 168 | float ori[4]; |
Alex Young |
24:2cc49dc854e3 | 169 | if (XbusMessage_getDataItem(ori, XDI_Quaternion, message)) |
Alex Young |
24:2cc49dc854e3 | 170 | { |
Alex Young |
24:2cc49dc854e3 | 171 | pc.printf(" Orientation: (% .3f, % .3f, % .3f, % .3f)", ori[0], ori[1], |
Alex Young |
24:2cc49dc854e3 | 172 | ori[2], ori[3]); |
Alex Young |
24:2cc49dc854e3 | 173 | } |
Alex Young |
24:2cc49dc854e3 | 174 | uint32_t status; |
Alex Young |
24:2cc49dc854e3 | 175 | if (XbusMessage_getDataItem(&status, XDI_StatusWord, message)) |
Alex Young |
24:2cc49dc854e3 | 176 | { |
Alex Young |
24:2cc49dc854e3 | 177 | pc.printf(" Status:%X", status); |
Alex Young |
24:2cc49dc854e3 | 178 | } |
Alex Young |
24:2cc49dc854e3 | 179 | pc.printf("\n"); |
Alex Young |
26:665d3624f9ab | 180 | deallocateMessageData(message->data); |
Alex Young |
24:2cc49dc854e3 | 181 | } |
Alex Young |
24:2cc49dc854e3 | 182 | |
Alex Young |
24:2cc49dc854e3 | 183 | static void mtMessageHandler(struct XbusMessage const* message) |
Alex Young |
4:98f063b2e6da | 184 | { |
Alex Young |
15:558d279addd9 | 185 | if (message->mid == XMID_MtData2) |
Alex Young |
7:c913a7cd5231 | 186 | { |
Alex Young |
24:2cc49dc854e3 | 187 | handleDataMessage(message); |
Alex Young |
7:c913a7cd5231 | 188 | } |
Alex Young |
7:c913a7cd5231 | 189 | else |
Alex Young |
7:c913a7cd5231 | 190 | { |
Alex Young |
26:665d3624f9ab | 191 | XbusMessage* m = g_messagePool.alloc(); |
Alex Young |
26:665d3624f9ab | 192 | memcpy(m, message, sizeof(XbusMessage)); |
Alex Young |
26:665d3624f9ab | 193 | g_responseQueue.put(m); |
Alex Young |
25:01356fb59467 | 194 | } |
Alex Young |
4:98f063b2e6da | 195 | } |
Alex Young |
4:98f063b2e6da | 196 | |
Alex Young |
4:98f063b2e6da | 197 | static void configureSerialPorts(void) |
Alex Young |
4:98f063b2e6da | 198 | { |
Alex Young |
4:98f063b2e6da | 199 | pc.baud(921600); |
Alex Young |
4:98f063b2e6da | 200 | pc.format(8, Serial::None, 2); |
Alex Young |
4:98f063b2e6da | 201 | |
Alex Young |
4:98f063b2e6da | 202 | mt.baud(921600); |
Alex Young |
4:98f063b2e6da | 203 | mt.format(8, Serial::None, 2); |
Alex Young |
4:98f063b2e6da | 204 | mt.attach(mtLowLevelHandler, Serial::RxIrq); |
Alex Young |
4:98f063b2e6da | 205 | } |
Alex Young |
4:98f063b2e6da | 206 | |
Alex Young |
29:d9310e7b58b5 | 207 | static uint32_t readDeviceId(void) |
Alex Young |
29:d9310e7b58b5 | 208 | { |
Alex Young |
29:d9310e7b58b5 | 209 | XbusMessage reqDid = {XMID_ReqDid}; |
Alex Young |
29:d9310e7b58b5 | 210 | XbusMessage const* didRsp = doTransaction(&reqDid); |
Alex Young |
31:ce1ea9ae861e | 211 | XbusMessageMemoryManager janitor(didRsp); |
Alex Young |
29:d9310e7b58b5 | 212 | uint32_t deviceId = 0; |
Alex Young |
29:d9310e7b58b5 | 213 | if (didRsp) |
Alex Young |
29:d9310e7b58b5 | 214 | { |
Alex Young |
29:d9310e7b58b5 | 215 | if (didRsp->mid == XMID_DeviceId) |
Alex Young |
29:d9310e7b58b5 | 216 | { |
Alex Young |
29:d9310e7b58b5 | 217 | deviceId = *(uint32_t*)didRsp->data; |
Alex Young |
29:d9310e7b58b5 | 218 | } |
Alex Young |
29:d9310e7b58b5 | 219 | } |
Alex Young |
29:d9310e7b58b5 | 220 | return deviceId; |
Alex Young |
29:d9310e7b58b5 | 221 | } |
Alex Young |
29:d9310e7b58b5 | 222 | |
Alex Young |
32:fafe0f42d82b | 223 | static bool setOutputConfiguration(OutputConfiguration const* conf, uint8_t elements) |
Alex Young |
29:d9310e7b58b5 | 224 | { |
Alex Young |
32:fafe0f42d82b | 225 | XbusMessage outputConfMsg = {XMID_SetOutputConfig, elements, (void*)conf}; |
Alex Young |
32:fafe0f42d82b | 226 | XbusMessage const* outputConfRsp = doTransaction(&outputConfMsg); |
Alex Young |
32:fafe0f42d82b | 227 | XbusMessageMemoryManager janitor(outputConfRsp); |
Alex Young |
32:fafe0f42d82b | 228 | if (outputConfRsp) |
Alex Young |
29:d9310e7b58b5 | 229 | { |
Alex Young |
32:fafe0f42d82b | 230 | if (outputConfRsp->mid == XMID_OutputConfig) |
Alex Young |
29:d9310e7b58b5 | 231 | { |
Alex Young |
32:fafe0f42d82b | 232 | pc.printf("Output configuration set to:\n"); |
Alex Young |
32:fafe0f42d82b | 233 | OutputConfiguration* conf = (OutputConfiguration*)outputConfRsp->data; |
Alex Young |
32:fafe0f42d82b | 234 | for (int i = 0; i < outputConfRsp->length; ++i) |
Alex Young |
32:fafe0f42d82b | 235 | { |
Alex Young |
32:fafe0f42d82b | 236 | pc.printf("\t%s: %d Hz\n", XbusMessage_dataDescription(conf->dtype), conf->freq); |
Alex Young |
32:fafe0f42d82b | 237 | ++conf; |
Alex Young |
32:fafe0f42d82b | 238 | } |
Alex Young |
32:fafe0f42d82b | 239 | return true; |
Alex Young |
29:d9310e7b58b5 | 240 | } |
Alex Young |
29:d9310e7b58b5 | 241 | else |
Alex Young |
29:d9310e7b58b5 | 242 | { |
Alex Young |
32:fafe0f42d82b | 243 | dumpResponse(outputConfRsp); |
Alex Young |
29:d9310e7b58b5 | 244 | } |
Alex Young |
32:fafe0f42d82b | 245 | } |
Alex Young |
32:fafe0f42d82b | 246 | else |
Alex Young |
32:fafe0f42d82b | 247 | { |
Alex Young |
32:fafe0f42d82b | 248 | pc.printf("Failed to set output configuration.\n"); |
Alex Young |
32:fafe0f42d82b | 249 | } |
Alex Young |
32:fafe0f42d82b | 250 | return false; |
Alex Young |
32:fafe0f42d82b | 251 | } |
Alex Young |
29:d9310e7b58b5 | 252 | |
Alex Young |
32:fafe0f42d82b | 253 | static bool configureMotionTracker(void) |
Alex Young |
32:fafe0f42d82b | 254 | { |
Alex Young |
32:fafe0f42d82b | 255 | uint32_t deviceId = readDeviceId(); |
Alex Young |
32:fafe0f42d82b | 256 | |
Alex Young |
32:fafe0f42d82b | 257 | if (deviceId) |
Alex Young |
32:fafe0f42d82b | 258 | { |
Alex Young |
32:fafe0f42d82b | 259 | uint8_t deviceType = (deviceId >> 24) & 0x0F; |
Alex Young |
32:fafe0f42d82b | 260 | pc.printf("Found MTi-%d\n", deviceType); |
Alex Young |
32:fafe0f42d82b | 261 | |
Alex Young |
32:fafe0f42d82b | 262 | if (deviceType == 1) |
Alex Young |
29:d9310e7b58b5 | 263 | { |
Alex Young |
32:fafe0f42d82b | 264 | OutputConfiguration conf[] = { |
Alex Young |
32:fafe0f42d82b | 265 | {XDI_PacketCounter, 65535}, |
Alex Young |
32:fafe0f42d82b | 266 | {XDI_SampleTimeFine, 65535}, |
Alex Young |
32:fafe0f42d82b | 267 | {XDI_Acceleration, 100}, |
Alex Young |
32:fafe0f42d82b | 268 | {XDI_RateOfTurn, 100}, |
Alex Young |
32:fafe0f42d82b | 269 | {XDI_MagneticField, 100} |
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 | else |
Alex Young |
29:d9310e7b58b5 | 275 | { |
Alex Young |
32:fafe0f42d82b | 276 | OutputConfiguration conf[] = { |
Alex Young |
32:fafe0f42d82b | 277 | {XDI_PacketCounter, 65535}, |
Alex Young |
32:fafe0f42d82b | 278 | {XDI_SampleTimeFine, 65535}, |
Alex Young |
32:fafe0f42d82b | 279 | {XDI_Quaternion, 100}, |
Alex Young |
32:fafe0f42d82b | 280 | {XDI_StatusWord, 65535} |
Alex Young |
32:fafe0f42d82b | 281 | }; |
Alex Young |
32:fafe0f42d82b | 282 | return setOutputConfiguration(conf, |
Alex Young |
32:fafe0f42d82b | 283 | sizeof(conf) / sizeof(OutputConfiguration)); |
Alex Young |
29:d9310e7b58b5 | 284 | } |
Alex Young |
29:d9310e7b58b5 | 285 | } |
Alex Young |
32:fafe0f42d82b | 286 | |
Alex Young |
32:fafe0f42d82b | 287 | return false; |
Alex Young |
29:d9310e7b58b5 | 288 | } |
Alex Young |
29:d9310e7b58b5 | 289 | |
Alex Young |
35:7e519b88c610 | 290 | /*! |
Alex Young |
35:7e519b88c610 | 291 | * \brief Wait for a wakeup message from the MTi. |
Alex Young |
35:7e519b88c610 | 292 | * \return true if wakeup received within 1 second, else false. |
Alex Young |
35:7e519b88c610 | 293 | * |
Alex Young |
35:7e519b88c610 | 294 | * The MTi sends a XMID_Wakeup message once it has completed its bootup |
Alex Young |
35:7e519b88c610 | 295 | * procedure. If this is acknowledged by a XMID_WakeupAck message then the MTi |
Alex Young |
35:7e519b88c610 | 296 | * will stay in configuration mode. Otherwise it will automatically enter |
Alex Young |
35:7e519b88c610 | 297 | * measurement mode with the stored output configuration. |
Alex Young |
35:7e519b88c610 | 298 | */ |
Alex Young |
35:7e519b88c610 | 299 | bool waitForWakeup(void) |
Alex Young |
35:7e519b88c610 | 300 | { |
Alex Young |
35:7e519b88c610 | 301 | osEvent ev = g_responseQueue.get(1000); |
Alex Young |
35:7e519b88c610 | 302 | if (ev.status == osEventMessage) |
Alex Young |
35:7e519b88c610 | 303 | { |
Alex Young |
35:7e519b88c610 | 304 | XbusMessage const* m = (XbusMessage const*)ev.value.p; |
Alex Young |
35:7e519b88c610 | 305 | XbusMessageMemoryManager janitor(m); |
Alex Young |
35:7e519b88c610 | 306 | return m->mid == XMID_Wakeup; |
Alex Young |
35:7e519b88c610 | 307 | } |
Alex Young |
35:7e519b88c610 | 308 | return false; |
Alex Young |
35:7e519b88c610 | 309 | } |
Alex Young |
35:7e519b88c610 | 310 | |
Alex Young |
35:7e519b88c610 | 311 | /*! |
Alex Young |
35:7e519b88c610 | 312 | * \brief Releases the MTi reset line and waits for a wakeup message. |
Alex Young |
35:7e519b88c610 | 313 | */ |
Alex Young |
35:7e519b88c610 | 314 | static void wakeupMotionTracker(void) |
Alex Young |
35:7e519b88c610 | 315 | { |
Alex Young |
35:7e519b88c610 | 316 | mtReset.write(1); // Release MT from reset. |
Alex Young |
35:7e519b88c610 | 317 | if (waitForWakeup()) |
Alex Young |
35:7e519b88c610 | 318 | { |
Alex Young |
35:7e519b88c610 | 319 | XbusMessage ack = {XMID_WakeupAck}; |
Alex Young |
35:7e519b88c610 | 320 | sendMessage(&ack); |
Alex Young |
35:7e519b88c610 | 321 | } |
Alex Young |
35:7e519b88c610 | 322 | } |
Alex Young |
35:7e519b88c610 | 323 | |
Alex Young |
2:b3e402dc11ca | 324 | int main(void) |
Alex Young |
2:b3e402dc11ca | 325 | { |
Alex Young |
4:98f063b2e6da | 326 | XbusParserCallback xbusCallback = {}; |
Alex Young |
25:01356fb59467 | 327 | xbusCallback.allocateBuffer = allocateMessageData; |
Alex Young |
25:01356fb59467 | 328 | xbusCallback.deallocateBuffer = deallocateMessageData; |
Alex Young |
24:2cc49dc854e3 | 329 | xbusCallback.handleMessage = mtMessageHandler; |
Alex Young |
4:98f063b2e6da | 330 | |
Alex Young |
4:98f063b2e6da | 331 | xbusParser = XbusParser_create(&xbusCallback); |
Alex Young |
4:98f063b2e6da | 332 | configureSerialPorts(); |
Alex Young |
35:7e519b88c610 | 333 | wakeupMotionTracker(); |
Alex Young |
29:d9310e7b58b5 | 334 | if (configureMotionTracker()) |
Alex Young |
5:abc52dd88be2 | 335 | { |
Alex Young |
29:d9310e7b58b5 | 336 | for (;;) |
Alex Young |
26:665d3624f9ab | 337 | { |
Alex Young |
29:d9310e7b58b5 | 338 | while (pc.readable()) |
Alex Young |
29:d9310e7b58b5 | 339 | { |
Alex Young |
29:d9310e7b58b5 | 340 | handlePcCommand(pc.getc()); |
Alex Young |
29:d9310e7b58b5 | 341 | } |
Alex Young |
26:665d3624f9ab | 342 | } |
Alex Young |
5:abc52dd88be2 | 343 | } |
Alex Young |
29:d9310e7b58b5 | 344 | else |
Alex Young |
29:d9310e7b58b5 | 345 | { |
Alex Young |
29:d9310e7b58b5 | 346 | pc.printf("Failed to configure motion tracker.\n"); |
Alex Young |
29:d9310e7b58b5 | 347 | return -1; |
Alex Young |
29:d9310e7b58b5 | 348 | } |
Alex Young |
4:98f063b2e6da | 349 | } |