Gabriel Delgado / Mbed 2 deprecated MTi-1_uart

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?

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 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 }