Revision 0:0bbf2f16da9c, committed 2011-11-18

Comitter:

madcowswe

Date:

Fri Nov 18 18:23:33 2011 +0000

Commit message:

Changed in this revision

diff -r 000000000000 -r 0bbf2f16da9c GlobalsNDefines.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GlobalsNDefines.h	Fri Nov 18 18:23:33 2011 +0000
@@ -0,0 +1,41 @@
+/*
+#ifndef GlobalsNDefines_H_
+#define GlobalsNDefines_H_
+*/
+
+#include "mbed.h"
+#include "RF12B.h"
+
+Serial pc(USBTX, USBRX); // tx, rx
+I2C i2c(p28, p27);
+//Rfm12b rfm12b(p5, p6, p7, p8, p11);
+RF12B radiolink(p5, p6, p7, p8, p9);
+DigitalOut myled(LED1);
+DigitalOut motormaxled(LED2);
+DigitalIn Nkill(p10);
+Ticker watchdog;
+
+PwmOut PWMfront(p23);
+PwmOut PWMrear(p22);
+PwmOut PWMleft(p24);
+PwmOut PWMright(p21);
+
+AnalogIn altsensor(p15);
+
+#define RPSPUNIT -0.00126766
+#define NOPOWER 0.048
+#define ACCADDRESS 0x4C
+#define LOOPTIME 0.01
+#define ACCDECAY 50
+#define YAWCAP 1.5f
+#define COMMANDPERIOD 0.1;
+#define ALTGAIN 
+//#define ALTSENSOR
+
+float gyrcalib[] = {0.0, 0.0, 0.0};
+bool loopalive = 1;
+bool commandsalive = 1;
+bool integratestop = 0;
+
+
+//#endif
\ No newline at end of file

diff -r 000000000000 -r 0bbf2f16da9c RF12B/RF12B.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RF12B/RF12B.cpp	Fri Nov 18 18:23:33 2011 +0000
@@ -0,0 +1,377 @@
+#include "RF12B.h"
+
+#include "RF_defs.h"
+#include <algorithm>
+
+RF12B::RF12B(PinName _SDI,
+             PinName _SDO,
+             PinName _SCK,
+             PinName _NCS,
+             PinName _NIRQ):spi(_SDI, _SDO, _SCK),
+        NCS(_NCS), NIRQ(_NIRQ), NIRQ_in(_NIRQ), rfled(LED3) {
+
+    /* SPI frequency, word lenght, polarity and phase */
+    spi.format(16,0);
+    spi.frequency(2000000);
+
+    /* Set ~CS high */
+    NCS = 1;
+
+    /* Initialise RF Module */
+    init();
+
+    /* Setup interrupt to happen on falling edge of NIRQ */
+    NIRQ.fall(this, &RF12B::rxISR);
+}
+
+/* Returns the packet length if data is available in the receive buffer, 0 otherwise*/
+unsigned int RF12B::available() {
+    return fifo.size();
+}
+
+/* Reads a packet of data, with length "size" Returns false if read failed. TODO: make a metafifo to isolate packets*/
+bool RF12B::read(unsigned char* data, unsigned int size) {
+    if (fifo.size() == 0) {
+        return false;
+    } else {
+        unsigned int i = 0;
+        while (fifo.size() > 0 && i < size) {
+            data[i++] = fifo.front();
+            fifo.pop();
+        }
+        return true;
+    }
+}
+
+/* Reads a byte of data from the receive buffer */
+unsigned char RF12B::read() {
+    if (available()) {
+        unsigned char data = fifo.front();
+        fifo.pop();
+        return data;
+    } else {
+        return 0xFF; // Error val although could also be data...
+    }
+}
+
+/* Sends a packet of data to the RF module for transmission TODO: Make asych*/
+void RF12B::write(unsigned char *data, unsigned char length) {
+    unsigned char crc = 0;
+        
+    /* Transmitter mode */
+    changeMode(TX);
+
+    writeCmd(0x0000);
+    send(0xAA); // PREAMBLE
+    send(0xAA);
+    send(0xAA);
+    send(0x2D); // SYNC
+    send(0xD4);
+    /* Packet Length */
+    send(length);
+    crc = crc8(crc, length);
+    send(crc);
+    crc = crc8(crc, crc);
+    /* Packet Data */
+    for (unsigned char i=0; i<length; i++) {
+        send(data[i]);
+        crc = crc8(crc, data[i]);
+    }
+    send(crc);
+    send(0xAA); // DUMMY BYTES
+    send(0xAA);
+    send(0xAA);
+
+    /* Back to receiver mode */
+    changeMode(RX);
+    status();
+}
+
+/* Transmit a 1-byte data packet */
+void RF12B::write(unsigned char data) {
+    write(&data, 1);
+}
+
+void RF12B::write(queue<char> &data, int length) {
+    char crc = 0;
+    char length_byte = 0;
+    
+    /* -1 means try to transmit everything in the queue */
+    if(length == -1) {
+        length = data.size();
+    }
+    
+    /* max length of packet is 255 */
+    length_byte = min(length, 255);
+    
+    /* Transmitter mode */
+    changeMode(TX);
+
+    writeCmd(0x0000);
+    send(0xAA); // PREAMBLE
+    send(0xAA);
+    send(0xAA);
+    send(0x2D); // SYNC
+    send(0xD4);
+    /* Packet Length */
+    send(length_byte);
+    crc = crc8(crc, length_byte);
+    send(crc);
+    crc = crc8(crc, crc);
+    /* Packet Data */
+    for (char i=0; i<length_byte; i++) {
+        send(data.front());
+        crc = crc8(crc, data.front());
+        data.pop();
+    }
+    send(crc);
+    send(0xAA); // DUMMY BYTES
+    send(0xAA);
+    send(0xAA);
+
+    /* Back to receiver mode */
+    changeMode(RX);
+    status();
+}
+
+/**********************************************************************
+ *  PRIVATE FUNCTIONS
+ *********************************************************************/
+
+/* Initialises the RF12B module */
+void RF12B::init() {
+    /* writeCmd(0x80E7); //EL,EF,868band,12.0pF
+     changeMode(RX);
+     writeCmd(0xA640); //frequency select
+     writeCmd(0xC647); //4.8kbps
+     writeCmd(0x94A0); //VDI,FAST,134kHz,0dBm,-103dBm
+     writeCmd(0xC2AC); //AL,!ml,DIG,DQD4
+     writeCmd(0xCA81); //FIFO8,SYNC,!ff,DR
+     writeCmd(0xCED4); //SYNC=2DD4
+     writeCmd(0xC483); //@PWR,NO RSTRIC,!st,!fi,OE,EN
+     writeCmd(0x9850); //!mp,90kHz,MAX OUT
+     writeCmd(0xCC17); //OB1, COB0, LPX, Iddy, CDDIT&#65533;CBW0
+     writeCmd(0xE000); //NOT USED
+     writeCmd(0xC800); //NOT USED
+     writeCmd(0xC040); //1.66MHz,2.2V */
+
+    writeCmd(
+        RFM_CONFIG_EL           |
+        RFM_CONFIG_EF           |
+        RFM_CONFIG_BAND_433     //|
+        //RFM_CONFIG_X_11_0pf // meh, using default
+    );
+
+    // 2. Power Management Command
+    // leave everything switched off for now
+    /*
+    writeCmd(
+        RFM_POWER_MANAGEMENT     // switch all off
+    );
+    */
+
+    // 3. Frequency Setting Command
+    writeCmd(
+        RFM_FREQUENCY            |
+        RFM_FREQ_433Band(435.7)  //I totally made this value up... if someone knows where the sweetspots are in this band, tell me!
+    );
+
+
+    // 4. Data Rate Command
+    writeCmd(RFM_DATA_RATE_9600);
+
+
+    // 5. Receiver Control Command
+    writeCmd(
+        RFM_RX_CONTROL_P20_VDI  |
+        RFM_RX_CONTROL_VDI_FAST |
+        //RFM_RX_CONTROL_BW(RFM_BAUD_RATE) |
+        RFM_RX_CONTROL_BW_134   |     // CHANGE THIS TO 67 TO IMPROVE RANGE! (though the bitrate must then be below 8kbaud, and fsk modulation changed)
+        RFM_RX_CONTROL_GAIN_0   |
+        RFM_RX_CONTROL_RSSI_103      // Might need adjustment. Datasheet says around 10^-5 bit error rate at this level and baudrate.
+    );
+
+    // 6. Data Filter Command
+    writeCmd(
+        RFM_DATA_FILTER_AL      |
+        RFM_DATA_FILTER_ML      |
+        RFM_DATA_FILTER_DIG     //|
+        //RFM_DATA_FILTER_DQD(4)
+    );
+
+    // 7. FIFO and Reset Mode Command
+    writeCmd(
+        RFM_FIFO_IT(8) |
+        RFM_FIFO_DR    |
+        0x8 //turn on 16bit sync word
+    );
+
+    // 8. FIFO Syncword
+    // Leave as default: 0xD4
+
+    // 9. Receiver FIFO Read
+    // when the interupt goes high, (and if we can assume that it was a fifo fill interrupt) we can read a byte using:
+    // result = RFM_READ_FIFO();
+
+    // 10. AFC Command
+    writeCmd(
+        //RFM_AFC_AUTO_VDI        |  //Note this might be changed to improve range. Refer to datasheet.
+        RFM_AFC_AUTO_INDEPENDENT    |
+        RFM_AFC_RANGE_LIMIT_7_8     |
+        RFM_AFC_EN                  |
+        RFM_AFC_OE                  |
+        RFM_AFC_FI
+    );
+
+    // 11. TX Configuration Control Command
+    writeCmd(
+        RFM_TX_CONTROL_MOD_60 |
+        RFM_TX_CONTROL_POW_0
+    );
+
+
+    // 12. PLL Setting Command
+    writeCmd(
+        0xCC77 & ~0x01 // Setting the PLL bandwith, less noise, but max bitrate capped at 86.2
+        // I think this will slow down the pll's reaction time. Not sure, check with someone!
+    );
+
+    changeMode(RX);
+    resetRX();
+    status();
+}
+
+/* Write a command to the RF Module */
+unsigned int RF12B::writeCmd(unsigned int cmd) {
+    NCS = 0;
+    unsigned int recv = spi.write(cmd);
+    NCS = 1;
+    return recv;
+}
+
+/* Sends a byte of data across RF */
+void RF12B::send(unsigned char data) {
+    while (NIRQ);
+    writeCmd(0xB800 + data);
+}
+
+/* Change the mode of the RF module to Transmitting or Receiving */
+void RF12B::changeMode(rfmode_t _mode) {
+    mode = _mode;
+    if (_mode == TX) {
+        writeCmd(0x8239); //!er,!ebb,ET,ES,EX,!eb,!ew,DC
+    } else { /* mode == RX */
+        writeCmd(0x8299); //er,!ebb,ET,ES,EX,!eb,!ew,DC
+    }
+}
+
+/* Interrupt routine for data reception */
+void RF12B::rxISR() {
+    unsigned int data = 0;
+    static int i = -2;
+    static unsigned char packet_length = 0;
+    static unsigned char crc = 0;
+    static queue<unsigned char> temp;
+
+    //Loop while interrupt is asserted
+    while (!NIRQ_in && mode == RX) {
+
+        /* Grab the packet's length byte */
+        if (i == -2) {
+            data = writeCmd(0x0000);
+            if ( (data&0x8000) ) {
+                data = writeCmd(0xB000);
+                packet_length = (data&0x00FF);
+                crc = crc8(crc, packet_length);
+                i++;
+            }
+        }
+
+        //If we exhaust the interrupt, exit
+        if (NIRQ_in)
+            break;
+
+        // Check that packet length was correct
+        if (i == -1) {
+            data = writeCmd(0x0000);
+            if ( (data&0x8000) ) {
+                data = writeCmd(0xB000);
+                unsigned char crcofsize = (data&0x00FF);
+                if (crcofsize != crc) {
+                    //It was wrong, start over
+                    i = -2;
+                    packet_length = 0;
+                    crc = 0;
+                    temp = queue<unsigned char>();
+                    resetRX();
+                } else {
+                    crc = crc8(crc, crcofsize);
+                    i++;
+                }
+            }
+        }
+
+        //If we exhaust the interrupt, exit
+        if (NIRQ_in)
+            break;
+
+        /* Grab the packet's data */
+        if (i >= 0 && i < packet_length) {
+            data = writeCmd(0x0000);
+            if ( (data&0x8000) ) {
+                data = writeCmd(0xB000);
+                temp.push(data&0x00FF);
+                crc = crc8(crc, (unsigned char)(data&0x00FF));
+                i++;
+            }
+        }
+
+        //If we exhaust the interrupt, exit
+        if (NIRQ_in)
+            break;
+
+        if (i >= packet_length) {
+            data = writeCmd(0x0000);
+            if ( (data&0x8000) ) {
+                data = writeCmd(0xB000);
+                if ((unsigned char)(data & 0x00FF) == crc) {
+                    //If the checksum is correct, add our data to the end of the output buffer
+                    while (!temp.empty()) {
+                        fifo.push(temp.front());
+                        temp.pop();
+                    }
+                }
+
+                /* Tell RF Module we are finished, and clean up */
+                i = -2;
+                packet_length = 0;
+                crc = 0;
+                temp = queue<unsigned char>();
+                resetRX();
+            }
+        }
+    }
+}
+
+unsigned int RF12B::status() {
+    return writeCmd(0x0000);
+}
+
+/* Tell the RF Module this packet is received and wait for the next */
+void RF12B::resetRX() {
+    writeCmd(0xCA81);
+    writeCmd(0xCA83);
+};
+
+/* Calculate CRC8 */
+unsigned char RF12B::crc8(unsigned char crc, unsigned char data) {
+    crc = crc ^ data;
+    for (int i = 0; i < 8; i++) {
+        if (crc & 0x01) {
+            crc = (crc >> 1) ^ 0x8C;
+        } else {
+            crc >>= 1;
+        }
+    }
+    return crc;
+}
\ No newline at end of file

diff -r 000000000000 -r 0bbf2f16da9c RF12B/RF12B.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RF12B/RF12B.h	Fri Nov 18 18:23:33 2011 +0000
@@ -0,0 +1,75 @@
+#ifndef _RF12B_H
+#define _RF12B_H
+
+#include "mbed.h"
+#include <queue>
+
+enum rfmode_t{RX, TX};
+
+class RF12B {
+public:
+    /* Constructor */
+    RF12B(PinName SDI,
+          PinName SDO,
+          PinName SCK,
+          PinName NCS,
+          PinName NIRQ);
+          
+    
+          
+    /* Reads a packet of data. Returns false if read failed. Use available() to check how much space to allocate for buffer */
+    bool read(unsigned char* data, unsigned int size);
+
+    /* Reads a byte of data from the receive buffer 
+        Returns 0xFF if there is no data */
+    unsigned char read();
+
+    /* Transmits a packet of data */
+    void write(unsigned char* data, unsigned char length);
+    void write(unsigned char data); /* 1-byte packet */
+    void write(queue<char> &data, int length = -1); /* sends a whole queue */
+    
+    /* Returns the packet length if data is available in the receive buffer, 0 otherwise*/
+    unsigned int available();
+
+protected:
+    /* Receive FIFO buffer */
+    queue<unsigned char> fifo;
+    
+    /* SPI module */
+    SPI spi;
+    
+    /* Other digital pins */
+    DigitalOut NCS;
+    InterruptIn NIRQ;
+    DigitalIn NIRQ_in;
+    DigitalOut rfled;
+    
+    rfmode_t mode;
+
+    /* Initialises the RF12B module */
+    void init();
+
+    /* Write a command to the RF Module */
+    unsigned int writeCmd(unsigned int cmd);
+    
+    /* Sends a byte of data across RF */
+    void send(unsigned char data);
+    
+    /* Switch module between receive and transmit modes */
+    void changeMode(rfmode_t mode);
+    
+    /* Interrupt routine for data reception */
+    void rxISR();
+    
+    /* Tell the RF Module this packet is received and wait for the next */
+    void resetRX();
+    
+    /* Return the RF Module Status word */
+    unsigned int status();
+    
+    /* Calculate CRC8 */
+    unsigned char crc8(unsigned char crc, unsigned char data);
+};
+
+#endif /* _RF12B_H */
\ No newline at end of file

diff -r 000000000000 -r 0bbf2f16da9c RF12B/RFSerial.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RF12B/RFSerial.cpp	Fri Nov 18 18:23:33 2011 +0000
@@ -0,0 +1,25 @@
+
+/* Constructor */
+#include "RFSerial.h"
+
+RFSerial::RFSerial(PinName _SDI,
+                   PinName _SDO,
+                   PinName _SCK,
+                   PinName _NCS,
+                   PinName _NIRQ)
+:RF12B(_SDI, _SDO, _SCK, _NCS, _NIRQ) {
+    
+}
+
+// Stream implementation functions
+int RFSerial::_putc(int value) {
+    RF12B::write((unsigned char) value);
+    return value;
+}
+int RFSerial::_getc() {
+    if(available()) {
+        return RF12B::read();
+    } else {
+        return EOF;
+    }
+}
\ No newline at end of file

diff -r 000000000000 -r 0bbf2f16da9c RF12B/RFSerial.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RF12B/RFSerial.h	Fri Nov 18 18:23:33 2011 +0000
@@ -0,0 +1,17 @@
+#include "mbed.h"
+#include "RF12B.h"
+
+class RFSerial : public Stream, public RF12B {
+public:
+    /* Constructor */
+    RFSerial(PinName _SDI,
+          PinName _SDO,
+          PinName _SCK,
+          PinName _NCS,
+          PinName _NIRQ);
+     
+protected:
+    // Stream implementation functions
+    virtual int _putc(int value);
+    virtual int _getc();    
+};
\ No newline at end of file

diff -r 000000000000 -r 0bbf2f16da9c RF12B/RF_defs.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RF12B/RF_defs.h	Fri Nov 18 18:23:33 2011 +0000
@@ -0,0 +1,478 @@
+/*
+ *  Open HR20
+ *
+ *  target:     ATmega169 @ 4 MHz in Honnywell Rondostat HR20E
+ *
+ *  compiler:   WinAVR-20071221
+ *              avr-libc 1.6.0
+ *              GCC 4.2.2
+ *
+ *  copyright:  2008 Dario Carluccio (hr20-at-carluccio-dot-de)
+ *              2008 Jiri Dobry (jdobry-at-centrum-dot-cz)
+ *              2008 Mario Fischer (MarioFischer-at-gmx-dot-net)
+ *              2007 Michael Smola (Michael-dot-Smola-at-gmx-dot-net)
+ *
+ *  license:    This program is free software; you can redistribute it and/or
+ *              modify it under the terms of the GNU Library General Public
+ *              License as published by the Free Software Foundation; either
+ *              version 2 of the License, or (at your option) any later version.
+ *
+ *              This program is distributed in the hope that it will be useful,
+ *              but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *              MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ *              GNU General Public License for more details.
+ *
+ *              You should have received a copy of the GNU General Public License
+ *              along with this program. If not, see http:*www.gnu.org/licenses
+ */
+
+/*
+ * \file       rfm.h
+ * \brief      functions to control the RFM12 Radio Transceiver Module
+ * \author     Mario Fischer <MarioFischer-at-gmx-dot-net>; Michael Smola <Michael-dot-Smola-at-gmx-dot-net>
+ * \date       $Date: 2010/04/17 17:57:02 $
+ * $Rev: 260 $
+ */
+
+
+//#pragma once // multi-iclude prevention. gcc knows this pragma
+#ifndef rfm_H
+#define rfm_H
+
+
+#define RFM_SPI_16(OUTVAL)            rfm_spi16(OUTVAL) //<! a function that gets a uint16_t (clocked out value) and returns a uint16_t (clocked in value)
+
+#define RFM_CLK_OUTPUT 0
+
+/*
+#define RFM_TESTPIN_INIT
+#define RFM_TESTPIN_ON
+#define RFM_TESTPIN_OFF
+#define RFM_TESTPIN_TOG
+
+#define RFM_CONFIG_DISABLE            0x00 //<! RFM_CONFIG_*** are combinable flags, what the RFM shold do
+#define RFM_CONFIG_BROADCASTSTATUS    0x01 //<! Flag that enables the HR20's status broadcast every minute
+
+#define RFM_CONFIG_ENABLEALL        0xff
+*/
+
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// RFM status bits
+//
+///////////////////////////////////////////////////////////////////////////////
+
+// Interrupt bits, latched ////////////////////////////////////////////////////
+
+#define RFM_STATUS_FFIT 0x8000 // RX FIFO reached the progr. number of bits
+                               // Cleared by any FIFO read method
+
+#define RFM_STATUS_RGIT 0x8000 // TX register is ready to receive
+                               // Cleared by TX write
+
+#define RFM_STATUS_POR  0x4000 // Power On reset
+                               // Cleared by read status
+
+#define RFM_STATUS_RGUR 0x2000 // TX register underrun, register over write
+                               // Cleared by read status
+
+#define RFM_STATUS_FFOV 0x2000 // RX FIFO overflow
+                               // Cleared by read status
+
+#define RFM_STATUS_WKUP 0x1000 // Wake up timer overflow
+                               // Cleared by read status
+
+#define RFM_STATUS_EXT  0x0800 // Interupt changed to low
+                               // Cleared by read status
+
+#define RFM_STATUS_LBD  0x0400 // Low battery detect
+
+// Status bits ////////////////////////////////////////////////////////////////
+
+#define RFM_STATUS_FFEM 0x0200 // FIFO is empty
+#define RFM_STATUS_ATS  0x0100 // TX mode: Strong enough RF signal
+#define RFM_STATUS_RSSI 0x0100 // RX mode: signal strength above programmed limit
+#define RFM_STATUS_DQD  0x0080 // Data Quality detector output
+#define RFM_STATUS_CRL  0x0040 // Clock recovery lock
+#define RFM_STATUS_ATGL 0x0020 // Toggling in each AFC cycle
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 1. Configuration Setting Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_CONFIG               0x8000
+
+#define RFM_CONFIG_EL            0x8080 // Enable TX Register
+#define RFM_CONFIG_EF            0x8040 // Enable RX FIFO buffer
+#define RFM_CONFIG_BAND_315      0x8000 // Frequency band
+#define RFM_CONFIG_BAND_433      0x8010
+#define RFM_CONFIG_BAND_868      0x8020
+#define RFM_CONFIG_BAND_915      0x8030
+#define RFM_CONFIG_X_8_5pf       0x8000 // Crystal Load Capacitor
+#define RFM_CONFIG_X_9_0pf       0x8001
+#define RFM_CONFIG_X_9_5pf       0x8002
+#define RFM_CONFIG_X_10_0pf      0x8003
+#define RFM_CONFIG_X_10_5pf      0x8004
+#define RFM_CONFIG_X_11_0pf      0x8005
+#define RFM_CONFIG_X_11_5pf      0x8006
+#define RFM_CONFIG_X_12_0pf      0x8007
+#define RFM_CONFIG_X_12_5pf      0x8008
+#define RFM_CONFIG_X_13_0pf      0x8009
+#define RFM_CONFIG_X_13_5pf      0x800A
+#define RFM_CONFIG_X_14_0pf      0x800B
+#define RFM_CONFIG_X_14_5pf      0x800C
+#define RFM_CONFIG_X_15_0pf      0x800D
+#define RFM_CONFIG_X_15_5pf      0x800E
+#define RFM_CONFIG_X_16_0pf      0x800F
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 2. Power Management Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_POWER_MANAGEMENT     0x8200
+
+#define RFM_POWER_MANAGEMENT_ER  0x8280 // Enable receiver
+#define RFM_POWER_MANAGEMENT_EBB 0x8240 // Enable base band block
+#define RFM_POWER_MANAGEMENT_ET  0x8220 // Enable transmitter
+#define RFM_POWER_MANAGEMENT_ES  0x8210 // Enable synthesizer
+#define RFM_POWER_MANAGEMENT_EX  0x8208 // Enable crystal oscillator
+#define RFM_POWER_MANAGEMENT_EB  0x8204 // Enable low battery detector
+#define RFM_POWER_MANAGEMENT_EW  0x8202 // Enable wake-up timer
+#define RFM_POWER_MANAGEMENT_DC  0x8201 // Disable clock output of CLK pin
+
+#ifndef RFM_CLK_OUTPUT
+    #error RFM_CLK_OUTPUT must be defined to 0 or 1
+#endif
+#if RFM_CLK_OUTPUT
+    #define RFM_TX_ON_PRE() RFM_SPI_16( \
+                                RFM_POWER_MANAGEMENT_ES | \
+                                RFM_POWER_MANAGEMENT_EX )
+    #define RFM_TX_ON()     RFM_SPI_16( \
+                                RFM_POWER_MANAGEMENT_ET | \
+                                RFM_POWER_MANAGEMENT_ES | \
+                                RFM_POWER_MANAGEMENT_EX )
+    #define RFM_RX_ON()     RFM_SPI_16( \
+                                RFM_POWER_MANAGEMENT_ER | \
+                                RFM_POWER_MANAGEMENT_EBB | \
+                                RFM_POWER_MANAGEMENT_ES | \
+                                RFM_POWER_MANAGEMENT_EX )
+    #define RFM_OFF()       RFM_SPI_16( \
+                                RFM_POWER_MANAGEMENT_EX )
+#else
+    #define RFM_TX_ON_PRE() RFM_SPI_16( \
+                                RFM_POWER_MANAGEMENT_DC | \
+                                RFM_POWER_MANAGEMENT_ES | \
+                                RFM_POWER_MANAGEMENT_EX )
+    #define RFM_TX_ON()     RFM_SPI_16( \
+                                RFM_POWER_MANAGEMENT_DC | \
+                                RFM_POWER_MANAGEMENT_ET | \
+                                RFM_POWER_MANAGEMENT_ES | \
+                                RFM_POWER_MANAGEMENT_EX )
+    #define RFM_RX_ON()     RFM_SPI_16( \
+                                RFM_POWER_MANAGEMENT_DC  | \
+                                RFM_POWER_MANAGEMENT_ER | \
+                                RFM_POWER_MANAGEMENT_EBB | \
+                                RFM_POWER_MANAGEMENT_ES | \
+                                RFM_POWER_MANAGEMENT_EX )
+    #define RFM_OFF()       RFM_SPI_16(RFM_POWER_MANAGEMENT_DC)
+#endif
+///////////////////////////////////////////////////////////////////////////////
+//
+// 3. Frequency Setting Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_FREQUENCY            0xA000
+
+#define RFM_FREQ_315Band(v) (uint16_t)((v/10.0-31)*4000)
+#define RFM_FREQ_433Band(v) (uint16_t)((v/10.0-43)*4000)
+#define RFM_FREQ_868Band(v) (uint16_t)((v/20.0-43)*4000)
+#define RFM_FREQ_915Band(v) (uint16_t)((v/30.0-30)*4000)
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 4. Data Rate Command
+//
+/////////////////////////////////////////////////////////////////////////////////
+
+#define RFM_BAUD_RATE            9600
+
+#define RFM_DATA_RATE            0xC600
+
+#define RFM_DATA_RATE_CS         0xC680
+#define RFM_DATA_RATE_4800       0xC647
+#define RFM_DATA_RATE_9600       0xC623
+#define RFM_DATA_RATE_19200      0xC611
+#define RFM_DATA_RATE_38400      0xC608
+#define RFM_DATA_RATE_57600      0xC605
+
+#define RFM_SET_DATARATE(baud)        ( ((baud)<5400) ? (RFM_DATA_RATE_CS|((43104/(baud))-1)) : (RFM_DATA_RATE|((344828UL/(baud))-1)) )
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 5. Receiver Control Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_RX_CONTROL           0x9000
+
+#define RFM_RX_CONTROL_P20_INT   0x9000 // Pin20 = ExternalInt
+#define RFM_RX_CONTROL_P20_VDI   0x9400 // Pin20 = VDI out
+
+#define RFM_RX_CONTROL_VDI_FAST  0x9000 // fast   VDI Response time
+#define RFM_RX_CONTROL_VDI_MED   0x9100 // medium
+#define RFM_RX_CONTROL_VDI_SLOW  0x9200 // slow
+#define RFM_RX_CONTROL_VDI_ON    0x9300 // Always on
+
+#define RFM_RX_CONTROL_BW_400    0x9020 // bandwidth 400kHz
+#define RFM_RX_CONTROL_BW_340    0x9040 // bandwidth 340kHz
+#define RFM_RX_CONTROL_BW_270    0x9060 // bandwidth 270kHz
+#define RFM_RX_CONTROL_BW_200    0x9080 // bandwidth 200kHz
+#define RFM_RX_CONTROL_BW_134    0x90A0 // bandwidth 134kHz
+#define RFM_RX_CONTROL_BW_67     0x90C0 // bandwidth 67kHz
+
+#define RFM_RX_CONTROL_GAIN_0    0x9000 // LNA gain  0db
+#define RFM_RX_CONTROL_GAIN_6    0x9008 // LNA gain -6db
+#define RFM_RX_CONTROL_GAIN_14   0x9010 // LNA gain -14db
+#define RFM_RX_CONTROL_GAIN_20   0x9018 // LNA gain -20db
+
+#define RFM_RX_CONTROL_RSSI_103  0x9000 // DRSSI threshold -103dbm
+#define RFM_RX_CONTROL_RSSI_97   0x9001 // DRSSI threshold -97dbm
+#define RFM_RX_CONTROL_RSSI_91   0x9002 // DRSSI threshold -91dbm
+#define RFM_RX_CONTROL_RSSI_85   0x9003 // DRSSI threshold -85dbm
+#define RFM_RX_CONTROL_RSSI_79   0x9004 // DRSSI threshold -79dbm
+#define RFM_RX_CONTROL_RSSI_73   0x9005 // DRSSI threshold -73dbm
+//#define RFM_RX_CONTROL_RSSI_67   0x9006 // DRSSI threshold -67dbm // RF12B reserved
+//#define RFM_RX_CONTROL_RSSI_61   0x9007 // DRSSI threshold -61dbm // RF12B reserved
+
+#define RFM_RX_CONTROL_BW(baud)        (((baud)<8000) ? \
+                                    RFM_RX_CONTROL_BW_67 : \
+                                    ( \
+                                        ((baud)<30000) ? \
+                                        RFM_RX_CONTROL_BW_134 : \
+                                        RFM_RX_CONTROL_BW_200 \
+                                    ))
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 6. Data Filter Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_DATA_FILTER          0xC228
+
+#define RFM_DATA_FILTER_AL       0xC2A8 // clock recovery auto-lock
+#define RFM_DATA_FILTER_ML       0xC268 // clock recovery fast mode
+#define RFM_DATA_FILTER_DIG      0xC228 // data filter type digital
+#define RFM_DATA_FILTER_ANALOG   0xC238 // data filter type analog
+#define RFM_DATA_FILTER_DQD(level) (RFM_DATA_FILTER | (level & 0x7))
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 7. FIFO and Reset Mode Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_FIFO                 0xCA00
+
+#define RFM_FIFO_AL              0xCA04 // FIFO Start condition sync-word/always
+#define RFM_FIFO_FF              0xCA02 // Enable FIFO fill
+#define RFM_FIFO_DR              0xCA01 // Disable hi sens reset mode
+#define RFM_FIFO_IT(level)       (RFM_FIFO | (( (level) & 0xF)<<4))
+
+#define RFM_FIFO_OFF()            RFM_SPI_16(RFM_FIFO_IT(8) |               RFM_FIFO_DR)
+#define RFM_FIFO_ON()             RFM_SPI_16(RFM_FIFO_IT(8) | RFM_FIFO_FF | RFM_FIFO_DR)
+
+/////////////////////////////////////////////////////////////////////////////
+//
+// 8. Receiver FIFO Read
+//
+/////////////////////////////////////////////////////////////////////////////
+
+#define RFM_READ_FIFO()           (RFM_SPI_16(0xB000) & 0xFF)
+
+/////////////////////////////////////////////////////////////////////////////
+//
+// 9. AFC Command
+//
+/////////////////////////////////////////////////////////////////////////////
+
+#define RFM_AFC                  0xC400
+
+#define RFM_AFC_EN               0xC401
+#define RFM_AFC_OE               0xC402
+#define RFM_AFC_FI               0xC404
+#define RFM_AFC_ST               0xC408
+
+// Limits the value of the frequency offset register to the next values:
+
+#define RFM_AFC_RANGE_LIMIT_NO    0xC400 // 0: No restriction
+#define RFM_AFC_RANGE_LIMIT_15_16 0xC410 // 1: +15 fres to -16 fres
+#define RFM_AFC_RANGE_LIMIT_7_8   0xC420 // 2: +7 fres to -8 fres
+#define RFM_AFC_RANGE_LIMIT_3_4   0xC430 // 3: +3 fres to -4 fres
+
+// fres=2.5 kHz in 315MHz and 433MHz Bands
+// fres=5.0 kHz in 868MHz Band
+// fres=7.5 kHz in 915MHz Band
+
+#define RFM_AFC_AUTO_OFF         0xC400 // 0: Auto mode off (Strobe is controlled by microcontroller)
+#define RFM_AFC_AUTO_ONCE        0xC440 // 1: Runs only once after each power-up
+#define RFM_AFC_AUTO_VDI         0xC480 // 2: Keep the foffset only during receiving(VDI=high)
+#define RFM_AFC_AUTO_INDEPENDENT 0xC4C0 // 3: Keep the foffset value independently trom the state of the VDI signal
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 10. TX Configuration Control Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_TX_CONTROL           0x9800
+
+#define RFM_TX_CONTROL_POW_0     0x9800
+#define RFM_TX_CONTROL_POW_3     0x9801
+#define RFM_TX_CONTROL_POW_6     0x9802
+#define RFM_TX_CONTROL_POW_9     0x9803
+#define RFM_TX_CONTROL_POW_12    0x9804
+#define RFM_TX_CONTROL_POW_15    0x9805
+#define RFM_TX_CONTROL_POW_18    0x9806
+#define RFM_TX_CONTROL_POW_21    0x9807
+#define RFM_TX_CONTROL_MOD_15    0x9800
+#define RFM_TX_CONTROL_MOD_30    0x9810
+#define RFM_TX_CONTROL_MOD_45    0x9820
+#define RFM_TX_CONTROL_MOD_60    0x9830
+#define RFM_TX_CONTROL_MOD_75    0x9840
+#define RFM_TX_CONTROL_MOD_90    0x9850
+#define RFM_TX_CONTROL_MOD_105   0x9860
+#define RFM_TX_CONTROL_MOD_120   0x9870
+#define RFM_TX_CONTROL_MOD_135   0x9880
+#define RFM_TX_CONTROL_MOD_150   0x9890
+#define RFM_TX_CONTROL_MOD_165   0x98A0
+#define RFM_TX_CONTROL_MOD_180   0x98B0
+#define RFM_TX_CONTROL_MOD_195   0x98C0
+#define RFM_TX_CONTROL_MOD_210   0x98D0
+#define RFM_TX_CONTROL_MOD_225   0x98E0
+#define RFM_TX_CONTROL_MOD_240   0x98F0
+#define RFM_TX_CONTROL_MP        0x9900
+
+#define RFM_TX_CONTROL_MOD(baud)    (((baud)<8000) ? \
+                                    RFM_TX_CONTROL_MOD_45 : \
+                                    ( \
+                                        ((baud)<20000) ? \
+                                        RFM_TX_CONTROL_MOD_60 : \
+                                        ( \
+                                            ((baud)<30000) ? \
+                                            RFM_TX_CONTROL_MOD_75 : \
+                                            ( \
+                                                ((baud)<40000) ? \
+                                                RFM_TX_CONTROL_MOD_90 : \
+                                                RFM_TX_CONTROL_MOD_120 \
+                                            ) \
+                                        ) \
+                                    ))
+
+/////////////////////////////////////////////////////////////////////////////
+//
+// 11. Transmitter Register Write Command
+//
+/////////////////////////////////////////////////////////////////////////////
+
+//#define RFM_WRITE(byte)  RFM_SPI_16(0xB800 | ((byte) & 0xFF))
+#define RFM_WRITE(byte)  RFM_SPI_16(0xB800 | (byte) )
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 12. Wake-up Timer Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_WAKEUP_TIMER         0xE000
+#define RFM_WAKEUP_SET(time)     RFM_SPI_16(RFM_WAKEUP_TIMER | (time))
+
+#define RFM_WAKEUP_480s          (RFM_WAKEUP_TIMER |(11 << 8)| 234)
+#define RFM_WAKEUP_240s          (RFM_WAKEUP_TIMER |(10 << 8)| 234)
+#define RFM_WAKEUP_120s          (RFM_WAKEUP_TIMER |(9  << 8)| 234)
+#define RFM_WAKEUP_119s          (RFM_WAKEUP_TIMER |(9  << 8)| 232)
+
+#define RFM_WAKEUP_60s           (RFM_WAKEUP_TIMER |(8 << 8) | 235)
+#define RFM_WAKEUP_59s           (RFM_WAKEUP_TIMER |(8 << 8) | 230)
+
+#define RFM_WAKEUP_30s           (RFM_WAKEUP_TIMER |(7 << 8) | 235)
+#define RFM_WAKEUP_29s           (RFM_WAKEUP_TIMER |(7 << 8) | 227)
+
+#define RFM_WAKEUP_8s            (RFM_WAKEUP_TIMER |(5 << 8) | 250)
+#define RFM_WAKEUP_7s            (RFM_WAKEUP_TIMER |(5 << 8) | 219)
+#define RFM_WAKEUP_6s            (RFM_WAKEUP_TIMER |(6 << 8) |  94)
+#define RFM_WAKEUP_5s            (RFM_WAKEUP_TIMER |(5 << 8) | 156)
+#define RFM_WAKEUP_4s            (RFM_WAKEUP_TIMER |(5 << 8) | 125)
+#define RFM_WAKEUP_1s            (RFM_WAKEUP_TIMER |(2 << 8) | 250)
+#define RFM_WAKEUP_900ms         (RFM_WAKEUP_TIMER |(2 << 8) | 225)
+#define RFM_WAKEUP_800ms         (RFM_WAKEUP_TIMER |(2 << 8) | 200)
+#define RFM_WAKEUP_700ms         (RFM_WAKEUP_TIMER |(2 << 8) | 175)
+#define RFM_WAKEUP_600ms         (RFM_WAKEUP_TIMER |(2 << 8) | 150)
+#define RFM_WAKEUP_500ms         (RFM_WAKEUP_TIMER |(2 << 8) | 125)
+#define RFM_WAKEUP_400ms         (RFM_WAKEUP_TIMER |(2 << 8) | 100)
+#define RFM_WAKEUP_300ms         (RFM_WAKEUP_TIMER |(2 << 8) |  75)
+#define RFM_WAKEUP_200ms         (RFM_WAKEUP_TIMER |(2 << 8) |  50)
+#define RFM_WAKEUP_100ms         (RFM_WAKEUP_TIMER |(2 << 8) |  25)
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 13. Low Duty-Cycle Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_LOW_DUTY_CYCLE       0xC800
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 14. Low Battery Detector Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_LOW_BATT_DETECT           0xC000
+#define RFM_LOW_BATT_DETECT_D_1MHZ    0xC000
+#define RFM_LOW_BATT_DETECT_D_1_25MHZ 0xC020
+#define RFM_LOW_BATT_DETECT_D_1_66MHZ 0xC040
+#define RFM_LOW_BATT_DETECT_D_2MHZ    0xC060
+#define RFM_LOW_BATT_DETECT_D_2_5MHZ  0xC080
+#define RFM_LOW_BATT_DETECT_D_3_33MHZ 0xC0A0
+#define RFM_LOW_BATT_DETECT_D_5MHZ    0xC0C0
+#define RFM_LOW_BATT_DETECT_D_10MHZ   0xC0E0
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// 15. Status Read Command
+//
+///////////////////////////////////////////////////////////////////////////////
+
+#define RFM_READ_STATUS()       RFM_SPI_16(0x0000)
+#define RFM_READ_STATUS_FFIT()  SPI_1 (0x00)
+#define RFM_READ_STATUS_RGIT    RFM_READ_STATUS_FFIT
+
+///////////////////////////////////////////////////////////////////////////////
+
+// RFM air protocol flags:
+
+#define RFMPROTO_FLAGS_BITASK_PACKETTYPE        0b11000000 //!< the uppermost 2 bits of the flags field encode the packettype
+#define RFMPROTO_FLAGS_PACKETTYPE_BROADCAST        0b00000000 //!< broadcast packettype (message from hr20, protocol; step 1)
+#define RFMPROTO_FLAGS_PACKETTYPE_COMMAND        0b01000000 //!< command packettype (message to hr20, protocol; step 2)
+#define RFMPROTO_FLAGS_PACKETTYPE_REPLY            0b10000000 //!< reply packettype (message from hr20, protocol; step 3)
+#define RFMPROTO_FLAGS_PACKETTYPE_SPECIAL        0b11000000 //!< currently unused packettype
+
+#define RFMPROTO_FLAGS_BITASK_DEVICETYPE        0b00011111 //!< the lowermost 5 bytes denote the device type. this way other sensors and actors may coexist
+#define RFMPROTO_FLAGS_DEVICETYPE_OPENHR20        0b00010100 //!< topen HR20 device type. 10100 is for decimal 20
+
+#define RFMPROTO_IS_PACKETTYPE_BROADCAST(FLAGS)    ( RFMPROTO_FLAGS_PACKETTYPE_BROADCAST == ((FLAGS) & RFMPROTO_FLAGS_BITASK_PACKETTYPE) )
+#define RFMPROTO_IS_PACKETTYPE_COMMAND(FLAGS)    ( RFMPROTO_FLAGS_PACKETTYPE_COMMAND   == ((FLAGS) & RFMPROTO_FLAGS_BITASK_PACKETTYPE) )
+#define RFMPROTO_IS_PACKETTYPE_REPLY(FLAGS)        ( RFMPROTO_FLAGS_PACKETTYPE_REPLY     == ((FLAGS) & RFMPROTO_FLAGS_BITASK_PACKETTYPE) )
+#define RFMPROTO_IS_PACKETTYPE_SPECIAL(FLAGS)    ( RFMPROTO_FLAGS_PACKETTYPE_SPECIAL   == ((FLAGS) & RFMPROTO_FLAGS_BITASK_PACKETTYPE) )
+#define RFMPROTO_IS_DEVICETYPE_OPENHR20(FLAGS)    ( RFMPROTO_FLAGS_DEVICETYPE_OPENHR20  == ((FLAGS) & RFMPROTO_FLAGS_BITASK_DEVICETYPE) )
+
+///////////////////////////////////////////////////////////////////////////////
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r 0bbf2f16da9c Sensors.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors.h	Fri Nov 18 18:23:33 2011 +0000
@@ -0,0 +1,100 @@
+
+//In here be functions related to Sensors
+
+//
+void setupaccel() {
+    i2c.start();
+    i2c.write(ACCADDRESS << 1 | 0);
+    i2c.write(7);
+    i2c.write(1);
+    i2c.stop();
+}
+
+//
+void getaccel(signed char returnbuff[3]) {
+    i2c.start();
+    i2c.write(ACCADDRESS << 1 | 0);
+    i2c.write(0);
+    i2c.stop();
+
+    //Read the axis data
+    i2c.read(ACCADDRESS << 1 | 1, (char*)returnbuff, 3);
+
+    for (int i = 0; i < 3; i++) {
+        returnbuff[i] = returnbuff[i] << 2;
+        returnbuff[i] /= 4;
+    }
+
+}
+
+void calibgyro() {
+
+    //activating the wiimotion +
+    i2c.start();
+    //device address | write
+    i2c.write(0x53 << 1 | 0);
+    //register address
+    i2c.write(0xfe);
+    //data
+    i2c.write(0x04);
+    i2c.stop();
+
+    for (int i = 0; i < 100; i++) {
+        //sending a 0x00 to flag that we want data
+        i2c.start();
+        //device address | write (note new address)
+        i2c.write(0x52 << 1 | 0);
+        //send 0x00
+        i2c.write(0x00);
+        i2c.stop();
+
+        //reading the data
+        char wmpdata[6];
+        i2c.read(0x52 << 1 | 1, wmpdata, 6);
+        //pc.printf("%x %x %x %x %x %x\r\n",  wmpdata[0], wmpdata[1], wmpdata[2], wmpdata[3], wmpdata[4], wmpdata[5]);
+
+        gyrcalib[0] += (((wmpdata[3] >> 2) << 8) + wmpdata[0]) / 100;
+        gyrcalib[1] += (((wmpdata[4] >> 2) << 8) + wmpdata[1]) / 100;
+        gyrcalib[2] += (((wmpdata[5] >> 2) << 8) + wmpdata[2]) / 100;
+
+        wait(0.05);
+
+    }
+
+}
+
+void getgyros(float gyrodata[3]) {
+    //sending a 0x00 to flag that we want data
+    i2c.start();
+    //device address | write (note new address)
+    i2c.write(0x52 << 1 | 0);
+    //send 0x00
+    i2c.write(0x00);
+    i2c.stop();
+
+    //reading the data
+    char wmpdata[6];
+    i2c.read(0x52 << 1 | 1, wmpdata, 6);
+    
+    //detect if we ever went into fast mode
+    bool fastdiscard = !(wmpdata[3] & 0x02 && wmpdata[4] & 0x02 && wmpdata[3] & 0x01);
+    
+    if (fastdiscard)
+        myled = 1;
+    else {
+        gyrodata[0] = RPSPUNIT * -(((wmpdata[3] >> 2) << 8) + wmpdata[0] - gyrcalib[0]);
+        gyrodata[1] = RPSPUNIT * (((wmpdata[4] >> 2) << 8) + wmpdata[1] - gyrcalib[1]);
+        gyrodata[2] = RPSPUNIT * (((wmpdata[5] >> 2) << 8) + wmpdata[2] - gyrcalib[2]);
+    }
+
+    //pc.printf("yaw: %f, pitch: %f, roll: %f\r\n", gyrodata[0], gyrodata[1], gyrodata[2]);
+
+    //wait(0.05);
+
+}
+
+#ifdef ALTSENSOR
+float getaltsensor() {
+    return altsensor * 
+}
+#endif
\ No newline at end of file

diff -r 000000000000 -r 0bbf2f16da9c System.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/System.h	Fri Nov 18 18:23:33 2011 +0000
@@ -0,0 +1,32 @@
+
+//General system functions
+
+#include <string>
+
+//
+void kill(string reason = "Killed for no reason!") {
+    PWMleft = NOPOWER - 0.004;
+    PWMright = NOPOWER - 0.004;
+    PWMfront = NOPOWER - 0.004;
+    PWMrear = NOPOWER - 0.004;
+    //TODO: send this over RF as well..
+    pc.printf("%s\r\n", reason.c_str());
+    /*wait(0.5);
+    left = 0;
+    right = 0;
+    front = 0;
+    rear = 0;
+    */
+    while (1);
+}
+
+//
+void wdt() {
+    if (!loopalive)
+        kill("Killed by WDT: loop has stalled");
+    if (!commandsalive)
+        kill("Killed by WDT: command starvation");
+
+    loopalive = 0;
+    commandsalive = 0;
+}

diff -r 000000000000 -r 0bbf2f16da9c main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Nov 18 18:23:33 2011 +0000
@@ -0,0 +1,224 @@
+/*
+TODO: Organize functions n files
+TODO: Add Altsensor
+TODO: Make yaw from P to PID
+TODO: Make radiolink bidirectional
+
+Positive X is forward, positive Y is left, positive Z is up!
+
+Wii motion plus wireing!
+blue: gnd
+black: also gnd
+green: data
+brown: 3v3
+red: clk
+
+
+*/
+
+#include "mbed.h"
+#include "GlobalsNDefines.h"
+#include <algorithm>
+#include "Sensors.h"
+#include "motors.h"
+#include "System.h"
+#include "RF12B.h"
+#include "main_init.h"
+#include <string>
+
+
+//Prototypes. TODO: Stick the functions into seperate files.
+void readcommands(float commands[5], float oldcommands[5]);
+
+int main() {
+
+    //Initialize motors, sensors, etc.
+    main_init();
+
+    //GOGOGO
+    wait(0.5);
+    pc.printf("Starting main control loop\r\n");
+
+    //Variables belonging to the control loop
+    float G_roll_P = 55.0;//60.0;
+    float G_roll_I = 46.0;
+    float G_roll_D = 53.0;
+    float G_pitch_P = 55.0;
+    float G_pitch_I = 46.0;
+    float G_pitch_D = 53.0;
+    float G_yaw_P = 70.0;
+    float rintegral = 0;
+    float pintegral = 0;
+    float yaw = 0;
+    float pitch = 0;
+    float roll = 0;
+    float eroll = 0;
+    float epitch = 0;
+    float apitch = 0;
+    float aroll = 0;
+    float gyros[3];
+    float depitch = 0;
+    float deroll = 0;
+    float altitude = 0.0; //Meters
+    float commands[5] = {0.0, 0.0, 0.0, 0.0, 0.0};
+    float oldcommands[5] = {0.0, 0.0, 0.0, 0.0, 0.0};
+    signed char axisbuff[3];
+
+    //Last thing before entering loop: start the watchdog timer
+    watchdog.attach(&wdt, 0.2);
+
+    while (true) {
+
+        //pc.printf("power is %f\r\n", (float));
+        //pc.printf("yaw is %f, yawrate is %f\r\n", yaw, gyros[0]);
+        //pc.printf("ptich is %f, pitchrate is %f\r\n", pitch, gyros[1]);
+        //pc.printf("roll is %f, rollrate is %f\r\n", roll, gyros[2]);
+        //pc.printf("\r\n");
+
+        //1 second inner loop to enable outer loop to print messages every 1s.
+        //WARNING any delays, including printing, in the outer loop will deteriorate loop performace
+        for (int looparound = 0; looparound < 100; looparound++) {
+
+            /*
+            //Stop button kill
+            if (!Nkill) {
+                kill("Killed by E-stop button");
+            }
+            */
+
+            /*
+            //Commands from PC Serial
+            if (pc.readable())
+                readcommandstemp(commands, oldcommands);
+            */
+
+            //Commands from RF link
+            if (radiolink.available() >= 5)
+                readcommands(commands, oldcommands);
+
+            //Get sensor readings
+            getaccel(axisbuff);
+            getgyros(gyros);
+#ifdef ALTSENSOR
+            altitude = getaltsensor();
+#endif
+
+            //One dimensional (small angle aproximation) linear to angular decoding
+            apitch = 0.05 + atan2(-(float)axisbuff[0], ((axisbuff[2] < 0) ? -1 : 1) * sqrt(pow((float)axisbuff[1], 2) + pow((float)axisbuff[2], 2)));
+            aroll = atan2(-(float)axisbuff[1], ((axisbuff[2] < 0) ? -1 : 1) * sqrt(pow((float)axisbuff[0], 2) + pow((float)axisbuff[2], 2)));
+
+            //low pass filter accelero and integrate gyros (note only 1 dimensional)
+            pitch += (apitch - pitch) / ACCDECAY + gyros[1] * LOOPTIME;
+            roll += (aroll - roll) / ACCDECAY + gyros[2] * LOOPTIME;
+            
+            if (!integratestop){
+                yaw += gyros[0] * LOOPTIME;
+    
+                //Add yaw control (warning this approach will not work for navigation: makes the copter think it is always pointing north)
+                yaw -= commands[3];
+            }
+
+            //Errors
+            epitch = pitch - commands[0];
+            eroll = roll - commands[1];
+
+            //Error derivatives
+            depitch = gyros[1] - (commands[0] - oldcommands[0]) / COMMANDPERIOD;
+            deroll = gyros[2] - (commands[1] - oldcommands[1]) / COMMANDPERIOD;
+
+            //Average power to motors
+#ifdef ALTSENSOR
+            float power = commands[2] - (altitude * ALTGAIN);
+#else
+            float power = commands[2];
+#endif
+
+            /*
+            //For some reason, the min function is not defined for float and float literal. Defining a temp float.
+            float maxyawwtf = YAWCAP;
+            SOLVED: use x.xf, regular = double..
+            */
+
+            //R+L corr term
+            float LRcorr = eroll * G_roll_P
+                           + rintegral * G_roll_I
+                           + deroll * G_roll_D;
+
+            //F+R corr term
+            float FRcorr = epitch * G_pitch_P
+                           + pintegral * G_pitch_I
+                           + depitch * G_pitch_D;
+
+            //yaw corr term
+            float yawcorr = min(yaw, YAWCAP) * G_yaw_P;
+
+            // Setting motor speeds
+            leftM(power + LRcorr - yawcorr);
+            rightM(power - LRcorr - yawcorr);
+            frontM(power + FRcorr + yawcorr);
+            rearM(power - FRcorr + yawcorr);
+
+            //integrate
+            if (!integratestop){
+                rintegral += eroll * LOOPTIME;
+                pintegral += epitch * LOOPTIME;
+            }
+
+            //pet the dog
+            loopalive = 1;
+
+            wait(LOOPTIME);
+        }
+    }
+}
+
+void readcommands(float commands[5], float oldcommands[5]) {
+    //pc.printf("We try to read commands\r\n");
+    signed char data[5];
+    radiolink.read((unsigned char*)data, 5);
+
+    for (int i = 0; i < 5; i++) {
+        oldcommands[i] = commands[i];
+
+        switch (i) {
+            case 0:
+                commands[0] = data[i] * 0.0020;
+                break;
+            case 1:
+                commands[1] = data[i] * -0.0020;
+                break;
+            case 2: {
+                float throttle = (unsigned char)data[i];
+                //commands[2] = (char)data[i] * 6.6 - (char)data[i] * (char)data[i] * 2.4; // / 5000.0;
+                commands[2] = throttle * 5.0f - 0.01f * throttle * throttle;
+                //pc.printf("throttle now at %f grams per motor\r\n", commands[2]);
+                if (commands[2] < 250)
+                    integratestop = 1;
+                else
+                    integratestop = 0;
+                /*if (data[i] < -100) {
+                    kill("Killed by throttle low position");
+                    commands[4] = 1.0;
+                }*/
+                break;
+                }
+            case 3:
+                commands[3] = data[i] * 0.0005;
+                break;
+            case 4:
+                commands[4] = max((float)data[i], commands[4]);
+                if (commands[4] > 0.1) {
+                    //pc.printf("Estopcommand was %f, last data was %d\r\n", commands[4], data[i]);
+                    kill("Killed by controller E-stop");
+                }
+                break;
+        }
+    }
+
+    commandsalive = 1;
+}
+
+
+
+
+

diff -r 000000000000 -r 0bbf2f16da9c main_init.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main_init.h	Fri Nov 18 18:23:33 2011 +0000
@@ -0,0 +1,36 @@
+
+
+void main_init() {
+
+    //Emergency stop button
+    Nkill.mode(PullDown);
+
+    pc.printf("Hello World!\r\n");
+
+    initmotor();
+
+    /*
+    //Init motor pwm
+    left.period_ms(20);
+    left = 0.01;
+    right.period_ms(20);
+    right = 0.01;
+
+    front.period_ms(20);
+    front = 0.01;
+    rear.period_ms(20);
+    rear = 0.01;
+    */
+
+    pc.printf("Motors initialized\r\n");
+
+    //Setup accelerometer
+    setupaccel();
+    pc.printf("Accelorometer initialized\r\n");
+
+    //Setup gyros, and calibrate them
+    wait(0.5);
+    calibgyro();
+    pc.printf("Gyros initialized and calibrated\r\n");
+
+}
\ No newline at end of file

diff -r 000000000000 -r 0bbf2f16da9c mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Nov 18 18:23:33 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/63bcd7ba4912

diff -r 000000000000 -r 0bbf2f16da9c motors.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motors.h	Fri Nov 18 18:23:33 2011 +0000
@@ -0,0 +1,52 @@
+
+void initmotor() {
+    PWMleft.period_ms(20);
+    PWMleft = NOPOWER;
+    PWMright.period_ms(20);
+    PWMright = NOPOWER;
+
+    PWMfront.period_ms(20);
+    PWMfront = NOPOWER;
+    PWMrear.period_ms(20);
+    PWMrear = NOPOWER;
+}
+
+void leftM(float thrust) {
+    float output = (542 + 0.462 * thrust) / 10000;
+    if (output > 0.0950) {
+        PWMleft = 0.0950;
+        motormaxled = 1;
+    } else {
+        PWMleft = output;
+    }
+}
+
+void rightM(float thrust) {
+    float output = (542 + 0.462 * thrust) / 10000;
+    if (output > 0.0950) {
+        PWMright = 0.0950;
+        motormaxled = 1;
+    } else {
+        PWMright = output;
+    }
+}
+
+void frontM(float thrust) {
+    float output = (542 + 0.462 * thrust) / 10000;
+    if (output > 0.950) {
+        PWMfront = 0.0950;
+        motormaxled = 1;
+    } else {
+        PWMfront = output;
+    }
+}
+
+void rearM(float thrust) {
+    float output = (542 + 0.462 * thrust) / 10000;
+    if (output > 0.0950) {
+        PWMrear = 0.0950;
+        motormaxled = 1;
+    } else {
+        PWMrear = output;
+    }
+}