File content as of revision 7:b00843da0530:

#include <math.h>
#include <stdint.h>

#include "mbed.h"
#include "APP.h"


static I2C    i2c(p28, p27);
static Serial uart(p9, p10);

void setup()
{
    // Setup UART and I2C.
    uart.format(8, Serial::None, 1);
    uart.baud(9600);
    i2c.frequency(100000);
 
    // Clear display
    uart.putc(0x76);
 
    // Decimal point for display
    uart.putc(0x77);
    uart.putc(0x02);
 
    // Maximum brightness
    uart.putc(0x7A);
    uart.putc(0xFF);
}
 
void activateAccelerometer()
{
    uint8_t data[2] = {CTRL_REG, 0x01};
    i2c.write(ACC_ADDRESS, (char*) data, 2);
}
 
uint8_t readStatus()
{
    uint8_t result[1];
    uint8_t reg[1] = {STATUS_REG};
    i2c.write(ACC_ADDRESS, (char*) reg, 1, true);
    i2c.read(ACC_ADDRESS, (char*) result, 1);
 
    return result[0];
}    
 
int16_t convertToAcceleration(uint8_t hi, uint8_t lo)
{
    int16_t value = (lo >> 2) | (hi << 6);
    if (value > UINT14_MAX/2)
    {
        value -= UINT14_MAX;
    }
 
    return value;
}
 
uint32_t squareRoot(uint32_t input)
{
    uint32_t op = input;
    uint32_t res = 0;
    uint32_t one = 1UL << 30;
 
    while (one > op)
    {
        one >>= 2;
    }
 
    while (one != 0)
    {
        if (op >= res + one)
        {
            op = op - (res + one);
            res = res + 2 * one;
        }
 
        res >>= 1;
        one >>= 2;
    }
 
    return res;
}
 
int16_t readAndComputeAngle()
{
    uint8_t result[6];
    uint8_t reg[1] = {ACC_X_REG};
    i2c.write(ACC_ADDRESS, (char*) reg, 1, true);
    i2c.read(ACC_ADDRESS, (char*) result, 6);
    int16_t accX = convertToAcceleration(result[0], result[1]);
    int16_t accY = convertToAcceleration(result[2], result[3]);
    int16_t accZ = convertToAcceleration(result[4], result[5]);
 
    uint32_t modulus = squareRoot(accX*accX + accY*accY + accZ*accZ);
    float angle = acos((float) accZ / modulus) * 180.0 / PI;
 
    // To use integer operations instead of floating-point arithmetic, the angle is multiplied by 100.
    // This allows for two decimals of precision on the display.
    int16_t angleHundreds = angle * 100;
    if (angleHundreds > 9000)
    {
        angleHundreds = 18000 - angleHundreds;
    }
 
    return angleHundreds;
}
 
void updateDisplay(int16_t angle)
{
    uint8_t digits[4];
    sprintf((char*) digits, "%d", angle);
 
    if (angle < 1000)
    {
        uart.putc(0);
        uart.putc(digits[0]);
        uart.putc(digits[1]);
        uart.putc(digits[2]);
    }
    else
    {
        uart.putc(digits[0]);
        uart.putc(digits[1]);
        uart.putc(digits[2]);
        uart.putc(digits[3]);
    }
}
 
int32_t main()
{
    setup();
    activateAccelerometer();
 
    while (true)
    {
        // Read bit 3 from the status register
        int8_t status = (readStatus() >> 3) & 0x01;
 
        if (status == 1)
        {
            // Get accelerations, compute angle
            uint16_t angle = readAndComputeAngle();
 
            // Update 7 segment display
            updateDisplay(angle);
        }
 
        wait(0.4);
    }
}