File content as of revision 1:b87e95907a18:

#include "mbed.h"
#define _MBED_
#include "Adafruit_9DOF.h"
#include "Serial_base.h"
#include "controller.h"


DigitalOut myled(LED1);
Adafruit_9DOF dof = Adafruit_9DOF();
Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(30301);
Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(30302);
Adafruit_L3GD20_Unified gyro = Adafruit_L3GD20_Unified(20);

Serial pc(USBTX, USBRX);



/* Offsets of gyro at rest */
float x_offset = 0, y_offset = 0, z_offset = 0;

void initSensors()
{
    s_com->println(("\r\nInitializing sensors!"));
    if (!accel.begin()) {
        /* There was a problem detecting the LSM303 ... check your connections */
        s_com->println(("Ooops, no LSM303 accel detected ... Check your wiring!"));
        while(1);
    }
    if (!mag.begin()) {
        /* There was a problem detecting the LSM303 ... check your connections */
        s_com->println("Ooops, no LSM303 mag detected ... Check your wiring!");
        while(1);
    }
    if (!gyro.begin(GYRO_RANGE_2000DPS)) {
        /* There was a problem detecting the L3GD20 ... check your connections */
        s_com->println("Ooops, no L3GD20 gyro detected ... Check your wiring or I2C ADDR!");
        while(1);
    }
    /* Calculate initial offsets and noise level of gyro */
    float sampleNum = 500;
    sensors_event_t gyro_event;
    for (int n = 0; n < sampleNum; n++) {
        gyro.getEvent(&gyro_event);
        x_offset += gyro_event.gyro.x;
        y_offset += gyro_event.gyro.y;
        z_offset += gyro_event.gyro.z;
    }
    x_offset = x_offset / sampleNum;
    y_offset = y_offset / sampleNum;
    z_offset = z_offset / sampleNum;
    s_com->print("Offsets... X: ");
    s_com->print(x_offset);
    s_com->print("\tY: ");
    s_com->print(y_offset);
    s_com->print("\tZ: ");
    s_com->print(z_offset);
}



int main()
{
    double F=0;
    double M_x=0;
    double M_y=0;
    double M_z=0;

    initSensors();


    //double *attitude = (double *) malloc(6 * sizeof(double));
    sensors_event_t accel_event;
    sensors_event_t mag_event;
    sensors_event_t gyro_event;
    sensors_vec_t   orientation;


    while (1) {

        /* Calculate pitch and roll from the raw accelerometer data */
        accel.getEvent(&accel_event);
        if (dof.accelGetOrientation(&accel_event, &orientation)) {
            /* 'orientation' should have valid .roll and .pitch fields */
            //s_com->print(("Roll: "));
            //s_com->print(orientation.roll);
            //s_com->print(("; "));
            //s_com->print(("Pitch: "));
            //s_com->print(orientation.pitch);
            //s_com->print((";\t"));
        }
        /* Calculate the heading using the magnetometer */
        mag.getEvent(&mag_event);
        if (dof.magGetOrientation(SENSOR_AXIS_Z, &mag_event, &orientation)) {
            /* 'orientation' should have valid .heading data now */
            //s_com->print(("Heading: "));
            //s_com->print(orientation.heading);
          //  s_com->print((";\r\n"));
        }
        /* Get angular rate data from gyroscope */
        gyro.getEvent(&gyro_event);
        gyro_event.gyro.x -= x_offset;
        gyro_event.gyro.y -= y_offset;
        gyro_event.gyro.z -= z_offset;

        wait(0.1);

        // get sensor values

        // call controller
        M_x= controller(gyro_event, orientation);

        // plot

        //pc.printf("F: %f M_x: %f M_y: %f M_z: %f\n\r",  F, M_x, M_y, M_z);
        pc.printf("M_x: %f \n\r", M_x);
    }
    //free(attitude);
}