This is for ICRS\' second generation Quadcopter

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
madcowswe
Date:
Fri Nov 18 18:23:33 2011 +0000
Commit message:

Changed in this revision

GlobalsNDefines.h Show annotated file Show diff for this revision Revisions of this file
RF12B/RF12B.cpp Show annotated file Show diff for this revision Revisions of this file
RF12B/RF12B.h Show annotated file Show diff for this revision Revisions of this file
RF12B/RFSerial.cpp Show annotated file Show diff for this revision Revisions of this file
RF12B/RFSerial.h Show annotated file Show diff for this revision Revisions of this file
RF12B/RF_defs.h Show annotated file Show diff for this revision Revisions of this file
Sensors.h Show annotated file Show diff for this revision Revisions of this file
System.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
main_init.h Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
motors.h Show annotated file Show diff for this revision Revisions of this file
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;
+    }
+}