FRDM-KL25Zand Xsens MTi-3
Dependencies: mbed mbed-rtos Xbus
main.cpp@71:9cf0c68ac380, 2020-08-14 (annotated)
- Committer:
- gabriel_delgado
- Date:
- Fri Aug 14 12:33:17 2020 +0000
- Revision:
- 71:9cf0c68ac380
- Parent:
- 70:ff3afeaa31be
MTi-3 UART and FRDM-KL25Z
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 |
40:b77a8c10c76d | 5 | #include "xsdeviceid.h" |
tjerkhofmeijer | 64:8a0f00a064bb | 6 | #include "xbusdef.h" |
tjerkhofmeijer | 64:8a0f00a064bb | 7 | |
tjerkhofmeijer | 64:8a0f00a064bb | 8 | // Select communication interface to use for MTi |
gabriel_delgado | 71:9cf0c68ac380 | 9 | #define MTI_USES_UART_INTERFACE |
tjerkhofmeijer | 64:8a0f00a064bb | 10 | |
tjerkhofmeijer | 64:8a0f00a064bb | 11 | #if !(defined(MTI_USES_I2C_INTERFACE) || defined(MTI_USES_SPI_INTERFACE) || defined(MTI_USES_UART_INTERFACE)) |
tjerkhofmeijer | 64:8a0f00a064bb | 12 | #error "Must select communication interface by defining one of: MTI_USES_I2C_INTERFACE, MTI_USES_SPI_INTERFACE or MTI_USES_UART_INTERFACE" |
tjerkhofmeijer | 64:8a0f00a064bb | 13 | #endif |
Alex Young |
4:98f063b2e6da | 14 | |
gabriel_delgado | 71:9cf0c68ac380 | 15 | #if defined(TARGET_KL25Z) |
tjerkhofmeijer | 64:8a0f00a064bb | 16 | |
tjerkhofmeijer | 66:f12dec1c0c3d | 17 | #define PC_TX USBTX |
tjerkhofmeijer | 66:f12dec1c0c3d | 18 | #define PC_RX USBRX |
tjerkhofmeijer | 66:f12dec1c0c3d | 19 | #define MT_TX PTE0 |
tjerkhofmeijer | 66:f12dec1c0c3d | 20 | #define MT_RX PTE1 |
tjerkhofmeijer | 64:8a0f00a064bb | 21 | |
tjerkhofmeijer | 64:8a0f00a064bb | 22 | |
Alex Young |
57:c3c85ebb7375 | 23 | #else |
tjerkhofmeijer | 66:f12dec1c0c3d | 24 | |
Alex Young |
57:c3c85ebb7375 | 25 | #error "Support for selected mbed platform has not been added." |
tjerkhofmeijer | 66:f12dec1c0c3d | 26 | |
Alex Young |
57:c3c85ebb7375 | 27 | #endif |
Alex Young |
57:c3c85ebb7375 | 28 | |
gabriel_delgado | 71:9cf0c68ac380 | 29 | #define PC_UART_BAUDRATE (115200) |
Alex Young |
57:c3c85ebb7375 | 30 | |
Alex Young |
25:01356fb59467 | 31 | #define MEMORY_POOL_SIZE (4) |
gabriel_delgado | 71:9cf0c68ac380 | 32 | |
Alex Young |
26:665d3624f9ab | 33 | #define RESPONSE_QUEUE_SIZE (1) |
gabriel_delgado | 71:9cf0c68ac380 | 34 | |
Alex Young |
43:470c019246e4 | 35 | #define DATA_QUEUE_SIZE (2) |
gabriel_delgado | 71:9cf0c68ac380 | 36 | |
Alex Young |
25:01356fb59467 | 37 | #define MAX_XBUS_DATA_SIZE (128) |
Alex Young |
25:01356fb59467 | 38 | |
Alex Young |
57:c3c85ebb7375 | 39 | static Serial pc(PC_TX, PC_RX); |
tjerkhofmeijer | 64:8a0f00a064bb | 40 | |
tjerkhofmeijer | 64:8a0f00a064bb | 41 | |
gabriel_delgado | 71:9cf0c68ac380 | 42 | #if defined(MTI_USES_UART_INTERFACE) |
Alex Young |
58:db60ef0a0d16 | 43 | static RawSerial mt(MT_TX, MT_RX); |
tjerkhofmeijer | 64:8a0f00a064bb | 44 | #endif |
tjerkhofmeijer | 64:8a0f00a064bb | 45 | |
tjerkhofmeijer | 64:8a0f00a064bb | 46 | |
Alex Young |
4:98f063b2e6da | 47 | static XbusParser* xbusParser; |
Alex Young |
25:01356fb59467 | 48 | |
Alex Young |
25:01356fb59467 | 49 | MemoryPool<XbusMessage, MEMORY_POOL_SIZE> g_messagePool; |
gabriel_delgado | 71:9cf0c68ac380 | 50 | |
Alex Young |
25:01356fb59467 | 51 | MemoryPool<uint8_t[MAX_XBUS_DATA_SIZE], MEMORY_POOL_SIZE> g_messageDataPool; |
gabriel_delgado | 71:9cf0c68ac380 | 52 | |
Alex Young |
44:b3980e8ac074 | 53 | Queue<XbusMessage, DATA_QUEUE_SIZE> g_dataQueue; |
gabriel_delgado | 71:9cf0c68ac380 | 54 | |
Alex Young |
26:665d3624f9ab | 55 | Queue<XbusMessage, RESPONSE_QUEUE_SIZE> g_responseQueue; |
Alex Young |
4:98f063b2e6da | 56 | |
Alex Young |
25:01356fb59467 | 57 | static void* allocateMessageData(size_t bufSize) |
Alex Young |
4:98f063b2e6da | 58 | { |
Alex Young |
25:01356fb59467 | 59 | return bufSize < MAX_XBUS_DATA_SIZE ? g_messageDataPool.alloc() : NULL; |
Alex Young |
25:01356fb59467 | 60 | } |
Alex Young |
25:01356fb59467 | 61 | |
Alex Young |
25:01356fb59467 | 62 | static void deallocateMessageData(void const* buffer) |
Alex Young |
25:01356fb59467 | 63 | { |
Alex Young |
25:01356fb59467 | 64 | g_messageDataPool.free((uint8_t(*)[MAX_XBUS_DATA_SIZE])buffer); |
Alex Young |
4:98f063b2e6da | 65 | } |
Alex Young |
4:98f063b2e6da | 66 | |
tjerkhofmeijer | 64:8a0f00a064bb | 67 | |
gabriel_delgado | 71:9cf0c68ac380 | 68 | #if defined(MTI_USES_UART_INTERFACE) |
tjerkhofmeijer | 64:8a0f00a064bb | 69 | |
Alex Young |
4:98f063b2e6da | 70 | static void mtLowLevelHandler(void) |
Alex Young |
4:98f063b2e6da | 71 | { |
Alex Young |
4:98f063b2e6da | 72 | while (mt.readable()) |
Alex Young |
4:98f063b2e6da | 73 | { |
Alex Young |
4:98f063b2e6da | 74 | XbusParser_parseByte(xbusParser, mt.getc()); |
Alex Young |
4:98f063b2e6da | 75 | } |
Alex Young |
4:98f063b2e6da | 76 | } |
Alex Young |
4:98f063b2e6da | 77 | |
gabriel_delgado | 71:9cf0c68ac380 | 78 | |
gabriel_delgado | 71:9cf0c68ac380 | 79 | |
tjerkhofmeijer | 64:8a0f00a064bb | 80 | static void configureMtCommunicationInterface(void) |
tjerkhofmeijer | 64:8a0f00a064bb | 81 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 82 | mt.baud(115200); |
tjerkhofmeijer | 64:8a0f00a064bb | 83 | mt.format(8, Serial::None, 1); |
tjerkhofmeijer | 64:8a0f00a064bb | 84 | mt.attach(mtLowLevelHandler, Serial::RxIrq); |
tjerkhofmeijer | 64:8a0f00a064bb | 85 | } |
tjerkhofmeijer | 64:8a0f00a064bb | 86 | |
tjerkhofmeijer | 64:8a0f00a064bb | 87 | #endif |
tjerkhofmeijer | 64:8a0f00a064bb | 88 | |
Alex Young |
34:3d7a6519a256 | 89 | |
Alex Young |
31:ce1ea9ae861e | 90 | class XbusMessageMemoryManager |
Alex Young |
26:665d3624f9ab | 91 | { |
Alex Young |
31:ce1ea9ae861e | 92 | public: |
Alex Young |
31:ce1ea9ae861e | 93 | XbusMessageMemoryManager(XbusMessage const* message) |
Alex Young |
31:ce1ea9ae861e | 94 | : m_message(message) |
Alex Young |
31:ce1ea9ae861e | 95 | { |
Alex Young |
31:ce1ea9ae861e | 96 | } |
Alex Young |
31:ce1ea9ae861e | 97 | |
Alex Young |
31:ce1ea9ae861e | 98 | ~XbusMessageMemoryManager() |
Alex Young |
31:ce1ea9ae861e | 99 | { |
Alex Young |
31:ce1ea9ae861e | 100 | if (m_message) |
Alex Young |
31:ce1ea9ae861e | 101 | { |
Alex Young |
31:ce1ea9ae861e | 102 | if (m_message->data) |
Alex Young |
31:ce1ea9ae861e | 103 | deallocateMessageData(m_message->data); |
Alex Young |
31:ce1ea9ae861e | 104 | g_messagePool.free(const_cast<XbusMessage*>(m_message)); |
Alex Young |
31:ce1ea9ae861e | 105 | } |
Alex Young |
31:ce1ea9ae861e | 106 | } |
Alex Young |
31:ce1ea9ae861e | 107 | |
Alex Young |
31:ce1ea9ae861e | 108 | private: |
Alex Young |
31:ce1ea9ae861e | 109 | XbusMessage const* m_message; |
Alex Young |
31:ce1ea9ae861e | 110 | }; |
Alex Young |
26:665d3624f9ab | 111 | |
Alex Young |
26:665d3624f9ab | 112 | |
Alex Young |
11:8593ba137917 | 113 | |
Alex Young |
24:2cc49dc854e3 | 114 | static void mtMessageHandler(struct XbusMessage const* message) |
Alex Young |
4:98f063b2e6da | 115 | { |
Alex Young |
43:470c019246e4 | 116 | XbusMessage* m = g_messagePool.alloc(); |
Alex Young |
43:470c019246e4 | 117 | if (m) |
Alex Young |
7:c913a7cd5231 | 118 | { |
Alex Young |
43:470c019246e4 | 119 | memcpy(m, message, sizeof(XbusMessage)); |
Alex Young |
43:470c019246e4 | 120 | if (message->mid == XMID_MtData2) |
Alex Young |
43:470c019246e4 | 121 | { |
Alex Young |
43:470c019246e4 | 122 | g_dataQueue.put(m); |
Alex Young |
43:470c019246e4 | 123 | } |
Alex Young |
43:470c019246e4 | 124 | else |
Alex Young |
43:470c019246e4 | 125 | { |
Alex Young |
43:470c019246e4 | 126 | g_responseQueue.put(m); |
Alex Young |
43:470c019246e4 | 127 | } |
Alex Young |
7:c913a7cd5231 | 128 | } |
Alex Young |
43:470c019246e4 | 129 | else if (message->data) |
Alex Young |
7:c913a7cd5231 | 130 | { |
Alex Young |
43:470c019246e4 | 131 | deallocateMessageData(message->data); |
Alex Young |
25:01356fb59467 | 132 | } |
Alex Young |
4:98f063b2e6da | 133 | } |
Alex Young |
4:98f063b2e6da | 134 | |
gabriel_delgado | 71:9cf0c68ac380 | 135 | |
tjerkhofmeijer | 64:8a0f00a064bb | 136 | static void configurePcInterface(void) |
Alex Young |
4:98f063b2e6da | 137 | { |
Alex Young |
53:3891f4259901 | 138 | pc.baud(PC_UART_BAUDRATE); |
Alex Young |
55:9a2d6f947f0d | 139 | pc.format(8, Serial::None, 1); |
Alex Young |
4:98f063b2e6da | 140 | } |
Alex Young |
4:98f063b2e6da | 141 | |
Alex Young |
40:b77a8c10c76d | 142 | |
Alex Young |
32:fafe0f42d82b | 143 | |
Alex Young |
43:470c019246e4 | 144 | static void printMessageData(struct XbusMessage const* message) |
Alex Young |
43:470c019246e4 | 145 | { |
Alex Young |
43:470c019246e4 | 146 | if (!message) |
Alex Young |
43:470c019246e4 | 147 | return; |
Alex Young |
43:470c019246e4 | 148 | |
Alex Young |
43:470c019246e4 | 149 | float ori[4]; |
Alex Young |
43:470c019246e4 | 150 | if (XbusMessage_getDataItem(ori, XDI_Quaternion, message)) |
Alex Young |
43:470c019246e4 | 151 | { |
gabriel_delgado | 71:9cf0c68ac380 | 152 | pc.printf("% .3f, % .3f, % .3f, % .3f", ori[0], ori[1], ori[2], ori[3]); |
Alex Young |
43:470c019246e4 | 153 | } |
Alex Young |
52:e2197b38c029 | 154 | pc.printf("\r\n"); |
Alex Young |
43:470c019246e4 | 155 | } |
Alex Young |
43:470c019246e4 | 156 | |
Alex Young |
2:b3e402dc11ca | 157 | int main(void) |
Alex Young |
2:b3e402dc11ca | 158 | { |
Alex Young |
4:98f063b2e6da | 159 | XbusParserCallback xbusCallback = {}; |
Alex Young |
25:01356fb59467 | 160 | xbusCallback.allocateBuffer = allocateMessageData; |
Alex Young |
25:01356fb59467 | 161 | xbusCallback.deallocateBuffer = deallocateMessageData; |
Alex Young |
24:2cc49dc854e3 | 162 | xbusCallback.handleMessage = mtMessageHandler; |
Alex Young |
4:98f063b2e6da | 163 | |
Alex Young |
4:98f063b2e6da | 164 | xbusParser = XbusParser_create(&xbusCallback); |
tjerkhofmeijer | 64:8a0f00a064bb | 165 | configurePcInterface(); |
tjerkhofmeijer | 64:8a0f00a064bb | 166 | configureMtCommunicationInterface(); |
gabriel_delgado | 71:9cf0c68ac380 | 167 | |
tjerkhofmeijer | 64:8a0f00a064bb | 168 | for (;;) |
Alex Young |
29:d9310e7b58b5 | 169 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 170 | osEvent ev = g_dataQueue.get(10); |
tjerkhofmeijer | 64:8a0f00a064bb | 171 | if (ev.status == osEventMessage) |
tjerkhofmeijer | 64:8a0f00a064bb | 172 | { |
tjerkhofmeijer | 64:8a0f00a064bb | 173 | XbusMessage const* data = (XbusMessage const*)ev.value.p; |
tjerkhofmeijer | 64:8a0f00a064bb | 174 | XbusMessageMemoryManager janitor(data); |
tjerkhofmeijer | 64:8a0f00a064bb | 175 | printMessageData(data); |
tjerkhofmeijer | 64:8a0f00a064bb | 176 | } |
Alex Young |
43:470c019246e4 | 177 | } |
gabriel_delgado | 71:9cf0c68ac380 | 178 | |
Alex Young |
4:98f063b2e6da | 179 | } |