FRDM-KL25Zand Xsens MTi-3
Dependencies: mbed mbed-rtos Xbus
Diff: main.cpp
- Revision:
- 64:8a0f00a064bb
- Parent:
- 63:138c196f0b88
- Child:
- 65:38c908d1b515
--- a/main.cpp Fri Jun 26 08:04:36 2015 +0000 +++ b/main.cpp Fri Oct 02 16:24:06 2015 +0200 @@ -21,28 +21,41 @@ * * The firmware uses the mbed-rtos library to provide RTOS features such as * memory pools and queues. A single thread (main) is used with reception of - * data from the motion tracker via a UART handled by interrupts. + * data from the motion tracker. * * \section Hardware setup * The firmware has been tested with a ST Nucleo F302R8 development board. * The Nucleo board should be connected to the MTi1 development board using the * Arduino compatible headers on the Nucleo board as follows: * - * | Nucleo pin | MTi1 func. | MTi1 dev. pin | - * |------------|-------------|---------------| - * | 5V | VDD | P300-1 | - * | IORef | VDDIO | P300-2 | - * | GND | GND | P300-3 | - * | D2 | nRST | P300-5 | - * | SCL/D15 | UART_TX | P300-9 | - * | SDA/D14 | UART_RX | P300-11 | + * | Nucleo pin | MTi1 func. | MTi1 dev. pin | Used for PSEL | + * |------------|-------------|---------------|---------------| + * | 5V | VDD | P300-1 | Any | + * | IORef | VDDIO | P300-2 | Any | + * | GND | GND | P300-3 | Any | + * | D2 | nRST | P300-5 | Any | + * | SCL/D15 | UART_TX/SCL | P300-9 | UART / I2C | + * | SDA/D14 | UART_RX/SDA | P300-11 | UART / I2C | + * | D3 | DRDY | P300-15 | SPI / I2C | + * | SCK/D13 | SCK/ADD0 | P300-17 | SPI / I2C | + * | MISO/D12 | MISO/ADD1 | P300-19 | SPI / I2C | + * | MOSI/D11 | MOSI/ADD2 | P300-21 | SPI / I2C | + * | CS/D10 | nCS | P300-23 | SPI | * * Communication with the host PC is achieved using the built-in USB serial - * bridge of the Nucleo board. + * bridge of the Nucleo board. Communication with the MT is achieved through + * either the UART, I2C or SPI interface. The active interface is chosen + * on the MT's side by use of the PSEL0 and PSEL1 switch on the MTi1 + * development board. This example needs to be built with the matching + * MTI_USES_xxxx_INTERFACE define set (see below) * * \subsection Porting - * To port to a different mbed platform only the serial Rx/Tx lines and the - * reset line pins should need to be updated. + * To port to a different mbed platform the following pin definitions need + * to be updated. + * In all cases: the reset line pin + * For UART: the serial Rx/Tx lines UART_TX and UART_RX + * For I2C: the SCL,SDA,DRDY and address lines + * For SPI: The SCK,MISO,MOSI,nCS and DRDY lines * * \section Firmware Operation * The firmware starts by initializing the serial ports used to communicate @@ -66,20 +79,52 @@ #include "xbusparser.h" #include "xbusmessage.h" #include "xsdeviceid.h" +#include "xbusdef.h" + +// Select communication interface to use for MTi +#define MTI_USES_I2C_INTERFACE + +#if !(defined(MTI_USES_I2C_INTERFACE) || defined(MTI_USES_SPI_INTERFACE) || defined(MTI_USES_UART_INTERFACE)) +#error "Must select communication interface by defining one of: MTI_USES_I2C_INTERFACE, MTI_USES_SPI_INTERFACE or MTI_USES_UART_INTERFACE" +#endif #if defined(TARGET_NUCLEO_F302R8) + #define PC_TX PA_2 #define PC_RX PA_3 #define MT_TX PB_9 #define MT_RX PB_8 +#define MT_SDA PB_9 +#define MT_SCL PB_8 +#define MT_ADD0 PB_13 +#define MT_ADD1 PB_14 +#define MT_ADD2 PB_15 +#define MT_MOSI PB_15 +#define MT_MISO PB_14 +#define MT_SCLK PB_13 +#define MT_nCS PB_6 #define MT_NRESET PA_10 +#define MT_DRDY PB_3 + #elif defined(TARGET_KL46Z) + +#if !defined(MTI_USES_UART_INTERFACE) +#error "Support for I2C/SPI has not been added for this platform." +#endif + #define PC_TX USBTX #define PC_RX USBRX #define MT_TX PTE0 #define MT_RX PTE1 #define MT_NRESET PTD3 + #elif defined(TARGET_LPC4088) + +#if !defined(MTI_USES_UART_INTERFACE) +#error "Support for I2C/SPI has not been added for this platform." +#endif + + #define PC_TX USBTX #define PC_RX USBRX #define MT_TX P0_25 @@ -121,6 +166,24 @@ /*! \brief Serial port for communication with the host PC. */ static Serial pc(PC_TX, PC_RX); + +#if defined(MTI_USES_I2C_INTERFACE) +/*! + * \brief I2C master used for communication with the MT. + */ +static I2C mt(MT_SDA, MT_SCL); +static DigitalOut add0(MT_ADD0); +static DigitalOut add1(MT_ADD1); +static DigitalOut add2(MT_ADD2); + +#elif defined(MTI_USES_SPI_INTERFACE) +/*! \brief SPI master used for communication with the MT. */ +static SPI mt(MT_MOSI, MT_MISO, MT_SCLK); + +/*! \brief Chip select line for the MT. */ +static DigitalOut cs(MT_nCS, 1); + +#elif defined(MTI_USES_UART_INTERFACE) /*! * \brief Serial port for communication with the MT. * @@ -128,6 +191,15 @@ * Serial class can have problems with the RTOS when using interrupts. */ static RawSerial mt(MT_TX, MT_RX); +#endif + +#if defined(MTI_USES_I2C_INTERFACE) || defined(MTI_USES_SPI_INTERFACE) +/*! + * \brief Interrput line used by MT to signal that data is available. + */ +static InterruptIn drdy(MT_DRDY); +#endif + /*! * \brief MT reset line. * @@ -172,6 +244,153 @@ g_messageDataPool.free((uint8_t(*)[MAX_XBUS_DATA_SIZE])buffer); } +#if defined(MTI_USES_I2C_INTERFACE) +#define MTI_I2C_ADDRESS (0x1D << 1) +static void readData(uint8_t pipe, uint16_t dataLength) +{ + const int preambleLength = 2; + uint8_t* buf = (uint8_t*)allocateMessageData(dataLength+preambleLength); + if (buf) + { + buf[0] = XBUS_PREAMBLE; + buf[1] = XBUS_MASTERDEVICE; + mt.write(MTI_I2C_ADDRESS, (char*)&pipe, sizeof(pipe), true); + mt.read(MTI_I2C_ADDRESS, (char*)buf+preambleLength, dataLength); + XbusParser_parseBuffer(xbusParser, buf, dataLength+preambleLength); + deallocateMessageData(buf); + } +} +static void mtInterruptHandler(void) +{ + while (true) + { + uint8_t opcode = XBUS_PIPE_STATUS; + uint8_t status[4]; + mt.write(MTI_I2C_ADDRESS, (char*)&opcode, sizeof(opcode), true); + mt.read(MTI_I2C_ADDRESS, (char*)status, sizeof(status)); + + uint16_t notificationSize = status[0] | (status[1] << 8); + uint16_t measurementSize = status[2] | (status[3] <<8); + + if (notificationSize) + { + readData(XBUS_NOTIFICATION_PIPE, notificationSize); + } + else if (measurementSize) + { + readData(XBUS_MEASUREMENT_PIPE, measurementSize); + } + else + break; // No more data available to read. + } +} + +static void configureMtCommunicationInterface(void) +{ + mt.frequency(400000); + //Use the addX pins to configure I2C address 0x1D + add0.write(0); + add1.write(0); + add2.write(0); + drdy.rise(&mtInterruptHandler); +} + +/*! + * \brief Send a message to the MT + * + * This function formats the message data and writes this to the MT I2C + * interface. It does not wait for any response. + */ +static void sendMessage(XbusMessage const* m) +{ + uint8_t buf[64]; + size_t rawLength = XbusMessage_format(buf, m, XLLF_I2c); + mt.write(MTI_I2C_ADDRESS, (char*)buf, rawLength); +} +#elif defined(MTI_USES_SPI_INTERFACE) +static void sendOpcode(uint8_t opcode) +{ + mt.write(opcode); + for (int filler = 0; filler < 3; ++filler) + { + mt.write(filler); + } +} + +static void readData(uint8_t pipe, uint16_t dataLength) +{ + const int preambleLength = 2; + uint8_t* buf = (uint8_t*)allocateMessageData(dataLength+preambleLength); + if (buf) + { + uint8_t* dptr = buf; + *dptr++ = XBUS_PREAMBLE; + *dptr++ = XBUS_MASTERDEVICE; + cs = 0; + sendOpcode(pipe); + for (int i = 0; i < dataLength; ++i) + { + *dptr++ = mt.write(0); + } + cs = 1; + XbusParser_parseBuffer(xbusParser, buf, dptr - buf); + deallocateMessageData(buf); + } +} +static void mtInterruptHandler(void) +{ + while (true) + { + cs = 0; + sendOpcode(XBUS_PIPE_STATUS); + uint8_t status[4]; + for (int i = 0; i < sizeof(status); ++i) + { + status[i] = mt.write(0); + } + cs = 1; + + uint16_t notificationSize = status[0] | (status[1] << 8); + uint16_t measurementSize = status[2] | (status[3] <<8); + + if (notificationSize) + { + readData(XBUS_NOTIFICATION_PIPE, notificationSize); + } + else if (measurementSize) + { + readData(XBUS_MEASUREMENT_PIPE, measurementSize); + } + else + break; // No more data available to read. + } +} + +static void configureMtCommunicationInterface(void) +{ + mt.frequency(1000000); + mt.format(8, 3); + drdy.rise(&mtInterruptHandler); +} + +/*! + * \brief Send a message to the MT + * + * This function formats the message data and writes this to the MT SPI + * interface. It does not wait for any response. + */ +static void sendMessage(XbusMessage const* m) +{ + uint8_t buf[64]; + size_t rawLength = XbusMessage_format(buf, m, XLLF_Spi); + cs = 0; + for (int i = 0; i < rawLength; ++i) + { + mt.write(buf[i]); + } + cs = 1; +} +#elif defined(MTI_USES_UART_INTERFACE) /*! * \brief RX Interrupt handler for the MT serial port. * @@ -186,6 +405,17 @@ } /*! + * \brief Configure the serial port used for communication with the + * motion tracker. + */ +static void configureMtCommunicationInterface(void) +{ + mt.baud(115200); + mt.format(8, Serial::None, 1); + mt.attach(mtLowLevelHandler, Serial::RxIrq); +} + +/*! * \brief Send a message to the MT * * This function formats the message data and writes this to the MT serial @@ -194,12 +424,14 @@ static void sendMessage(XbusMessage const* m) { uint8_t buf[64]; - size_t rawLength = XbusMessage_format(buf, m); + size_t rawLength = XbusMessage_format(buf, m, XLLF_Uart); for (size_t i = 0; i < rawLength; ++i) { mt.putc(buf[i]); } } +#endif + /*! * \brief Send a message to the MT and wait for a response. @@ -340,17 +572,12 @@ } /*! - * \brief Configure the serial ports used to communicate with the motion - * tracker and host PC. + * \brief Configure the serial port used to communicate with the host PC. */ -static void configureSerialPorts(void) +static void configurePcInterface(void) { pc.baud(PC_UART_BAUDRATE); pc.format(8, Serial::None, 1); - - mt.baud(115200); - mt.format(8, Serial::None, 1); - mt.attach(mtLowLevelHandler, Serial::RxIrq); } /*! @@ -502,6 +729,7 @@ pc.printf("Device ready for operation.\r\n"); } +#ifdef MTI_USES_UART_INTERFACE /*! * \brief Restore communication with the MTi. * @@ -525,6 +753,7 @@ sendWakeupAck(); } +#endif /*! * \brief Releases the MTi reset line and waits for a wakeup message. @@ -532,7 +761,7 @@ * If no wakeup message is received within 1 second the restore communications * procedure is done to reset the MTi to default baudrate and output configuration. */ -static void wakeupMotionTracker(void) +static bool wakeupMotionTracker(void) { mtReset.write(1); // Release MT from reset. if (waitForWakeup(1000)) @@ -541,8 +770,14 @@ } else { +#ifdef MTI_USES_UART_INTERFACE restoreCommunication(); +#else + pc.printf("Failed to communicate with MTi device\r\n"); + return true; +#endif } + return true; } static void printIntroMessage(void) @@ -609,32 +844,35 @@ xbusCallback.handleMessage = mtMessageHandler; xbusParser = XbusParser_create(&xbusCallback); - configureSerialPorts(); + configurePcInterface(); + configureMtCommunicationInterface(); printIntroMessage(); - wakeupMotionTracker(); - if (configureMotionTracker()) + if (wakeupMotionTracker()) { - printUsageInstructions(); - for (;;) + if (configureMotionTracker()) { - while (pc.readable()) + printUsageInstructions(); + for (;;) { - handlePcCommand(pc.getc()); - } + while (pc.readable()) + { + handlePcCommand(pc.getc()); + } - osEvent ev = g_dataQueue.get(10); - if (ev.status == osEventMessage) - { - XbusMessage const* data = (XbusMessage const*)ev.value.p; - XbusMessageMemoryManager janitor(data); - printMessageData(data); + osEvent ev = g_dataQueue.get(10); + if (ev.status == osEventMessage) + { + XbusMessage const* data = (XbusMessage const*)ev.value.p; + XbusMessageMemoryManager janitor(data); + printMessageData(data); + } } } - } - else - { - pc.printf("Failed to configure motion tracker.\r\n"); - return -1; + else + { + pc.printf("Failed to configure motion tracker.\r\n"); + return -1; + } } }