File content as of revision 0:d1960beb98fe:

#include "MPU6050.h"
#include "Serial_HL.h"
#include "math.h"
BusOut leds(LED4,LED3,LED2,LED1);
DigitalOut mpu6050Address (p29);

SerialBLK pc(USBTX, USBRX);
SvProtocol ua0(&pc);

void CommandHandler();

int setAddress (int address) {
    return address-0x68;            // This function determines the address of the sensor via the AD0-pin.
}                                   // If it should be 0x68, the AD0-pin is set to GND (0), otherwise it's set to VCC (1).

int16_t accelX;
double sinX;
int16_t angleX;
int16_t gyroXRate;
int16_t gyroXAngle;

main() {

    ua0.SvMessage("MPU6050-Test");  // Meldung zum PC senden
    mpu6050Address = setAddress(MPU6050_DEFAULT_ADDRESS);
    MPU6050 mpu6050;
    mpu6050.initialize();           //für Ausgabe in Terminal "//" von debug-Befehlen entfernen
    //mpu6050.setDLPFMode(MPU6050_DLPF_BW_98);
    //mpu6050.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
    accelX = mpu6050.getAccelerationX();
    //mpu6050.debugSerial.printf("Beschleunigung in x-Richtung: %d\n\r", accelX);
    sinX = (double)accelX/16384;
    angleX = (int16_t) asin(sinX)*57.296;
    //mpu6050.debugSerial.printf("Neigung: %f\n\r", angleX*57.296);
  
    Timer stw; stw.start();
//    Timer gyro; gyro.start();
    
    while(1)
    {
        CommandHandler();
 /*       if (gyro.read_us()>125)
        {
            gyro.reset();
            gyroXRate = mpu6050.getRotationX();
            gyroXRate = gyroXRate*0.125+3;
            gyroXRate = gyroXRate*0.000125;
            gyroXAngle = gyroXAngle + gyroXRate;
        }
*/        accelX = mpu6050.getAccelerationX();
        if( (stw.read_ms()>10) ) // 100Hz
        { // dieser Teil wird mit 100Hz aufgerufen
            stw.reset();
            sinX = (double)accelX/16384;
            angleX = (float)asin(sinX)*57.296;
            if( ua0.acqON ) {
            // nur wenn vom PC aus das Senden eingeschaltet wurde
            // wird auch etwas gesendet
                ua0.WriteSvI16(1, angleX-1);
//                ua0.WriteSvI16(2, gyroXAngle);
            }
        }
    }
}


void CommandHandler()
{
    uint8_t cmd;
    int16_t idata1, idata2;
  
    // Fragen ob überhaupt etwas im RX-Reg steht
    if( !pc.IsDataAvail() )
        return;
  
    // wenn etwas im RX-Reg steht
    // Kommando lesen
    cmd = ua0.GetCommand();

    if( cmd==3 )
    {
        idata1 = ua0.ReadI16();
        leds = idata1;
        ua0.SvMessage("Leds schalten");
    }
    if( cmd==4 )
    {
        idata1 = leds;
        ua0.SvPrintf("LEDS %d", idata1);
    }
}