Revision 0:99928431bb44, committed 2015-07-07

Comitter:

millanea

Date:

Tue Jul 07 09:36:12 2015 +0000

Commit message:

First commit. Committing the entire project such that it can be published.

Changed in this revision

diff -r 000000000000 -r 99928431bb44 DW1000/DW1000.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DW1000/DW1000.cpp	Tue Jul 07 09:36:12 2015 +0000
@@ -0,0 +1,282 @@
+#include "DW1000.h"
+
+DW1000::DW1000(PinName MOSI, PinName MISO, PinName SCLK, PinName CS, PinName IRQ) : irq(IRQ), spi(MOSI, MISO, SCLK), cs(CS) {
+    setCallbacks(NULL, NULL);
+    
+    deselect();                         // Chip must be deselected first
+    spi.format(8,0);                    // Setup the spi for standard 8 bit data and SPI-Mode 0 (GPIO5, GPIO6 open circuit or ground on DW1000)
+    spi.frequency(5000000);             // with a 1MHz clock rate (worked up to 49MHz in our Test)
+    
+    resetAll();                         // we do a soft reset of the DW1000 everytime the driver starts
+
+    // Configuration TODO: make method for that
+    // User Manual "2.5.5 Default Configurations that should be modified" p. 22
+    //Those values are for the standard mode (6.8Mbps, 5, 16Mhz, 32 Symbols) and are INCOMPLETE!
+//    writeRegister16(DW1000_AGC_CTRL, 0x04, 0x8870);
+//    writeRegister32(DW1000_AGC_CTRL, 0x0C, 0x2502A907);
+//    writeRegister32(DW1000_DRX_CONF, 0x08, 0x311A002D);
+//    writeRegister8 (DW1000_LDE_CTRL, 0x0806, 0xD);
+//    writeRegister16(DW1000_LDE_CTRL, 0x1806, 0x1607);
+//    writeRegister32(DW1000_TX_POWER, 0, 0x0E082848);
+//    writeRegister32(DW1000_RF_CONF, 0x0C, 0x001E3FE0);
+//    writeRegister8 (DW1000_TX_CAL, 0x0B, 0xC0);
+//    writeRegister8 (DW1000_FS_CTRL, 0x0B, 0xA6);
+
+
+    //Those values are for the 110kbps mode (5, 16MHz, 1024 Symbols) and are quite complete
+    writeRegister16(DW1000_AGC_CTRL, 0x04, 0x8870);             //AGC_TUNE1 for 16MHz PRF
+    writeRegister32(DW1000_AGC_CTRL, 0x0C, 0x2502A907);         //AGC_TUNE2 (Universal)
+    writeRegister16(DW1000_AGC_CTRL, 0x12, 0x0055);             //AGC_TUNE3 (Universal)
+
+    writeRegister16(DW1000_DRX_CONF, 0x02, 0x000A);             //DRX_TUNE0b for 110kbps
+    writeRegister16(DW1000_DRX_CONF, 0x04, 0x0087);             //DRX_TUNE1a for 16MHz PRF
+    writeRegister16(DW1000_DRX_CONF, 0x06, 0x0064);             //DRX_TUNE1b for 110kbps & > 1024 symbols
+    writeRegister32(DW1000_DRX_CONF, 0x08, 0x351A009A);         //PAC size for 1024 symbols preamble & 16MHz PRF
+    //writeRegister32(DW1000_DRX_CONF, 0x08, 0x371A011D);               //PAC size for 2048 symbols preamble
+
+    writeRegister8 (DW1000_LDE_CTRL, 0x0806, 0xD);              //LDE_CFG1
+    writeRegister16(DW1000_LDE_CTRL, 0x1806, 0x1607);           //LDE_CFG2 for 16MHz PRF
+
+    writeRegister32(DW1000_TX_POWER, 0, 0x28282828);            //Power for channel 5
+
+    writeRegister8(DW1000_RF_CONF, 0x0B, 0xD8);                 //RF_RXCTRLH for channel 5
+    writeRegister32(DW1000_RF_CONF, 0x0C, 0x001E3FE0);          //RF_TXCTRL for channel 5
+
+    writeRegister8 (DW1000_TX_CAL, 0x0B, 0xC0);                 //TC_PGDELAY for channel 5
+
+    writeRegister32 (DW1000_FS_CTRL, 0x07, 0x0800041D);         //FS_PLLCFG for channel 5
+    writeRegister8 (DW1000_FS_CTRL, 0x0B, 0xA6);                //FS_PLLTUNE for channel 5
+
+    loadLDE();                          // important everytime DW1000 initialises/awakes otherwise the LDE algorithm must be turned off or there's receiving malfunction see User Manual LDELOAD on p22 & p158
+    
+    // 110kbps CAUTION: a lot of other registers have to be set for an optimized operation on 110kbps
+    writeRegister16(DW1000_TX_FCTRL, 1, 0x0800 | 0x0100 | 0x0080); // use 1024 symbols preamble (0x0800) (previously 2048 - 0x2800), 16MHz pulse repetition frequency (0x0100), 110kbps bit rate (0x0080) see p.69 of DW1000 User Manual
+    writeRegister8(DW1000_SYS_CFG, 2, 0x44);    // enable special receiving option for 110kbps (disable smartTxPower)!! (0x44) see p.64 of DW1000 User Manual [DO NOT enable 1024 byte frames (0x03) becuase it generates disturbance of ranging don't know why...]
+
+    writeRegister16(DW1000_TX_ANTD, 0, 16384); // set TX and RX Antenna delay to neutral because we calibrate afterwards
+    writeRegister16(DW1000_LDE_CTRL, 0x1804, 16384); // = 2^14 a quarter of the range of the 16-Bit register which corresponds to zero calibration in a round trip (TX1+RX2+TX2+RX1)
+
+    writeRegister8(DW1000_SYS_CFG, 3, 0x20);    // enable auto reenabling receiver after error
+    
+    irq.rise(this, &DW1000::ISR);       // attach interrupt handler to rising edge of interrupt pin from DW1000
+}
+
+void DW1000::setCallbacks(void (*callbackRX)(void), void (*callbackTX)(void)) {
+    bool RX = false;
+    bool TX = false;
+    if (callbackRX) {
+        DW1000::callbackRX.attach(callbackRX);
+        RX = true;
+    }
+    if (callbackTX) {
+        DW1000::callbackTX.attach(callbackTX);
+        TX = true;
+    }
+    setInterrupt(RX,TX);
+}
+
+uint32_t DW1000::getDeviceID() {
+    uint32_t result;
+    readRegister(DW1000_DEV_ID, 0, (uint8_t*)&result, 4);
+    return result;
+}
+
+uint64_t DW1000::getEUI() {
+    uint64_t result;
+    readRegister(DW1000_EUI, 0, (uint8_t*)&result, 8);
+    return result;
+}
+
+void DW1000::setEUI(uint64_t EUI) {
+    writeRegister(DW1000_EUI, 0, (uint8_t*)&EUI, 8);
+}
+
+float DW1000::getVoltage() {
+    uint8_t buffer[7] = {0x80, 0x0A, 0x0F, 0x01, 0x00};             // algorithm form User Manual p57
+    writeRegister(DW1000_RF_CONF, 0x11, buffer, 2);
+    writeRegister(DW1000_RF_CONF, 0x12, &buffer[2], 1);
+    writeRegister(DW1000_TX_CAL, 0x00, &buffer[3], 1);
+    writeRegister(DW1000_TX_CAL, 0x00, &buffer[4], 1);
+    readRegister(DW1000_TX_CAL, 0x03, &buffer[5], 2);               // get the 8-Bit readings for Voltage and Temperature
+    float Voltage = buffer[5] * 0.0057 + 2.3;
+    //float Temperature = buffer[6] * 1.13 - 113.0;                 // TODO: getTemperature was always ~35 degree with better formula/calibration
+    return Voltage;
+}
+
+uint64_t DW1000::getStatus() {
+    return readRegister40(DW1000_SYS_STATUS, 0);
+}
+
+uint64_t DW1000::getRXTimestamp() {
+    return readRegister40(DW1000_RX_TIME, 0);
+}
+
+uint64_t DW1000::getTXTimestamp() {
+    return readRegister40(DW1000_TX_TIME, 0);
+}
+
+void DW1000::sendString(char* message) {
+    sendFrame((uint8_t*)message, strlen(message)+1);
+}
+
+void DW1000::receiveString(char* message) {
+    readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)message, getFramelength());  // get data from buffer
+}
+
+void DW1000::sendFrame(uint8_t* message, uint16_t length) {
+    //if (length >= 1021) length = 1021;                            // check for maximim length a frame can have with 1024 Byte frames [not used, see constructor]
+    if (length >= 125) length = 125;                                // check for maximim length a frame can have with 127 Byte frames
+    writeRegister(DW1000_TX_BUFFER, 0, message, length);            // fill buffer
+    
+    uint8_t backup = readRegister8(DW1000_TX_FCTRL, 1);             // put length of frame
+    length += 2;                                                    // including 2 CRC Bytes
+    length = ((backup & 0xFC) << 8) | (length & 0x03FF);
+    writeRegister16(DW1000_TX_FCTRL, 0, length);
+    
+    stopTRX();                                                      // stop receiving
+    writeRegister8(DW1000_SYS_CTRL, 0, 0x02);                       // trigger sending process by setting the TXSTRT bit
+    startRX();                                                      // enable receiver again
+}
+
+void DW1000::sendDelayedFrame(uint8_t* message, uint16_t length, uint64_t TxTimestamp) {
+    //if (length >= 1021) length = 1021;                            // check for maximim length a frame can have with 1024 Byte frames [not used, see constructor]
+    if (length >= 125) length = 125;                                // check for maximim length a frame can have with 127 Byte frames
+    writeRegister(DW1000_TX_BUFFER, 0, message, length);            // fill buffer
+
+    uint8_t backup = readRegister8(DW1000_TX_FCTRL, 1);             // put length of frame
+    length += 2;                                                    // including 2 CRC Bytes
+    length = ((backup & 0xFC) << 8) | (length & 0x03FF);
+    writeRegister16(DW1000_TX_FCTRL, 0, length);
+
+    writeRegister40(DW1000_DX_TIME, 0, TxTimestamp);                //write the timestamp on which to send the message
+
+    stopTRX();                                                      // stop receiving
+    writeRegister8(DW1000_SYS_CTRL, 0, 0x02 | 0x04);                // trigger sending process by setting the TXSTRT and TXDLYS bit
+    startRX();                                                      // enable receiver again
+}
+
+void DW1000::startRX() {
+    writeRegister8(DW1000_SYS_CTRL, 0x01, 0x01);                    // start listening for preamble by setting the RXENAB bit
+}
+
+void DW1000::stopTRX() {
+    writeRegister8(DW1000_SYS_CTRL, 0, 0x40);                       // disable tranceiver go back to idle mode
+}
+
+// PRIVATE Methods ------------------------------------------------------------------------------------
+void DW1000::loadLDE() {                                            // initialise LDE algorithm LDELOAD User Manual p22
+    writeRegister16(DW1000_PMSC, 0, 0x0301);                        // set clock to XTAL so OTP is reliable
+    writeRegister16(DW1000_OTP_IF, 0x06, 0x8000);                   // set LDELOAD bit in OTP
+    wait_us(150);
+    writeRegister16(DW1000_PMSC, 0, 0x0200);                        // recover to PLL clock
+}
+
+void DW1000::resetRX() {    
+    writeRegister8(DW1000_PMSC, 3, 0xE0);   // set RX reset
+    writeRegister8(DW1000_PMSC, 3, 0xF0);   // clear RX reset
+}
+
+void DW1000::resetAll() {
+    writeRegister8(DW1000_PMSC, 0, 0x01);   // set clock to XTAL
+    writeRegister8(DW1000_PMSC, 3, 0x00);   // set All reset
+    wait_us(10);                            // wait for PLL to lock
+    writeRegister8(DW1000_PMSC, 3, 0xF0);   // clear All reset
+}
+
+
+void DW1000::setInterrupt(bool RX, bool TX) {
+    writeRegister16(DW1000_SYS_MASK, 0, RX*0x4000 | TX*0x0080);  // RX good frame 0x4000, TX done 0x0080
+}
+
+void DW1000::ISR() {
+    uint64_t status = getStatus();
+    if (status & 0x4000) {                                          // a frame was received
+        callbackRX.call();
+        writeRegister16(DW1000_SYS_STATUS, 0, 0x6F00);              // clearing of receiving status bits
+    }
+    if (status & 0x80) {                                            // sending complete
+        callbackTX.call();
+        writeRegister8(DW1000_SYS_STATUS, 0, 0xF8);                 // clearing of sending status bits
+    }
+}
+
+uint16_t DW1000::getFramelength() {
+    uint16_t framelength = readRegister16(DW1000_RX_FINFO, 0);      // get framelength
+    framelength = (framelength & 0x03FF) - 2;                       // take only the right bits and subtract the 2 CRC Bytes
+    return framelength;
+}
+
+// SPI Interface ------------------------------------------------------------------------------------
+uint8_t DW1000::readRegister8(uint8_t reg, uint16_t subaddress) {
+    uint8_t result;
+    readRegister(reg, subaddress, &result, 1);
+    return result;
+}
+
+uint16_t DW1000::readRegister16(uint8_t reg, uint16_t subaddress) {
+    uint16_t result;
+    readRegister(reg, subaddress, (uint8_t*)&result, 2);
+    return result;
+}
+
+uint64_t DW1000::readRegister40(uint8_t reg, uint16_t subaddress) {
+    uint64_t result;
+    readRegister(reg, subaddress, (uint8_t*)&result, 5);
+    result &= 0xFFFFFFFFFF;                                 // only 40-Bit
+    return result;
+}
+
+void DW1000::writeRegister8(uint8_t reg, uint16_t subaddress, uint8_t buffer) {
+    writeRegister(reg, subaddress, &buffer, 1);
+}
+
+void DW1000::writeRegister16(uint8_t reg, uint16_t subaddress, uint16_t buffer) {
+    writeRegister(reg, subaddress, (uint8_t*)&buffer, 2);
+}
+
+void DW1000::writeRegister32(uint8_t reg, uint16_t subaddress, uint32_t buffer) {
+    writeRegister(reg, subaddress, (uint8_t*)&buffer, 4);
+}
+
+void DW1000::writeRegister40(uint8_t reg, uint16_t subaddress, uint64_t buffer) {
+    writeRegister(reg, subaddress, (uint8_t*)&buffer, 5);
+}
+
+void DW1000::readRegister(uint8_t reg, uint16_t subaddress, uint8_t *buffer, int length) {
+    setupTransaction(reg, subaddress, false);
+    for(int i=0; i<length; i++)                             // get data
+        buffer[i] = spi.write(0x00);
+    deselect();
+}
+
+void DW1000::writeRegister(uint8_t reg, uint16_t subaddress, uint8_t *buffer, int length) {
+    setupTransaction(reg, subaddress, true);
+    for(int i=0; i<length; i++)                             // put data
+        spi.write(buffer[i]);
+    deselect();
+}
+
+void DW1000::setupTransaction(uint8_t reg, uint16_t subaddress, bool write) {
+    reg |=  (write * DW1000_WRITE_FLAG);                                        // set read/write flag
+    select();
+    if (subaddress > 0) {                                                       // there's a subadress, we need to set flag and send second header byte
+        spi.write(reg | DW1000_SUBADDRESS_FLAG);
+        if (subaddress > 0x7F) {                                                // sub address too long, we need to set flag and send third header byte
+            spi.write((uint8_t)(subaddress & 0x7F) | DW1000_2_SUBADDRESS_FLAG); // and 
+            spi.write((uint8_t)(subaddress >> 7));
+        } else {
+            spi.write((uint8_t)subaddress);
+        }
+    } else {
+        spi.write(reg);                                                         // say which register address we want to access
+    }
+}
+
+void DW1000::select() {     // always called to start an SPI transmission
+    irq.disable_irq();      // disable interrupts from DW1000 during SPI becaus this leads to crashes!      TODO: if you have other interrupt handlers attached on the micro controller, they could also interfere.
+    cs = 0;                 // set Cable Select pin low to start transmission
+}
+void DW1000::deselect() {   // always called to end an SPI transmission
+    cs = 1;                 // set Cable Select pin high to stop transmission
+    irq.enable_irq();       // reenable the interrupt handler
+}

diff -r 000000000000 -r 99928431bb44 DW1000/DW1000.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DW1000/DW1000.h	Tue Jul 07 09:36:12 2015 +0000
@@ -0,0 +1,113 @@
+// by Matthias Grob & Manuel Stalder - ETH Zürich - 2015
+
+#ifndef DW1000_H
+#define DW1000_H
+
+#include "mbed.h"
+
+// register addresses
+//      Mnemonic                    Address Bytes Description
+#define DW1000_DEV_ID               0x00 //     4 Device Identifier – includes device type and revision information
+#define DW1000_EUI                  0x01 //     8 Extended Unique Identifier
+#define DW1000_PANADR               0x03 //     4 PAN Identifier and Short Address
+#define DW1000_SYS_CFG              0x04 //     4 System Configuration bitmap
+#define DW1000_SYS_TIME             0x06 //     5 System Time Counter (40-bit)
+#define DW1000_TX_FCTRL             0x08 //     5 Transmit Frame Control
+#define DW1000_TX_BUFFER            0x09 //  1024 Transmit Data Buffer
+#define DW1000_DX_TIME              0x0A //     5 Delayed Send or Receive Time (40-bit)
+#define DW1000_RX_FWTO              0x0C //     2 Receive Frame Wait Timeout Period
+#define DW1000_SYS_CTRL             0x0D //     4 System Control Register
+#define DW1000_SYS_MASK             0x0E //     4 System Event Mask Register
+#define DW1000_SYS_STATUS           0x0F //     5 System Event Status Register
+#define DW1000_RX_FINFO             0x10 //     4 RX Frame Information                (in double buffer set)
+#define DW1000_RX_BUFFER            0x11 //  1024 Receive Data Buffer                 (in double buffer set)
+#define DW1000_RX_FQUAL             0x12 //     8 Rx Frame Quality information        (in double buffer set)
+#define DW1000_RX_TTCKI             0x13 //     4 Receiver Time Tracking Interval     (in double buffer set)
+#define DW1000_RX_TTCKO             0x14 //     5 Receiver Time Tracking Offset       (in double buffer set)
+#define DW1000_RX_TIME              0x15 //    14 Receive Message Time of Arrival     (in double buffer set)
+#define DW1000_TX_TIME              0x17 //    10 Transmit Message Time of Sending    (in double buffer set)
+#define DW1000_TX_ANTD              0x18 //     2 16-bit Delay from Transmit to Antenna
+#define DW1000_SYS_STATE            0x19 //     5 System State information
+#define DW1000_ACK_RESP_T           0x1A //     4 Acknowledgement Time and Response Time
+#define DW1000_RX_SNIFF             0x1D //     4 Pulsed Preamble Reception Configuration
+#define DW1000_TX_POWER             0x1E //     4 TX Power Control
+#define DW1000_CHAN_CTRL            0x1F //     4 Channel Control
+#define DW1000_USR_SFD              0x21 //    41 User-specified short/long TX/RX SFD sequences
+#define DW1000_AGC_CTRL             0x23 //    32 Automatic Gain Control configuration
+#define DW1000_EXT_SYNC             0x24 //    12 External synchronisation control.
+#define DW1000_ACC_MEM              0x25 //  4064 Read access to accumulator data
+#define DW1000_GPIO_CTRL            0x26 //    44 Peripheral register bus 1 access - GPIO control
+#define DW1000_DRX_CONF             0x27 //    44 Digital Receiver configuration
+#define DW1000_RF_CONF              0x28 //    58 Analog RF Configuration
+#define DW1000_TX_CAL               0x2A //    52 Transmitter calibration block
+#define DW1000_FS_CTRL              0x2B //    21 Frequency synthesiser control block
+#define DW1000_AON                  0x2C //    12 Always-On register set
+#define DW1000_OTP_IF               0x2D //    18 One Time Programmable Memory Interface
+#define DW1000_LDE_CTRL             0x2E //     - Leading edge detection control block
+#define DW1000_DIG_DIAG             0x2F //    41 Digital Diagnostics Interface
+#define DW1000_PMSC                 0x36 //    48 Power Management System Control Block
+
+#define DW1000_WRITE_FLAG           0x80 // First Bit of the address has to be 1 to indicate we want to write
+#define DW1000_SUBADDRESS_FLAG      0x40 // if we have a sub address second Bit has to be 1
+#define DW1000_2_SUBADDRESS_FLAG    0x80 // if we have a long sub adress (more than 7 Bit) we set this Bit in the first part
+
+class DW1000 {
+    public:            
+        DW1000(PinName MOSI, PinName MISO, PinName SCLK, PinName CS, PinName IRQ);              // constructor, uses SPI class
+        void setCallbacks(void (*callbackRX)(void), void (*callbackTX)(void));                  // setter for callback functions, automatically enables interrupt, if NULL is passed the coresponding interrupt gets disabled
+        template<typename T>
+            void setCallbacks(T* tptr, void (T::*mptrRX)(void), void (T::*mptrTX)(void)) {      // overloaded setter to treat member function pointers of objects
+            callbackRX.attach(tptr, mptrRX);                                                    // possible client code: dw.setCallbacks(this, &A::callbackRX, &A::callbackTX);
+            callbackTX.attach(tptr, mptrTX);                                                    // concept seen in line 100 of http://developer.mbed.org/users/mbed_official/code/mbed/docs/4fc01daae5a5/InterruptIn_8h_source.html
+            setInterrupt(true,true);
+        }
+
+        // Device API
+        uint32_t getDeviceID();                                                                 // gets the Device ID which should be 0xDECA0130 (good for testing SPI!)
+        uint64_t getEUI();                                                                      // gets 64 bit Extended Unique Identifier according to IEEE standard
+        void setEUI(uint64_t EUI);                                                              // sets 64 bit Extended Unique Identifier according to IEEE standard
+        float getVoltage();                                                                     // gets the current chip voltage measurement form the A/D converter
+        uint64_t getStatus();                                                                   // get the 40 bit device status
+        uint64_t getRXTimestamp();
+        uint64_t getTXTimestamp();
+        
+        void sendString(char* message);                                                         // to send String with arbitrary length
+        void receiveString(char* message);                                                      // to receive char string (length of the buffer must be 1021 to be safe)
+        void sendFrame(uint8_t* message, uint16_t length);                                      // send a raw frame (length in bytes)
+        void sendDelayedFrame(uint8_t* message, uint16_t length, uint64_t TxTimestamp);
+        void startRX();                                                                         // start listening for frames
+        void stopTRX();                                                                         // disable tranceiver go back to idle mode
+        
+    //private:
+        void loadLDE();                                                                         // load the leading edge detection algorithm to RAM, [IMPORTANT because receiving malfunction may occur] see User Manual LDELOAD on p22 & p158
+        void resetRX();                                                                         // soft reset only the tranciever part of DW1000
+        void resetAll();                                                                        // soft reset the entire DW1000 (some registers stay as they were see User Manual)
+
+        // Interrupt
+        InterruptIn irq;                                                                        // Pin used to handle Events from DW1000 by an Interrupthandler
+        FunctionPointer callbackRX;                                                             // function pointer to callback which is called when successfull RX took place
+        FunctionPointer callbackTX;                                                             // function pointer to callback which is called when successfull TX took place
+        void setInterrupt(bool RX, bool TX);                                                    // set Interrupt for received a good frame (CRC ok) or transmission done
+        void ISR();                                                                             // interrupt handling method (also calls according callback methods)
+        uint16_t getFramelength();                                                              // to get the framelength of the received frame from the PHY header
+        
+        // SPI Inteface
+        SPI spi;                                                                                // SPI Bus
+        DigitalOut cs;                                                                          // Slave selector for SPI-Bus (here explicitly needed to start and end SPI transactions also usable to wake up DW1000)
+        
+        uint8_t readRegister8(uint8_t reg, uint16_t subaddress);                                // expressive methods to read or write the number of bits written in the name
+        uint16_t readRegister16(uint8_t reg, uint16_t subaddress);
+        uint64_t readRegister40(uint8_t reg, uint16_t subaddress);
+        void writeRegister8(uint8_t reg, uint16_t subaddress, uint8_t buffer);
+        void writeRegister16(uint8_t reg, uint16_t subaddress, uint16_t buffer);
+        void writeRegister32(uint8_t reg, uint16_t subaddress, uint32_t buffer);
+        void writeRegister40(uint8_t reg, uint16_t subaddress, uint64_t buffer);
+
+        void readRegister(uint8_t reg, uint16_t subaddress, uint8_t *buffer, int length);       // reads the selected part of a slave register into the buffer memory
+        void writeRegister(uint8_t reg, uint16_t subaddress, uint8_t *buffer, int length);      // writes the buffer memory to the selected slave register
+        void setupTransaction(uint8_t reg, uint16_t subaddress, bool write);                    // sets up an SPI read or write transaction with correct register address and offset
+        void select();                                                                          // selects the only slave for a transaction
+        void deselect();                                                                        // deselects the only slave after transaction
+};
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r 99928431bb44 Debug/debug.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Debug/debug.h	Tue Jul 07 09:36:12 2015 +0000
@@ -0,0 +1,23 @@
+
+#ifndef DEBUG_H
+#define DEBUG_H
+
+#include "mbed.h"
+
+// The debug serial connection
+extern Serial serialUSB ;
+
+// Serial debug level
+#define DEBUG_LEVEL 0   // 0 - NO DEBUG
+                        // 1 - DEBUG - SERIAL USB
+
+// Debug print defines
+#if DEBUG_LEVEL == 0
+    #define debugprintf(format, args...)
+#elif DEBUG_LEVEL == 1
+    #define debugprintf(format, args...) serialUSB.printf (format , ##args)
+#endif
+
+
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r 99928431bb44 MM2WayRanging/MM2WayRanging.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MM2WayRanging/MM2WayRanging.cpp	Tue Jul 07 09:36:12 2015 +0000
@@ -0,0 +1,178 @@
+#include "MM2WayRanging.h"
+
+MM2WayRanging::MM2WayRanging(DW1000& DW) : dw(DW) {
+    overflow = false;
+    address = 0;
+    rxTimestamp = 0;
+    timediffRec = 0;
+    timediffSend = 0;
+    for (int i = 0; i < 10; i++)
+        acknowledgement[i] = true;
+
+    dw.setCallbacks(this, &MM2WayRanging::callbackRX, &MM2WayRanging::callbackTX);
+
+    LocalTimer.start();
+
+    dw.startRX();
+}
+
+void MM2WayRanging::callbackRX() {
+    dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&receivedFrame, dw.getFramelength());
+
+    if (receivedFrame.destination == address)
+        switch (receivedFrame.type) {
+            case PING:
+                rxTimestamp = dw.getRXTimestamp();
+                receiverTimestamps[receivedFrame.source][0] = rxTimestamp;      //Save the first timestamp on the receiving node/anchor (T_rp)
+                sendDelayedAnswer(receivedFrame.source, ANCHOR_RESPONSE, rxTimestamp);
+                break;
+            case ANCHOR_RESPONSE:
+                rxTimestamp = dw.getRXTimestamp();
+                senderTimestamps[receivedFrame.source][1] = rxTimestamp;        //Save the second timestamp on the sending node/beacon (T_rr)
+                sendDelayedAnswer(receivedFrame.source, 3, rxTimestamp);
+                break;
+            case BEACON_RESPONSE:
+                rxTimestamp = dw.getRXTimestamp();
+                receiverTimestamps[receivedFrame.source][2] = rxTimestamp;      //Save the third timestamp on the receiving node/anchor (T_rf)
+
+                correctReceiverTimestamps(receivedFrame.source);                //Correct the timestamps for the case of a counter overflow
+                //calculation of the summand on the receiving node/anchor
+                timediffRec = - 2*receiverTimestamps[receivedFrame.source][1] + receiverTimestamps[receivedFrame.source][0] + receiverTimestamps[receivedFrame.source][2];
+                sendTransferFrame(receivedFrame.source, timediffRec );
+                break;
+            case TRANSFER_FRAME:
+                //calculation of the summand on the sending node/beacon
+                timediffSend = 2 * senderTimestamps[receivedFrame.source][1] - senderTimestamps[receivedFrame.source][0] - senderTimestamps[receivedFrame.source][2];
+                //calculation of the resulting sum of all four ToFs.
+                tofs[receivedFrame.source] = receivedFrame.signedTime + timediffSend;
+                acknowledgement[receivedFrame.source] = true;
+                break;
+            default : break;
+        }
+
+    dw.startRX();
+}
+
+void MM2WayRanging::callbackTX() {
+    switch (rangingFrame.type) {
+    case PING:
+        senderTimestamps[rangingFrame.destination][0] = dw.getTXTimestamp();    //Save the first timestamp on the sending node/beacon (T_sp)
+        break;
+    case ANCHOR_RESPONSE:
+        receiverTimestamps[rangingFrame.destination][1] = dw.getTXTimestamp();  //Save the second timestamp on the receiving node/anchor (T_sr)
+        break;
+    case BEACON_RESPONSE:
+        senderTimestamps[rangingFrame.destination][2] = dw.getTXTimestamp();    //Save the third timestamp on the sending node/beacon (T_sr)
+        correctSenderTimestamps(rangingFrame.destination);                      //Correct the timestamps for the case of a counter overflow
+        break;
+    default:
+        break;
+    }
+
+}
+
+/**
+ *  Get the distance to the Anchor with address @param destination.
+ *
+ *   @param destination The address of the anchor
+ */
+void MM2WayRanging::requestRanging(uint8_t destination) {
+    acknowledgement[destination] = false;
+    float time_before = LocalTimer.read();
+
+    sendPingFrame(destination);
+
+    // TODO: Here 0.02 seconds is hardcoded as a timeout. Should be added as an argument.
+    while(!acknowledgement[destination] && (LocalTimer.read() < time_before + 0.02f)); // wait for succeeding ranging or timeout
+
+    roundtriptimes[destination] = LocalTimer.read() - time_before;
+
+    if(acknowledgement[destination]){
+    distances[destination] = calibratedDistance(destination);
+    } else {
+        distances[destination] = -1;
+    }
+}
+
+inline float MM2WayRanging::calibratedDistance(uint8_t destination) {
+
+    float rawDistance = (tofs[destination] * 300 * TIMEUNITS_TO_US / 4);
+
+
+
+ // Calibration for Nucleo 0 (and 1)
+
+ //   if (this->address == 1) rawDistance+= 10;
+//    switch(destination){
+//        case 2:
+//            return  rawDistance * 0.9754 - 0.5004;
+//        case 3:
+//            return  rawDistance * 0.9759 - 0.4103;
+//        case 4:
+//            return  rawDistance * 0.9798 - 0.5499;
+//        case 5:
+//            return  rawDistance * 0.9765 - 0.5169;
+//        }
+
+    return rawDistance;
+
+}
+
+void MM2WayRanging::requestRangingAll() {
+    for (int i = 1; i <= 4; i++) {  // Request ranging to all anchors
+        requestRanging(i);
+    }
+}
+
+void MM2WayRanging::sendPingFrame(uint8_t destination) {
+    rangingFrame.source = address;
+    rangingFrame.destination = destination;
+    rangingFrame.type = PING;
+    dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
+}
+
+void MM2WayRanging::sendTransferFrame(uint8_t destination, int timeDiffsReceiver) {
+    transferFrame.source = address;
+    transferFrame.destination = destination;
+    transferFrame.type = TRANSFER_FRAME;
+    transferFrame.signedTime =  timeDiffsReceiver;                      //cast the time difference
+    dw.sendFrame((uint8_t*)&transferFrame, sizeof(transferFrame));
+}
+
+void MM2WayRanging::sendDelayedAnswer(uint8_t destination, uint8_t type, uint64_t rxTimestamp) {
+
+    rangingFrame.source = address;
+    rangingFrame.destination = destination;
+    rangingFrame.type = type;
+
+    if(rxTimestamp + ANSWER_DELAY_TIMEUNITS > MMRANGING_2POWER40)
+        dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS - MMRANGING_2POWER40);
+    else
+        dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS);
+}
+
+void MM2WayRanging::correctReceiverTimestamps(uint8_t source){
+
+    if(receiverTimestamps[source][0] > receiverTimestamps[source][1]){
+        receiverTimestamps[source][1] += MMRANGING_2POWER40;
+        receiverTimestamps[source][2] += MMRANGING_2POWER40;
+    }
+
+    if(receiverTimestamps[source][1] > receiverTimestamps[source][2]){
+            receiverTimestamps[source][2] += MMRANGING_2POWER40;
+        }
+
+}
+
+void MM2WayRanging::correctSenderTimestamps(uint8_t source){
+
+    if (senderTimestamps[source][0] > senderTimestamps[source][1]) {
+        senderTimestamps[source][1] += MMRANGING_2POWER40;
+        senderTimestamps[source][2] += MMRANGING_2POWER40;
+        overflow = true;
+    } else if (senderTimestamps[source][1] > senderTimestamps[source][2]) {
+        senderTimestamps[source][2] += MMRANGING_2POWER40;
+        overflow = true;
+    }else overflow = false;
+
+}

diff -r 000000000000 -r 99928431bb44 MM2WayRanging/MM2WayRanging.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MM2WayRanging/MM2WayRanging.h	Tue Jul 07 09:36:12 2015 +0000
@@ -0,0 +1,94 @@
+// by Matthias Grob & Manuel Stalder - ETH Zürich - 2015
+
+#ifndef MM2WAYRANGING_H
+#define MM2WAYRANGING_H
+
+#include "mbed.h"
+#include "DW1000.h"
+
+#define TIMEUNITS_TO_US       (1/(128*499.2))               // conversion between the decawave timeunits (ca 15.65ps) to microseconds.
+#define US_TO_TIMEUNITS       (128*499.2)                   // conversion between microseconds to the decawave timeunits (ca 15.65ps).
+#define MMRANGING_2POWER40          1099511627776               // decimal value of 2^40 to correct timeroverflow between timestamps
+
+
+
+//Predefined delay for the critical answers in the ranging algorithm
+//HAS TO BE BIGGER THAN THE PROCESSING TIME OF THE FRAME ON THE NODE
+#define ANSWER_DELAY_US             2500                                    //2500 works for 110kbps, 900 for 6.8Mbps
+#define ANSWER_DELAY_TIMEUNITS      ANSWER_DELAY_US * (128*499.2)
+
+class MM2WayRanging {
+
+public:
+    MM2WayRanging(DW1000& DW);
+
+    void requestRanging(uint8_t destination);
+    void requestRangingAll();
+
+
+
+    //TODO: Better capsulation on those?
+    uint8_t address; // Identifies the nodes as source and destination in rangingframes
+
+    //TODO: Make those PRIVATE!
+    float roundtriptimes[10]; // Array containing the round trip times to the anchors or the timeout which occured
+    float distances[10]; // Array containing the finally calculated Distances to the anchors
+
+    bool overflow;              // TRUE if counter overflows while ranging
+
+private:
+
+
+    DW1000& dw;
+    Timer LocalTimer;
+
+    void callbackRX();
+    void callbackTX();
+    void sendPingFrame(uint8_t destination);
+    void sendDelayedAnswer(uint8_t destination, uint8_t type, uint64_t rxTimestamp);
+    void sendTransferFrame(uint8_t destination, int timestamp);
+
+    inline float calibratedDistance(uint8_t destination);
+
+    /**
+     * These two functions correct the timestamps if the counter had an overflow between measurements
+     */
+    void correctReceiverTimestamps(uint8_t source);
+    void correctSenderTimestamps(uint8_t source);
+
+    int timediffRec;
+    int timediffSend;
+
+    enum FrameType{
+        PING=1,
+        ANCHOR_RESPONSE,
+        BEACON_RESPONSE,
+        TRANSFER_FRAME,
+        DISTANCES_FRAME
+    };
+
+    //the packed attribute makes sure the types only use their respective size in memory (8 bit for uint8_t), otherwise they would always use 32 bit
+    //IT IS A GCC SPECIFIC DIRECTIVE
+    struct __attribute__((packed, aligned(1))) RangingFrame {
+        uint8_t source;
+        uint8_t destination;
+        uint8_t type;
+    };
+
+    struct __attribute__((packed, aligned(1))) ExtendedRangingFrame : RangingFrame{
+        int signedTime;
+    };
+
+
+    RangingFrame rangingFrame;                  // buffer in class for sending a frame (not made locally because then we can recall in the interrupt what was sent)
+    ExtendedRangingFrame transferFrame;
+    ExtendedRangingFrame receivedFrame;
+    uint64_t rxTimestamp;
+    uint64_t senderTimestamps[10][3];
+    uint64_t receiverTimestamps[10][3];
+    bool acknowledgement[10];                   // flag to indicate if ranging has succeeded
+    uint32_t tofs[10];                          // Array containing time of flights for each node (index is address of node)
+
+};
+
+#endif

diff -r 000000000000 -r 99928431bb44 Node/Anchor/Anchor.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Node/Anchor/Anchor.cpp	Tue Jul 07 09:36:12 2015 +0000
@@ -0,0 +1,12 @@
+#include "Anchor.h"
+
+Anchor::Anchor( MM2WayRanging& newRanging, DW1000& newDw ) : Node( newRanging, newDw )
+{
+    debugprintf("Anchor Initialized\r\n") ;
+}
+
+void Anchor::execute()
+{
+    // Just waiting in the background as the anchor functionality is interrupt driven
+    wait(0.5) ;
+}
\ No newline at end of file

diff -r 000000000000 -r 99928431bb44 Node/Anchor/Anchor.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Node/Anchor/Anchor.h	Tue Jul 07 09:36:12 2015 +0000
@@ -0,0 +1,22 @@
+
+#ifndef ANCHOR_H
+#define ANCHOR_H
+
+#include "Node.h"
+#include "debug.h"
+
+class Anchor : public Node
+{
+    public:
+        
+        // Constructor
+        Anchor( MM2WayRanging& newRanging, DW1000& newDw ) ;
+        
+        // Executes the anchor's functionality at each timestep
+        virtual void execute() ;
+        
+    private:
+    
+} ;
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r 99928431bb44 Node/Beacon/Beacon.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Node/Beacon/Beacon.cpp	Tue Jul 07 09:36:12 2015 +0000
@@ -0,0 +1,14 @@
+#include "Beacon.h"
+
+Beacon::Beacon( MM2WayRanging& newRanging, DW1000& newDw ) : Node( newRanging, newDw )
+{
+    debugprintf("Beacon Initialized\r\n") ;
+}
+
+void Beacon::execute()
+{
+    // Requesting ranging from node at address #1 TODO: 1 is hardcoded.
+    ranging.requestRanging(1) ;
+    // Printing received distance to serial
+    debugprintf("%f\r\n", ranging.distances[1]) ;
+}
\ No newline at end of file

diff -r 000000000000 -r 99928431bb44 Node/Beacon/Beacon.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Node/Beacon/Beacon.h	Tue Jul 07 09:36:12 2015 +0000
@@ -0,0 +1,23 @@
+
+#ifndef BEACON_H
+#define BEACON_H
+
+#include "Node.h"
+#include "debug.h"
+
+
+class Beacon : public Node
+{
+    public:
+        
+        // Constructor
+        Beacon( MM2WayRanging& newRanging, DW1000& newDw ) ;
+        
+        // Executes the beacon's functionality at each timestep
+        virtual void execute() ;
+        
+    private:
+    
+} ;
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r 99928431bb44 Node/Node.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Node/Node.cpp	Tue Jul 07 09:36:12 2015 +0000
@@ -0,0 +1,20 @@
+#include "Node.h"
+
+
+Node::Node( MM2WayRanging& newRanging, DW1000& newDw ) : ranging(newRanging), dw(newDw)
+{
+    debugprintf("Node Initialized\r\n") ;
+}
+
+void Node::execute()
+{
+
+}
+
+void Node::setAddress( uint8_t address )
+{
+    // Setting the address of the ranging object
+    ranging.address = address ;
+    // Debug output
+    debugprintf("Address set: %u\r\n", address) ;
+}

diff -r 000000000000 -r 99928431bb44 Node/Node.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Node/Node.h	Tue Jul 07 09:36:12 2015 +0000
@@ -0,0 +1,38 @@
+
+#include "mbed.h"
+#include "MM2WayRanging.h"
+#include "DW1000.h"
+#include "debug.h"
+
+
+#ifndef NODE_H
+#define NODE_H
+
+
+enum NodeType { ANCHOR, BEACON, OBSERVER } ;
+
+class Node
+{
+    public:
+    
+        // Constructor
+        Node( MM2WayRanging& newRanging, DW1000& newDw ) ;
+        
+        // Executes the node's functionality at each timestep
+        virtual void execute() ;
+        
+        // Sets the address of the node
+        void setAddress( uint8_t address ) ;
+
+    protected:
+        
+        // Ranging object for ranging
+        MM2WayRanging& ranging ;
+        
+        // Decawave object for direct interaction with the device
+        DW1000& dw ;
+
+} ;
+
+
+#endif

diff -r 000000000000 -r 99928431bb44 Node/Observer/BufferedOutput/BufferedOutput.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Node/Observer/BufferedOutput/BufferedOutput.cpp	Tue Jul 07 09:36:12 2015 +0000
@@ -0,0 +1,86 @@
+#include "BufferedOutput.h"
+
+BufferedOutput::BufferedOutput( Serial& serial ) : framesOut( serial )
+{
+    // Debug startup message
+    debugprintf("Initializing Buffered Output object\r\n") ;
+    
+    // Attaching the serial interrupt callbacks
+    framesOut.attach(this, &BufferedOutput::framesOutCallback, Serial::TxIrq ) ;
+    
+    // Initializing the buffer pointers
+    framesBufferIn = 0 ;
+    framesBufferOut = 0 ;
+    
+    // Initializing frame transfer state
+    frameTransferInProgress = false ;
+
+}
+
+void BufferedOutput::executeBackgroundTask()
+{
+    // Checking if the Frame buffer is non empty
+    if( !(framesBufferOut == framesBufferIn) && !frameTransferInProgress )
+    {
+        //debugprintf("Starting frames Tx\r\n") ;
+        // Moving characters from the mavlink buffer to the USART device buffer
+        // while there are still chars in the MAVLink Buffer
+        if( framesOut.writeable() )
+        {
+            // Disabling interrupts while interacting with UART device
+            __disable_irq();
+            // Putting character to the USART device buffer
+            framesOut.putc( framesBuffer[ framesBufferOut ] ) ;
+            // Disabling interrupts while modifying buffers
+            __enable_irq();
+            // Incrementing the output pointer
+            framesBufferOut = ( framesBufferOut + 1 ) % framesBufferSize ;
+            // Indicating that the frame transfer has started
+            frameTransferInProgress = true ;
+        }
+    }
+}
+
+/*
+void BufferedOutput::addFrame( char* frame, char length )
+{
+    // Disabling interrupt while modifying global buffers
+    __disable_irq() ;
+    // Looping over all characters in the frame
+    for( char index = 0 ; index < length ; index++ )
+    {
+        // Placing char output buffer
+        framesBuffer[ framesBufferIn ] = frame[index] ;
+        // Incrementing the input pointer
+        framesBufferIn = ( framesBufferIn + 1 ) % framesBufferSize ;
+    }
+    // Re-enabling interrupts
+    __enable_irq() ;
+}
+*/
+
+void BufferedOutput::addChar( char c )
+{
+    // Placing char output buffer
+    framesBuffer[ framesBufferIn ] = c ;
+    // Incrementing the input pointer
+    framesBufferIn = ( framesBufferIn + 1 ) % framesBufferSize ;
+}
+
+void BufferedOutput::framesOutCallback()
+{
+    // Moving characters from the mavlink buffer to the USART device buffer
+    // while there are still chars in the MAVLink Buffer
+    while( framesOut.writeable() && !( framesBufferOut == framesBufferIn ) )
+    {
+        // Putting character to the USART device buffer
+        framesOut.putc( framesBuffer[ framesBufferOut ] ) ;
+        // Incrementing the output pointer
+        framesBufferOut = ( framesBufferOut + 1 ) % framesBufferSize ;
+    }
+    // If buffer emptied indicate that tx interrupts are over
+    if( ( framesBufferOut == framesBufferIn ) )
+    {
+        frameTransferInProgress = false ;
+    }
+}
\ No newline at end of file

diff -r 000000000000 -r 99928431bb44 Node/Observer/BufferedOutput/BufferedOutput.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Node/Observer/BufferedOutput/BufferedOutput.h	Tue Jul 07 09:36:12 2015 +0000
@@ -0,0 +1,48 @@
+
+#ifndef BUFFEREDOUTPUT_H
+#define BUFFEREDOUTPUT_H
+
+// Dependancies
+#include "mbed.h"
+#include "debug.h"
+
+// Circular buffer for mavlink characters
+const int framesBufferSize = 255;
+
+class BufferedOutput
+{
+    public:
+    
+        // Constructor
+        BufferedOutput( Serial& serial ) ;
+        
+        // Contains the MAVlink passthrough background tasks
+        void executeBackgroundTask() ;
+        
+        // Adding a frame to the output buffer
+        //void addFrame( char* frame, char length ) ;
+        
+        // Adding a character to the output buffer
+        void addChar( char c ) ;
+
+    protected:
+    
+        // Serial object where outward frames are written
+        Serial& framesOut ;
+        
+        // Frames Send Callback
+        void framesOutCallback() ;
+        
+        // Buffer which hold mavlink characters received via the serial connection
+        char framesBuffer[ framesBufferSize + 1 ] ;
+        // Pointers to entry and exit points to the buffer
+        volatile char framesBufferIn  ;
+        volatile char framesBufferOut  ;
+        
+        // Indicates if a frame is currently being transfered over serial
+        bool frameTransferInProgress ;
+    
+} ;
+
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r 99928431bb44 Node/Observer/DistanceFrame.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Node/Observer/DistanceFrame.h	Tue Jul 07 09:36:12 2015 +0000
@@ -0,0 +1,23 @@
+
+#ifndef DISTANCEFRAME_H_
+#define DISTANCEFRAME_H_
+
+
+struct __attribute__((packed, aligned(1))) DistancesFrame {
+        uint8_t source;
+        uint8_t destination;
+        uint8_t type;
+        float dist[4];
+    };
+
+
+/*
+struct __attribute__((packed, aligned(1))) DistanceFrame {
+        uint8_t source ;
+        uint8_t destination ;
+        uint8_t type ;
+        float dist ;
+    };
+*/
+
+#endif /* DISTANCEFRAME_H_ */
\ No newline at end of file

diff -r 000000000000 -r 99928431bb44 Node/Observer/MavlinkPassthrough/MavlinkPassthrough.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Node/Observer/MavlinkPassthrough/MavlinkPassthrough.cpp	Tue Jul 07 09:36:12 2015 +0000
@@ -0,0 +1,121 @@
+#include "MavlinkPassthrough.h"
+
+MavlinkPassthrough::MavlinkPassthrough( Serial& serial, BufferedOutput& bufferedOutput ) : mavlinkIn(serial), bufferedOutput(bufferedOutput)
+{
+    // Debug startup message
+    debugprintf("Initializing Mavlink Passthrough object\r\n") ;
+    
+    // Attaching the serial interrupt callbacks
+    mavlinkIn.attach(this, &MavlinkPassthrough::mavlinkInCallback, Serial::RxIrq ) ;
+    
+    // The state of the current mavlink message
+    mavlinkTransferState = START ;
+    currentMavlinkFrameLength = 0 ;
+    
+    // Initializing the buffer pointers
+    mavlinkBufferIn = 0 ;
+    mavlinkBufferOut = 0 ;
+    mavlinkBufferRead = 0 ;
+}
+
+void MavlinkPassthrough::executeBackgroundTask()
+{
+    // Blinking LED for debug
+    //led = !led ;
+    // Maintaing the buffer
+    maintainMavlinkBuffer() ;
+}
+
+void MavlinkPassthrough::mavlinkInCallback()
+{
+    // Reading out all chars in the USART buffer in the mavlink buffer
+    // while the mavlink buffer is not full
+    while( mavlinkIn.readable() && !((( mavlinkBufferIn + 1 ) % mavlinkBufferSize) == mavlinkBufferOut ) )
+    {
+        // Taking character from device and putting the buffer
+        mavlinkBuffer[ mavlinkBufferIn ] = mavlinkIn.getc() ;
+        // Incrementing the buffer pointer 
+        mavlinkBufferIn = ( mavlinkBufferIn + 1 ) % mavlinkBufferSize ;
+    }
+}
+
+void MavlinkPassthrough::maintainMavlinkBuffer()
+{
+    // Switching actions depending on the mavlink transfer case
+    switch( mavlinkTransferState )
+    {
+        case START :
+            // Searching for start character until end of mavlink buffer
+            while( !( mavlinkBufferRead == mavlinkBufferIn ) )
+            {
+                //debugprintf("Buffer not empty searching for start char\r\n") ;
+                // Detect the start character
+                if( mavlinkBuffer[mavlinkBufferRead] == MAVLINKSTARTCHAR )
+                {
+                    //debugprintf("Start character found. Switching state\r\n") ;
+                    // Changing state, advancing read pointer and stopping search
+                    mavlinkTransferState = LENGTH ;
+                    debugprintf("Mavlink state LENGTH\r\n") ;
+                    mavlinkBufferRead = ( mavlinkBufferRead + 1 ) % mavlinkBufferSize ;
+                    break ;
+                }
+                // Else advance the read and output pointer and continue the search
+                else
+                {
+                    mavlinkBufferRead = ( mavlinkBufferRead + 1 ) % mavlinkBufferSize ;
+                    mavlinkBufferOut = ( mavlinkBufferOut + 1 ) % mavlinkBufferSize ;
+                }   
+            }
+            break ;
+        case LENGTH :
+            // Checking if there's another byte in the mavlink buffer
+            if( !( mavlinkBufferRead == mavlinkBufferIn ) )
+            {
+                // Reading the character as the datalength
+                currentMavlinkFrameLength = mavlinkBuffer[mavlinkBufferRead] + MAVLINKADDITIONALDATA ;
+                //debugprintf("Length character found: %u\r\n", currentMavlinkFrameLength ) ;
+                // Advancing the read pointer and changing the state
+                mavlinkTransferState = DATA ;
+                debugprintf("Mavlink state DATA\r\n") ;
+                mavlinkBufferRead = ( mavlinkBufferRead + 1 ) % mavlinkBufferSize ;
+            }
+            break ;
+        case DATA :
+            // Looping while there are still data bits to be read
+            while( !( mavlinkBufferRead == mavlinkBufferIn ) )
+            {
+                // Incrementing the read pointer and decrementing the data bits left counter
+                mavlinkBufferRead = ( mavlinkBufferRead + 1 ) % mavlinkBufferSize ;
+                currentMavlinkFrameLength -= 1 ;
+                //debugprintf("Databits left: %u\r\n", currentMavlinkFrameLength) ;
+                // If no databits left move to the next state
+                if( currentMavlinkFrameLength == 0 )
+                {
+                    // Advancing the read pointer and changing the state
+                    mavlinkTransferState = TRANSFER ;
+                    debugprintf("Mavlink state TRANSFER\r\n") ;
+                    break ;
+                }
+            }
+            break ;
+        case TRANSFER :
+            
+            // Disabling while accessing shared output buffer
+            __disable_irq() ;
+            // Looping until all characters moved to the transferBuffer frame
+            while( !( mavlinkBufferOut == mavlinkBufferRead ) )
+            {
+                // Transfering a character to the frames buffer
+                bufferedOutput.addChar( mavlinkBuffer[mavlinkBufferOut] ) ;
+                // Incrementing the mavlinkOut pointer
+                mavlinkBufferOut = ( mavlinkBufferOut + 1 ) % mavlinkBufferSize ;
+            }
+            // Re-enabling interrupts
+            __enable_irq() ;
+            
+            // Changing back to start state
+            mavlinkTransferState = START ;
+            debugprintf("Mavlink state START\r\n") ;
+            break ;
+    }
+}
\ No newline at end of file

diff -r 000000000000 -r 99928431bb44 Node/Observer/MavlinkPassthrough/MavlinkPassthrough.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Node/Observer/MavlinkPassthrough/MavlinkPassthrough.h	Tue Jul 07 09:36:12 2015 +0000
@@ -0,0 +1,59 @@
+
+#ifndef MAVLINKPASSTHROUGH_H
+#define MAVLINKPASSTHROUGH_H
+
+// Dependancies
+#include "mbed.h"
+#include "debug.h"
+#include "BufferedOutput.h"
+
+// MAVlink defines
+#define MAVLINKSTARTCHAR 254
+#define MAVLINKADDITIONALDATA 6
+
+// Circular buffer for mavlink characters
+const int mavlinkBufferSize = 255 ;
+
+// Enumeration possible mavlink frame states
+enum mavlinkTransferStateType { START, LENGTH, DATA, TRANSFER } ;
+
+class MavlinkPassthrough
+{
+    public:
+    
+        // Constructor
+        MavlinkPassthrough( Serial& serial, BufferedOutput& bufferedOutput ) ;
+        
+        // Contains the MAVlink passthrough background tasks
+        void executeBackgroundTask() ;
+
+    protected:
+    
+        // Serial object for receiving mavlink
+        Serial& mavlinkIn ;
+        
+        // Buffered output for outgoing frames
+        BufferedOutput& bufferedOutput ;
+        
+        // MAVlink Receive Callback
+        void mavlinkInCallback() ;
+        
+        // Maintains the mavlink buffer, transfering frames out as they arrive
+        void maintainMavlinkBuffer() ;
+        
+        // The state of the current MAVlink frame 
+        mavlinkTransferStateType mavlinkTransferState ;
+        // The remaining characters to be received in the current mavlink frame
+        char currentMavlinkFrameLength ;
+        
+        // Buffer which hold mavlink characters received via the serial connection
+        char mavlinkBuffer[ mavlinkBufferSize + 1 ] ;
+        // Pointers to entry and exit points to the buffer
+        volatile char mavlinkBufferIn ;
+        volatile char mavlinkBufferOut ;
+        volatile char mavlinkBufferRead ;
+
+} ;
+
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r 99928431bb44 Node/Observer/Observer.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Node/Observer/Observer.cpp	Tue Jul 07 09:36:12 2015 +0000
@@ -0,0 +1,64 @@
+#include "Observer.h"
+
+Observer::Observer( MM2WayRanging& newRanging, DW1000& newDw, Serial& output, Serial& mavlinkIn ) : Node( newRanging, newDw ), bufferedOutput( output ), mavlinkPassthrough( mavlinkIn, bufferedOutput )
+{
+    debugprintf("Observer Initialized\r\n") ;
+    // Registering the callback for observer functionality
+    dw.setCallbacks(this, &Observer::observerCallbackRX, &Observer::observerCallbackTX);   
+    //dw.setCallbacks(this, &Observer::observerCallbackRX, NULL);   
+}
+
+void Observer::execute()
+{
+    // Executing the passthrough background task
+    mavlinkPassthrough.executeBackgroundTask() ;
+    // Executing the frames output background task
+    bufferedOutput.executeBackgroundTask() ;
+}
+
+void Observer::observerCallbackRX()
+{   
+    // Creating a distance frame to hold received message
+    DistancesFrame distFrame ;
+    // Reading received message into distance frame
+    dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&distFrame, dw.getFramelength()) ;
+       
+    // If message directed at this node
+    if ( distFrame.destination == ranging.address )
+    {
+        // Disabling while accessing shared output buffer
+        __disable_irq() ;
+        
+        // A float for debug
+        //volatile float debugFloat = 0.00 ;
+        
+        // Printing the heading code
+        bufferedOutput.addChar( HEADERCODE ) ;
+        // Printing the message length
+        bufferedOutput.addChar( 4*sizeof(float) ) ;
+        // Looping through and printing received distance
+        for( int nodeIndex=1; nodeIndex<=4; nodeIndex++ )
+        {
+            // Retreving address of distance as char address
+            char *pdist = (char *) &distFrame.dist[nodeIndex-1] ;
+            //debugFloat ++ ;
+            //char *pdist = (char *) &debugFloat ;
+            // Looping over the bytes in the float and sending
+            for( char index = 0 ; index < sizeof(float) ; index++ )
+            {
+                 bufferedOutput.addChar( pdist[ index ] ) ;
+            }
+        }
+        
+        // Re-enabling interrupts
+        __enable_irq() ;
+    }
+    // Starting receive again
+    dw.startRX();
+}
+
+void Observer::observerCallbackTX()
+{
+    // Debug output
+    //debugprintf("Message sent\r\n") ;
+}
\ No newline at end of file

diff -r 000000000000 -r 99928431bb44 Node/Observer/Observer.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Node/Observer/Observer.h	Tue Jul 07 09:36:12 2015 +0000
@@ -0,0 +1,39 @@
+
+
+
+#ifndef OBSERVER_H
+#define OBSERVER_H
+
+#include "Node.h"
+#include "DistanceFrame.h"
+#include "debug.h"
+#include "BufferedOutput.h"
+#include "MavlinkPassthrough.h"
+
+#define HEADERCODE 253
+
+class Observer : public Node
+{
+    public:
+        
+        // Constructor
+        Observer( MM2WayRanging& newRanging, DW1000& newDw, Serial& output, Serial& mavlinkIn ) ;
+        
+        // Executes the observers's functionality at each timestep
+        virtual void execute() ;
+        
+        // The callback which implements the observer functionality
+        void observerCallbackRX() ;
+        void observerCallbackTX() ;
+        
+    private:
+    
+        // Buffered output for sending data frames
+        BufferedOutput bufferedOutput ;
+        
+        // Mavlink passthrough object
+        MavlinkPassthrough mavlinkPassthrough ;
+    
+} ;
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r 99928431bb44 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Jul 07 09:36:12 2015 +0000
@@ -0,0 +1,94 @@
+/*
+*   Author:     Alex Millane (millanea@ethz.ch)
+*               Matthias Grob
+*               Manuel Stalder
+*   Date:       04/06/2015
+*   Purpose:    Implements nodes which form a localization network.
+*               Localization is performed using Decawave DW1000 ranging
+*               UWB radios. Additionally initial measurements from the
+*               px4 flight controller are used. The project implements
+*               three nodes:
+*               - Beacon:   Initiates message transmissions, calculates
+                            ranges and sends them to the observer node.
+                - Anchor:   Received ranging messages and sends calculated
+                            time of flight values to the Beacon.
+                - Observer: Receives calculated ranges from the beacon and
+                            passes them out over a serial connection to a 
+                            host performing estimation. Additionally passes
+                            mavlink messages out over the same serial connection. 
+*/
+
+// Includes
+#include "mbed.h"
+#include "DW1000.h"
+#include "MM2WayRanging.h"
+#include "Node.h"
+#include "Beacon.h"
+#include "Anchor.h"
+#include "Observer.h"
+#include "debug.h"
+
+// Initializing hardware devices
+Serial          serialUSB(USBTX, USBRX) ;           // UART2
+Serial          serial1(PA_9,PA_10) ;               // UART1
+Serial          serial6(PA_11,PA_12) ;              // UART6
+
+// Observer Defines
+Serial& mavlinkIn = serial6 ;
+Serial& framesOut = serial1 ;
+
+// Ranging related objects
+DW1000          dw(PA_7, PA_6, PA_5, PB_6, PB_9);   // Device driver instanceSPI pins: (MOSI, MISO, SCLK, CS, IRQ)
+MM2WayRanging   ranging(dw);                        // Instance of the two way ranging algorithm
+
+// Node factory function
+Node& createNode( NodeType nodeType ) ;
+
+// Main programm 
+int main() {
+
+    // Setting the baud rates of the serial connections
+    serialUSB.baud(57600) ;
+    serial1.baud(57600) ;
+    serial6.baud(57600) ;
+
+    // Debug start up messages
+    debugprintf("Alex Ranging\r\n") ;
+    dw.setEUI(0xFAEDCD01FAEDCD01);                                  // basic methods called to check if we have a working SPI connection
+    debugprintf("DEVICE_ID register: 0x%X\r\n", dw.getDeviceID());
+    debugprintf("EUI register: %016llX\r\n", dw.getEUI());
+    debugprintf("Voltage: %fV\r\n", dw.getVoltage());
+    
+    // Setting the node parameters
+    NodeType nodeType   = OBSERVER ; //ANCHOR ; BEACON ; OBSERVER ;
+    uint8_t nodeAddress = 5 ;
+    
+    // Creating the node
+    Node& node = createNode( nodeType ) ;
+    node.setAddress(nodeAddress) ;
+    
+    // Executing the application
+    while(1){
+        // Calling the node functionality
+        node.execute() ;
+    }
+    
+}
+
+// Node factory function
+Node& createNode( NodeType nodeType ) {
+ 
+    // Creating different node types dependant on argument
+    if( nodeType == ANCHOR ){
+        Anchor* anchor = new Anchor( ranging, dw ) ;
+        return *anchor ;
+    }
+    else if( nodeType == BEACON ){
+        Beacon* beacon = new Beacon( ranging, dw ) ;
+        return *beacon ;
+    }
+    else { // if( nodeType == OBSERVER ){
+        Observer* observer = new Observer( ranging, dw, framesOut, mavlinkIn ) ;
+        return *observer ;
+    }
+}
\ No newline at end of file

diff -r 000000000000 -r 99928431bb44 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Jul 07 09:36:12 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/cbbeb26dbd92
\ No newline at end of file