main.c

#include "stm32f30x.h"
#include "stm32f3_discovery.h"
#include "usb.h"
#include "math.h"
#include "usart.h"
#include "gyro.h"
#include "compass.h"
#include "interrupts.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define ABS(x)         (x < 0) ? (-x) : x
#define PI             ((float) 3.14159265f)

  RCC_ClocksTypeDef RCC_Clocks;
__IO uint32_t UserButtonPressed = 0;
__IO float HeadingValue = 0.0f;  

__IO uint8_t DataReady = 0;
__IO uint8_t PrevXferComplete = 1;

extern __IO uint8_t currentlyReadingI2C;
extern __IO uint32_t timeReadI2C;
extern __IO uint32_t totalTime;


const uint32_t leds[8] = {LED3, LED4, LED6, LED8, LED10, LED9, LED7, LED5};

/* axes is a rotation matrix from board coords to world coords */
float axes[3][3];
float *const Xaxis = axes[0], *const Yaxis = axes[1], *const Zaxis = axes[2];
float zeroAngRate[3];


void vecCross(float *o, float *a, float *b)
{
    o[0] = a[1]*b[2] - a[2]*b[1];
    o[1] = a[2]*b[0] - a[0]*b[2];
    o[2] = a[0]*b[1] - a[1]*b[0];
}

void vecNegate(float *v)
{
    int i;
    for (i = 0; i < 3; ++i) {
        v[i] = -v[i];
    }
}

float vecDot(const float *a, const float *b)
{
    return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
}

float vecLen(const float *v)
{
    return sqrt(vecDot(v, v));
}

void vecNorm(float *v)
{
    float len = vecLen(v);
    int i;
    for (i = 0; i < 3; ++i) {
        v[i] /= len;
    }
}

void vecMul(float x[3][3], float *v)
{
    int i;
    float vcopy[3];
    memcpy(vcopy, v, sizeof(vcopy));
    for (i = 0; i < 3; ++i) {
        v[i] = vecDot(vcopy, x[i]);
    }
}

void Compass_ReadAccAvg(float *v, int n)
{
    float vals[3] = {};
    int i = n, x;
    memset(v, 0, sizeof(float[3]));
    while (i--) {
        Compass_ReadAcc(vals);
        for (x = 0; x < 3; ++x)
            v[x] += vals[x];
    }
    for (x = 0; x < 3; ++x)
        v[x] /= n;
}

void Compass_ReadMagAvg(float *v, int n)
{
    float vals[3] = {};
    int i = n, x;
    memset(v, 0, sizeof(float[3]));
    while (i--) {
        Compass_ReadMag(vals);
        for (x = 0; x < 3; ++x)
            v[x] += vals[x];
    }
    for (x = 0; x < 3; ++x)
        v[x] /= n;
}

void Gyro_ReadAngRateAvg(float *v, int n)
{
    float vals[3] = {};
    int i = n, x;
    memset(v, 0, sizeof(float[3]));
    while (i--) {
        Gyro_ReadAngRate(vals);
        for (x = 0; x < 3; ++x)
            v[x] += vals[x];
    }
    for (x = 0; x < 3; ++x)
        v[x] /= n;
}

void calibrate()
{
    // at rest the accelerometer reports an acceleration straight upwards
    // due to gravity. use this as our Z axis
    Compass_ReadAccAvg(Zaxis, 500);
    vecNorm(Zaxis);

    // the magnetic strength is greatest towards magnetic north. use this as
    // our X axis
    Compass_ReadMagAvg(Xaxis, 10);
    vecNorm(Xaxis);

    // create a Y axis perpendicular to the X and Z axes
    vecCross(Yaxis, Zaxis, Xaxis);
    vecNorm(Yaxis);
    // ensire that the X axis is actually perpendicular to the Y and Z axes
    vecCross(Xaxis, Yaxis, Zaxis);
    vecNorm(Xaxis);

    // calibrate the zero error on the gyroscope
    Gyro_ReadAngRateAvg(zeroAngRate, 100);
}


int main()
{
    setvbuf(stdout, NULL, _IONBF, 0);
    setvbuf(stderr, NULL, _IONBF, 0);

    /*!< At this stage the microcontroller clock setting is already configured, 
      this is done through SystemInit() function which is called from startup
      file (startup_stm32f30x.s) before to branch to application main.
      To reconfigure the default setting of SystemInit() function, refer to
      system_stm32f30x.c file
      */ 

    /* SysTick end of count event each 10ms */
    RCC_GetClocksFreq(&RCC_Clocks);
    SysTick_Config(RCC_Clocks.HCLK_Frequency / 100);

    /* initialise USART1 debug output (TX on pin PA9 and RX on pin PA10) */
    USART1_Init();

    //printf("Starting\n");
    USART1_flush();

    /* Initialise LEDs */
    //printf("Initialising LEDs\n");
    int i;
    for (i = 0; i < 8; ++i) {
        STM_EVAL_LEDInit(leds[i]);
        STM_EVAL_LEDOff(leds[i]);
    }

    /* Initialise gyro */
    //printf("Initialising gyroscope\n");
    Gyro_Init();

    /* Initialise compass */
    //printf("Initialising compass\n");
    Compass_Init();

    Delay(100);
    // perform calibration
    calibrate();


    while (1) {
        float angRate[3], mag[3];

        // read average compass values
        Compass_ReadMagAvg(mag, 2);
        // rotate the compass values so that they are aligned with Earth
        vecMul(axes, mag);
        // calculate the heading through inverse tan of the Y/X magnetic strength
        float compassAngle = atan2f(mag[1], mag[0]) * 180.f / PI;
        // fix heading to be in range -180 to 180
        if (compassAngle > 180.f) compassAngle -= 360.f;
        // read average gyro values
        Gyro_ReadAngRateAvg(angRate, 2);
        // print out everything
        printf("c%6.3f\ng%6.3f\n", compassAngle, angRate[2]-zeroAngRate[2]);

    }

    return 1;
}