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