FRDM-KL25Zand Xsens MTi-3
Dependencies: mbed mbed-rtos Xbus
Diff: main.cpp
- Revision:
- 44:b3980e8ac074
- Parent:
- 43:470c019246e4
- Child:
- 49:38ecfbff5391
--- a/main.cpp Thu May 21 15:34:18 2015 +0200 +++ b/main.cpp Thu May 21 16:01:20 2015 +0200 @@ -19,12 +19,33 @@ #include "xbusmessage.h" #include "xsdeviceid.h" +/*! + * \brief The number of items to hold in the memory pools. + */ #define MEMORY_POOL_SIZE (4) +/*! + * \brief The size of the queue used for device responses. + * This is set to one as in typical Xbus operation each command receives a + * response before the next command is sent. + */ #define RESPONSE_QUEUE_SIZE (1) +/*! + * \brief The size of the queue used for data messages. + * This is set to two to allow some overlap between printing received data to + * the PC serial port and the reception of the subsequent data packet. In + * more complex applications it might be necessary to increase this if + * message processing might occasionally require more time than normal. + */ #define DATA_QUEUE_SIZE (2) +/*! + * \brief The maximum size of a xbus message supported by the application. + * This is the size of the message buffers in the message data memory pool. + */ #define MAX_XBUS_DATA_SIZE (128) +/*! \brief Serial port for communication with the host PC. */ static Serial pc(PA_2, PA_3); +/*! \brief Serial port for communication with the MT. */ static Serial mt(PB_9, PB_8); /*! * \brief MT reset line. @@ -32,23 +53,49 @@ * MT is held in reset on startup. */ static DigitalOut mtReset(PA_10, 0); +/*! \brief XbusParser used to parse incoming Xbus messages from the MT. */ static XbusParser* xbusParser; +/*! + * \brief Memory pool used for storing Xbus messages when passing them + * to the main thread. + */ MemoryPool<XbusMessage, MEMORY_POOL_SIZE> g_messagePool; +/*! + * \brief Memory pool used for storing the payload of Xbus messages. + */ MemoryPool<uint8_t[MAX_XBUS_DATA_SIZE], MEMORY_POOL_SIZE> g_messageDataPool; +/*! + * \brief Queue used to pass data messages to the main thread for processing. + */ +Queue<XbusMessage, DATA_QUEUE_SIZE> g_dataQueue; +/*! + * \brief Queue used for passing all other messages to the main thread for processing. + */ Queue<XbusMessage, RESPONSE_QUEUE_SIZE> g_responseQueue; -Queue<XbusMessage, DATA_QUEUE_SIZE> g_dataQueue; +/*! + * \brief Allocate message data buffer from the message data pool. + */ static void* allocateMessageData(size_t bufSize) { return bufSize < MAX_XBUS_DATA_SIZE ? g_messageDataPool.alloc() : NULL; } +/*! + * \brief Deallocate message data previously allocated from the message + * data pool. + */ static void deallocateMessageData(void const* buffer) { g_messageDataPool.free((uint8_t(*)[MAX_XBUS_DATA_SIZE])buffer); } +/*! + * \brief RX Interrupt handler for the MT serial port. + * + * Passes received data to an XbusParser to extract messages. + */ static void mtLowLevelHandler(void) { while (mt.readable()) @@ -57,6 +104,12 @@ } } +/*! + * \brief Send a message to the MT + * + * This function formats the message data and writes this to the MT serial + * port. It does not wait for any response. + */ static void sendMessage(XbusMessage const* m) { uint8_t buf[64]; @@ -67,6 +120,14 @@ } } +/*! + * \brief Send a message to the MT and wait for a response. + * \returns Response message from the MT, or NULL is no response received + * within 500ms. + * + * Blocking behaviour is implemented by waiting for a response to be written + * to the response queue by the XbusParser. + */ static XbusMessage const* doTransaction(XbusMessage const* m) { sendMessage(m); @@ -103,6 +164,9 @@ XbusMessage const* m_message; }; +/*! + * \brief Dump information from a message to the PC serial port. + */ static void dumpResponse(XbusMessage const* response) { switch (response->mid) @@ -121,6 +185,12 @@ } } +/*! + * \brief Send a command to the MT and wait for a response. + * \param cmdId The XsMessageId of the command to send. + * + * Commands are simple messages without and payload data. + */ static void sendCommand(XsMessageId cmdId) { XbusMessage m = {cmdId}; @@ -137,6 +207,12 @@ } } +/*! + * \brief Handle a command from the PC + * + * The example application supports single character commands from the host + * PC to switch between configuration and measurement modes. + */ static void handlePcCommand(char cmd) { switch (cmd) @@ -151,6 +227,16 @@ } } +/*! + * \brief XbusParser callback function to handle received messages. + * \param message Pointer to the last received message. + * + * In this example received messages are copied into one of two message + * queues for later handling by the main thread. Data messages are put + * in one queue, while all other responses are place in the second queue. + * This is done so that data and other messages can be handled separately + * by the application code. + */ static void mtMessageHandler(struct XbusMessage const* message) { XbusMessage* m = g_messagePool.alloc(); @@ -172,6 +258,10 @@ } } +/*! + * \brief Configure the serial ports used to communicate with the motion + * tracker and host PC. + */ static void configureSerialPorts(void) { pc.baud(921600); @@ -182,6 +272,9 @@ mt.attach(mtLowLevelHandler, Serial::RxIrq); } +/*! + * \brief Read the device ID of the motion tracker. + */ static uint32_t readDeviceId(void) { XbusMessage reqDid = {XMID_ReqDid}; @@ -198,6 +291,16 @@ return deviceId; } +/*! + * \brief Sets MT output configuration. + * \param conf Pointer to an array of OutputConfiguration elements. + * \param elements The number of elements in the configuration array. + * + * The response from the device indicates the actual values that will + * be used by the motion tracker. These may differ from the requested + * parameters as the motion tracker validates the requested parameters + * before applying them. + */ static bool setOutputConfiguration(OutputConfiguration const* conf, uint8_t elements) { XbusMessage outputConfMsg = {XMID_SetOutputConfig, elements, (void*)conf}; @@ -228,6 +331,16 @@ return false; } +/*! + * \brief Sets the motion tracker output configuration based on the function + * of the attached device. + * + * The output configuration depends on the type of MTi-1 device connected. + * A MTI-1 (IMU) device does not have an onboard orientation filter so + * cannot output quaternion data, only inertial and magnetic measurement + * data. + * MTi-2 and MTi-3 devices have an onboard filter so can send quaternions. + */ static bool configureMotionTracker(void) { uint32_t deviceId = readDeviceId(); @@ -363,6 +476,9 @@ pc.printf("Press 'm' to start measuring and 'c' to return to config mode.\n"); } +/*! + * \brief Output the contents of a data message to the PC serial port. + */ static void printMessageData(struct XbusMessage const* message) { if (!message)