Revision 0:711905e937b9, committed 2011-07-24

Comitter:

fyazgan

Date:

Sun Jul 24 19:49:51 2011 +0000

Commit message:

Changed in this revision

diff -r 000000000000 -r 711905e937b9 ADXL345.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ADXL345.lib	Sun Jul 24 19:49:51 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/ADXL345/#bd8f0f20f433

diff -r 000000000000 -r 711905e937b9 EthernetNetIf.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EthernetNetIf.lib	Sun Jul 24 19:49:51 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/donatien/code/EthernetNetIf/#bc7df6da7589

diff -r 000000000000 -r 711905e937b9 HMC6352.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC6352.lib	Sun Jul 24 19:49:51 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/HMC6352/#83c0cb554099

diff -r 000000000000 -r 711905e937b9 IMUTICKER/IMUTICKER.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMUTICKER/IMUTICKER.h	Sun Jul 24 19:49:51 2011 +0000
@@ -0,0 +1,277 @@
+/**
+ * IMU filter example.
+ *
+ * Calculate the roll, pitch and yaw angles.
+ */
+#include "IMUfilter.h"
+#include "ADXL345.h"
+#include "ITG3200.h"
+
+//Gravity at Earth's surface in m/s/s
+#define g0 9.812865328
+//Number of samples to average.
+#define SAMPLES 4
+//Number of samples to be averaged for a null bias calculation
+//during calibration.
+#define CALIBRATION_SAMPLES 128
+//Convert from radians to degrees.
+#define toDegrees(x) (x * 57.2957795)
+//Convert from degrees to radians.
+#define toRadians(x) (x * 0.01745329252)
+//ITG-3200 sensitivity is 14.375 LSB/(degrees/sec).
+#define GYROSCOPE_GAIN (1 / 14.375)
+//Full scale resolution on the ADXL345 is 4mg/LSB.
+#define ACCELEROMETER_GAIN (0.004 * g0)
+//Sampling gyroscope at 200Hz.
+#define GYRO_RATE   0.005
+//Sampling accelerometer at 200Hz.
+#define ACC_RATE    0.005
+//Updating filter at 40Hz.
+#define FILTER_RATE 0.1
+
+Serial pc(USBTX, USBRX);
+//At rest the gyroscope is centred around 0 and goes between about
+//-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly
+//5/15 = 0.3 degrees/sec.
+IMUfilter imuFilter(FILTER_RATE, 0.3);
+ADXL345 accelerometer(p5, p6, p7, p8);
+ITG3200 gyroscope(p9, p10);
+Ticker accelerometerTicker;
+Ticker gyroscopeTicker;
+Ticker filterTicker;
+
+//Offsets for the gyroscope.
+//The readings we take when the gyroscope is stationary won't be 0, so we'll
+//average a set of readings we do get when the gyroscope is stationary and
+//take those away from subsequent readings to ensure the gyroscope is offset
+//or "biased" to 0.
+double w_xBias;
+double w_yBias;
+double w_zBias;
+
+//Offsets for the accelerometer.
+//Same as with the gyroscope.
+double a_xBias;
+double a_yBias;
+double a_zBias;
+
+//Accumulators used for oversampling and then averaging.
+volatile double a_xAccumulator = 0;
+volatile double a_yAccumulator = 0;
+volatile double a_zAccumulator = 0;
+volatile double w_xAccumulator = 0;
+volatile double w_yAccumulator = 0;
+volatile double w_zAccumulator = 0;
+
+//Accelerometer and gyroscope readings for x, y, z axes.
+volatile double a_x;
+volatile double a_y;
+volatile double a_z;
+volatile double w_x;
+volatile double w_y;
+volatile double w_z;
+
+//Buffer for accelerometer readings.
+int readings[3];
+//Number of accelerometer samples we're on.
+int accelerometerSamples = 0;
+//Number of gyroscope samples we're on.
+int gyroscopeSamples = 0;
+
+/**
+ * Prototypes
+ */
+//Set up the ADXL345 appropriately.
+void initializeAcceleromter(void);
+//Calculate the null bias.
+void calibrateAccelerometer(void);
+//Take a set of samples and average them.
+void sampleAccelerometer(void);
+//Set up the ITG3200 appropriately.
+void initializeGyroscope(void);
+//Calculate the null bias.
+void calibrateGyroscope(void);
+//Take a set of samples and average them.
+void sampleGyroscope(void);
+//Update the filter and calculate the Euler angles.
+void filter(void);
+
+void initializeAccelerometer(void) {
+
+    //Go into standby mode to configure the device.
+    accelerometer.setPowerControl(0x00);
+    //Full resolution, +/-16g, 4mg/LSB.
+    accelerometer.setDataFormatControl(0x0B);
+    //200Hz data rate.
+    accelerometer.setDataRate(ADXL345_200HZ);
+    //Measurement mode.
+    accelerometer.setPowerControl(0x08);
+    //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
+    wait_ms(22);
+
+}
+
+void sampleAccelerometer(void) {
+
+    //Have we taken enough samples?
+    if (accelerometerSamples == SAMPLES) {
+
+        //Average the samples, remove the bias, and calculate the acceleration
+        //in m/s/s.
+        a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
+        a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
+        a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
+
+        a_xAccumulator = 0;
+        a_yAccumulator = 0;
+        a_zAccumulator = 0;
+        accelerometerSamples = 0;
+
+    } else {
+        //Take another sample.
+        accelerometer.getOutput(readings);
+
+        a_xAccumulator += (int16_t) readings[0];
+        a_yAccumulator += (int16_t) readings[1];
+        a_zAccumulator += (int16_t) readings[2];
+
+        accelerometerSamples++;
+
+    }
+
+}
+
+void calibrateAccelerometer(void) {
+
+    a_xAccumulator = 0;
+    a_yAccumulator = 0;
+    a_zAccumulator = 0;
+    
+    //Take a number of readings and average them
+    //to calculate the zero g offset.
+    for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
+
+        accelerometer.getOutput(readings);
+
+        a_xAccumulator += (int16_t) readings[0];
+        a_yAccumulator += (int16_t) readings[1];
+        a_zAccumulator += (int16_t) readings[2];
+
+        wait(ACC_RATE);
+
+    }
+
+    a_xAccumulator /= CALIBRATION_SAMPLES;
+    a_yAccumulator /= CALIBRATION_SAMPLES;
+    a_zAccumulator /= CALIBRATION_SAMPLES;
+
+    //At 4mg/LSB, 250 LSBs is 1g.
+    a_xBias = a_xAccumulator;
+    a_yBias = a_yAccumulator;
+    a_zBias = (a_zAccumulator - 250);
+
+    a_xAccumulator = 0;
+    a_yAccumulator = 0;
+    a_zAccumulator = 0;
+
+}
+
+void initializeGyroscope(void) {
+
+    //Low pass filter bandwidth of 42Hz.
+    gyroscope.setLpBandwidth(LPFBW_42HZ);
+    //Internal sample rate of 200Hz. (1kHz / 5).
+    gyroscope.setSampleRateDivider(4);
+
+}
+
+void calibrateGyroscope(void) {
+
+    w_xAccumulator = 0;
+    w_yAccumulator = 0;
+    w_zAccumulator = 0;
+
+    //Take a number of readings and average them
+    //to calculate the gyroscope bias offset.
+    for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
+
+        w_xAccumulator += gyroscope.getGyroX();
+        w_yAccumulator += gyroscope.getGyroY();
+        w_zAccumulator += gyroscope.getGyroZ();
+        wait(GYRO_RATE);
+
+    }
+
+    //Average the samples.
+    w_xAccumulator /= CALIBRATION_SAMPLES;
+    w_yAccumulator /= CALIBRATION_SAMPLES;
+    w_zAccumulator /= CALIBRATION_SAMPLES;
+
+    w_xBias = w_xAccumulator;
+    w_yBias = w_yAccumulator;
+    w_zBias = w_zAccumulator;
+
+    w_xAccumulator = 0;
+    w_yAccumulator = 0;
+    w_zAccumulator = 0;
+
+}
+
+void sampleGyroscope(void) {
+
+    //Have we taken enough samples?
+    if (gyroscopeSamples == SAMPLES) {
+
+        //Average the samples, remove the bias, and calculate the angular
+        //velocity in rad/s.
+        w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
+        w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
+        w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
+
+        w_xAccumulator = 0;
+        w_yAccumulator = 0;
+        w_zAccumulator = 0;
+        gyroscopeSamples = 0;
+
+    } else {
+        //Take another sample.
+        w_xAccumulator += gyroscope.getGyroX();
+        w_yAccumulator += gyroscope.getGyroY();
+        w_zAccumulator += gyroscope.getGyroZ();
+
+        gyroscopeSamples++;
+
+    }
+
+}
+
+void filter(void) {
+
+    //Update the filter variables.
+    imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
+    //Calculate the new Euler angles.
+    imuFilter.computeEuler();
+
+}
+
+void start() {
+
+    pc.printf("Starting IMU filter test...\n");
+
+    //Initialize inertial sensors.
+    initializeAccelerometer();
+    calibrateAccelerometer();
+    initializeGyroscope();
+    calibrateGyroscope();
+
+
+    //Set up timers.
+    //Accelerometer data rate is 200Hz, so we'll sample at this speed.
+    accelerometerTicker.attach(&sampleAccelerometer, 0.005);
+    //Gyroscope data rate is 200Hz, so we'll sample at this speed.
+    gyroscopeTicker.attach(&sampleGyroscope, 0.005);
+    //Update the filter variables at the correct rate.
+    filterTicker.attach(&filter, FILTER_RATE);
+   
+
+}

diff -r 000000000000 -r 711905e937b9 IMUfilter.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMUfilter.lib	Sun Jul 24 19:49:51 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/IMUfilter/#8a920397b510

diff -r 000000000000 -r 711905e937b9 ITG3200.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ITG3200.lib	Sun Jul 24 19:49:51 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/ITG3200/#b098d99dd81e

diff -r 000000000000 -r 711905e937b9 SCP1000/SCP1000.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SCP1000/SCP1000.cpp	Sun Jul 24 19:49:51 2011 +0000
@@ -0,0 +1,85 @@
+/**
+ * @section LICENSE
+ * This program is free software; you can redistribute it and/or modify 
+ * it under the terms of the GNU 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, write to the Free Software Foundation, Inc., 
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * @section DESCRIPTION
+ * Library to interface with the SCP1000 pressure and temperature sensor.
+ */
+
+#include "SCP1000.h"
+
+SCP1000::SCP1000(PinName mosi, PinName miso, PinName sclk, PinName cs)
+    : m_spi(mosi, miso, sclk)
+    , m_cs(cs) {
+    m_cs=1;
+    m_spi.frequency(500000); // the fastest of the sensor
+    m_spi.format(8, 0); // duda son dos palabras de 8 bits? 
+    wait(0.5);
+    //------------------------------------------------
+    // pc.printf("RESET\r\n");
+    write_register(0x06,0x01);
+    wait(0.5);
+
+    // pc.printf("Initialize High Resolution Constant Reading Mode\r\n");
+    write_register(0x03,0x0A);
+    wait(0.5);
+}
+
+unsigned long SCP1000::readPressure() {
+    unsigned long pressure_msb = read_register(PRESSURE);
+    pressure_msb &= 0x07;
+    unsigned long pressure_lsb = read_register16(PRESSURE_LSB);
+    unsigned long pressure = ((pressure_msb<<16)| pressure_lsb);
+    pressure /= 4;
+    
+    return pressure;
+}
+
+float SCP1000::readTemperature() {
+    float temp_in = read_register16(TEMP);
+    temp_in /= 20;
+    return temp_in;
+}
+
+ char SCP1000::read_register(char register_name) {
+    register_name <<=2;
+    register_name &= 0xFC;
+    m_cs=0; //Select SPI device
+    m_spi.write(register_name); //Send register location
+    char register_value=m_spi.write(0x00);
+    m_cs=1;
+    return register_value;  
+}
+
+void SCP1000::write_register(char register_name, char register_value) {
+    register_name <<= 2;
+    register_name |= 0x02; //le estamos diciendo que escriba
+    m_cs=0; //Select SPI device
+    m_spi.write(register_name); //Send register location
+    m_spi.write(register_value); //Send value to record into register
+    m_cs=1;
+}
+
+float SCP1000::read_register16(char register_name) {   
+    register_name <<= 2;
+    register_name &= 0xFC; //Read command
+    m_cs=0; //Select SPI Device
+    m_spi.write(register_name); //Write byte to device
+    int in_byte1 = m_spi.write(0x00);    
+    int in_byte2 = m_spi.write(0x00);
+    m_cs=1;
+    float in_word= (in_byte1<<=8) | (in_byte2);   
+    return(in_word);
+}
\ No newline at end of file

diff -r 000000000000 -r 711905e937b9 SCP1000/SCP1000.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SCP1000/SCP1000.h	Sun Jul 24 19:49:51 2011 +0000
@@ -0,0 +1,71 @@
+/**
+ * @section LICENSE
+ * This program is free software; you can redistribute it and/or modify 
+ * it under the terms of the GNU 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, write to the Free Software Foundation, Inc., 
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * @section DESCRIPTION
+ * Library to interface with the SCP1000 pressure and temperature sensor.
+ */
+
+#ifndef _SCP1000_H
+#define _SCP1000_H
+
+#include "mbed.h"
+
+/**
+ * Class to interface with the SCP1000 pressure and temperature sensor.
+ */
+class SCP1000 {
+    public:
+        /**
+         * Constructor.
+         *
+         * @param mosi SPI MOSI pin
+         * @param miso SPI MISO pin
+         * @param sclk SPI SCLK pin
+         * @param cs Chip select pin
+         */
+        SCP1000(PinName mosi, PinName miso, PinName sclk, PinName cs);
+        
+        ~SCP1000() { /* empty */ };
+        
+        /**
+         * Read the pressure.
+         *
+         * @returns The pressure in pascals.
+         */
+        unsigned long readPressure();
+        
+        /**
+         * Read the temperature.
+         *
+         * @returns The temperature in Celsius.
+         */
+        float readTemperature();
+        
+        
+    private:
+        static const char PRESSURE = 0x1F;   //Pressure 3 MSB
+        static const char PRESSURE_LSB = 0x20; //Pressure 16 LSB
+        static const char TEMP = 0x21;       //16 bit temp
+        SPI m_spi;
+        DigitalOut m_cs;
+        
+        char read_register(char register_name);
+        void write_register(char register_name, char register_value);
+        float read_register16(char register_name);
+
+};
+
+#endif // _SCP1000_H
\ No newline at end of file

diff -r 000000000000 -r 711905e937b9 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Jul 24 19:49:51 2011 +0000
@@ -0,0 +1,297 @@
+#include "mbed.h"
+#include "EthernetNetIf.h"
+#include "TCPSocket.h"
+#include "IMUTICKER.h"
+#include "SCP1000.h"
+#include "HMC6352.h"
+#include <iostream>
+#include <math.h>
+
+
+
+
+
+
+
+
+SCP1000 scp1(p11,p12,p13,p14);
+SCP1000 scp2(p11,p12,p13,p15);
+HMC6352 compass(p28, p27);
+
+
+//++++++++++++++++++++++++++Ethernet++++++++++++++++++++++++++++++
+int t0=0,t1=0,t2=0,t3=0;   
+Timer tmr;
+
+void ByteToChar(int bytes[29], char chars[29], unsigned int count) {
+    for (unsigned int i = 0; i < count; i++)
+        chars[i] = (char)bytes[i];
+}
+
+
+
+EthernetNetIf eth(
+    IpAddr(10,0,0,100), //IP Address
+    IpAddr(255,255,255,0), //Network Mask
+    IpAddr(10,0,0,1), //Gateway
+    IpAddr(10,0,0,1)  //DNS
+);
+
+
+// EthernetNetIf eth;
+
+DigitalOut connectionLED(LED2);
+DigitalOut statusLED(LED1);
+
+TCPSocket socket;
+
+void onTCPSocketEvent(TCPSocketEvent e) {
+    switch (e) {
+        case TCPSOCKET_CONNECTED: {
+            // Tells me that I'm connected to the server
+            pc.printf("Connected\r\n");
+            connectionLED = 1;
+            wait(0.5);
+ 
+        int b[29] = { 0, 0, 0, 0, 2,0,0,0,0,0,0,0,85,49 };
+        char m[29];
+        ByteToChar(b,m,29);
+        int sendError = socket.send(m,14);
+        }
+        break;
+
+        case TCPSOCKET_READABLE: {
+        
+        
+          // Get the data and send them back to pc
+            char rxBuffer[29];
+            int len = socket.recv(rxBuffer, sizeof(rxBuffer)-1);
+            if ( len != 0 ) {
+                rxBuffer[len] = '\0';
+
+                pc.printf("Received:\r\n%s\r\n");
+
+                int sendError = socket.send(rxBuffer,(strlen(rxBuffer) + 1));
+            }
+        }
+        break;
+
+        case TCPSOCKET_WRITEABLE: {
+
+
+            char m[29];
+            int b[29] = { 2, 0, 0, 0, 2,0,0,0,15,0,0,0,85,49,64,t0,t1,t2,t3,255,255,255,255,255,255,255,255,255,255 };
+
+            ByteToChar(b,m,27);
+
+            int sendError = socket.send(m,sizeof(m));
+ 
+
+        }
+        break;
+
+        case TCPSOCKET_CONTIMEOUT:
+        case TCPSOCKET_CONRST:
+        case TCPSOCKET_CONABRT:
+        case TCPSOCKET_ERROR: {
+            pc.printf("Connection Error: %i\r\n",e);
+            socket.close();
+        }
+        break;
+
+        case TCPSOCKET_DISCONNECTED: {
+            pc.printf("Disconnected\r\n");
+            socket.close();
+            connectionLED = 0;
+        }
+        break;
+    }
+}
+
+string binary( unsigned long n ) {
+    char     result[ (sizeof( unsigned long ) * 8) + 1 ];
+    unsigned index  = sizeof( unsigned long ) * 8;
+    result[ index ] = '\0';
+
+    do result[ --index ] = '0' + (n & 1);
+    while (n >>= 1);
+
+    return string( result + index );
+}
+
+
+int main() {
+    pc.baud(9600);
+
+    start();
+
+    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+
+//+++++++++++++++++++++++++++++ETHERNET+++++++++++++++++++++++++
+
+    // Ethernet setup, callback setup and server declaration
+    EthernetErr ethErr = eth.setup();
+    if (ethErr)
+        error("Error %d in setup.\r\n", ethErr);
+
+    socket.setOnEvent(&onTCPSocketEvent);
+    Host server(IpAddr(10, 0, 0, 7), 1000);
+    TCPSocketErr bindStatus = socket.connect(server);
+    if (bindStatus != TCPSOCKET_OK)
+    pc.printf("Bind error\r\n");
+    
+    tmr.start();
+
+
+    while (true) 
+    {
+    
+        Net::poll();
+        
+        
+                   if (tmr.read() > 2) 
+        {
+        tmr.reset();
+
+        statusLED = !statusLED;
+        
+        pc.printf("%f,%f,%f\n\r",toDegrees(imuFilter.getRoll()),toDegrees(imuFilter.getPitch()),toDegrees(imuFilter.getYaw()));
+        pc.printf("The pressure is %d Pa,temperature is %f C\n\r", scp1.readPressure(), scp1.readTemperature()/100);
+        pc.printf("The pressure is %d Pa,temperature is %f C\n\r", scp2.readPressure(), scp2.readTemperature()/100);
+        pc.printf("Heading is: %f", compass.sample() / 10.0);
+
+        float rol=toDegrees(imuFilter.getRoll());
+        int i=0;
+        int r[16];
+        double bol=3;
+        while (rol>2) {
+            bol=int(floor(rol/2));
+            r[i]=floor(rol-(bol*2));
+            rol=((rol-r[i])/2);
+            i=i+1;
+        }
+        bol=int(floor(rol/2));
+        r[i]=floor(rol-(bol*2));
+        r[i+1]=1;
+        for (int ii=i+2; ii<16; ii++) {
+            r[ii]=0;
+        }
+        int kov[16];
+        for (int ii=0; ii<8; ii++) {
+            kov[ii]=r[ii];
+            r[ii]=r[15-ii];
+            r[15-ii]=kov[ii];
+        }
+
+        rol=toDegrees(imuFilter.getPitch());
+        i=0;
+        int p[16];
+        bol=3;
+        while (rol>2) {
+            bol=(floor(rol/2));
+            p[i]=floor(rol-(bol*2));
+            rol=((rol-p[i])/2);
+            i=i+1;
+        }
+        bol=int(floor(rol/2));
+        p[i]=floor(rol-(bol*2));
+        p[i+1]=1;
+        for (int ii=i+2; ii<16; ii++) {
+            p[ii]=0;
+        }
+        for (int ii=0; ii<8; ii++) {
+            kov[ii]=p[ii];
+            p[ii]=p[15-ii];
+            p[15-ii]=kov[ii];
+        }
+
+        rol=toDegrees(imuFilter.getYaw());
+        i=0;
+        int y[16];
+        bol=3;
+        while (rol>2) {
+            bol=(floor(rol/2));
+            y[i]=floor(rol-(bol*2));
+            rol=((rol-y[i])/2);
+            i=i+1;
+        }
+        bol=(floor(rol/2));
+        y[i]=floor(rol-(bol*2));
+        y[i+1]=1;
+        for (int ii=i+2; ii<16; ii++) {
+            y[ii]=0;
+        }
+        for (int ii=0; ii<8; ii++) {
+            kov[ii]=y[ii];
+            y[ii]=y[15-ii];
+            y[15-ii]=kov[ii];
+        }
+
+        char pp[32];
+        for (int ii=0; ii<9; ii++) {
+            pp[ii]=p[ii+8];
+        }
+        pp[8]=p[7];
+
+        for (int ii=0; ii<9; ii++) {
+            pp[ii+9]=r[ii+8];
+        }
+        pp[17]=r[7];
+
+        for (int ii=0; ii<9; ii++) {
+            pp[ii+18]=y[ii+8];
+        }
+        pp[26]=y[7];
+
+        for (int ii=0; ii<6; ii++) {
+            pp[ii+27]=0;
+        }
+
+        char by[3][7];
+
+        for (int ii=0; ii<4; ii++) {
+            for (int jj=0; jj<8; jj++) {
+                by[ii][jj]=pp[(ii*8)+jj];
+            }
+        }
+
+        char by0[8],by1[8],by2[8],by3[8];
+        for (int ii=0; ii<8; ii++) {
+            by0[ii]=by[0][ii];
+        }
+        for (int ii=0; ii<8; ii++) {
+            by1[ii]=by[1][ii];
+        }
+        for (int ii=0; ii<8; ii++) {
+            by2[ii]=by[2][ii];
+        }
+        for (int ii=0; ii<8; ii++) {
+            by3[ii]=by[3][ii];
+        }
+
+
+
+        for (int ii=0; ii<8; ii++) {
+            i=by0[ii];
+            t0=t0+(i*pow(2.0,ii));
+        }
+        for (int ii=0; ii<8; ii++) {
+            i=by1[ii];
+            t1=t1+(i*pow(2.0,ii));
+        }
+        for (int ii=0; ii<8; ii++) {
+            i=by2[ii];
+            t2=t2+(i*pow(2.0,ii));
+        }
+        for (int ii=0; ii<8; ii++) {
+            i=by3[ii];
+
+            t3=t3+(i*pow(2.0,ii));
+        }
+//-----------------------------------------------------
+}
+
+//+++++++++++++++++++++++++++++IMU+++++++++++++++++++++++++++++++++++
+
+    }
+}

diff -r 000000000000 -r 711905e937b9 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sun Jul 24 19:49:51 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/63bcd7ba4912