File content as of revision 2:c463326536ae:

/* mbed Microcontroller Library
 * Copyright (c) 2019 ARM Limited
 * SPDX-License-Identifier: Apache-2.0
 */

#include "mbed.h"
#include "math.h"
#include "platform/mbed_thread.h"


// Blinking rate in milliseconds
#define BLINKING_RATE_MS                                                    500
Thread thre;
Serial pc(USBTX, USBRX);
Serial Ardu(p9,p10);
void inline MPU6050Values(Serial *Ardu,int16_t *accy,int16_t *accz,int16_t *gyr, float *samplerate, int *k, float *offset,float *pitch, float *angleX); 
float inline filteredAngle(float *pitch, float *gyrX);
int main()
{
    uint8_t charArr[6];
    int16_t accy;
    int16_t accz;
    int16_t gyr;
    float angleX =0;
    float samplerate = 9600.0;
    float finalAx=0;
    float pitch =0;
    float offset =0;
    float gyr_with_ofset =0;
    int k =1; // this is for getting the initial angle the first time around so we can start at zero
    int j=0;
    while (true) {
        MPU6050Values(&Ardu,&accy,&accz,&gyr, &samplerate, &k, &offset,&pitch, &angleX);
        finalAx = filteredAngle(&pitch, &angleX);
        
        
        
        
        
        
        
        
        
        
        if(j=1000){
        pc.printf("calc angle: %f.2\n\r",finalAx);
        j=0;
        }else
        j++;
        /*
        for(int i =0; i<6;i++)
        charArr[i]=Ardu.getc();
        
        int16_t accy = charArr[0]<<8|charArr[1];
        int16_t accz = charArr[2]<<8|charArr[3];
        int16_t gyr = charArr[4]<<8|charArr[5];
        //pc.printf(" Acc: %d Gyr: %d\n\r",acc, gyr);
        pitch = atan(accy/accz);// angle to check the gyro data against in the complementary filter
        if(k){
        offset = gyr/samplerate;//gets the offset or starting point the first time around
        k=0;
        }
        
        gyr_with_ofset = gyr - offset;
        angleX += gyr_with_ofset/samplerate;//intergration for the angular velocity
        finalAx = pitch*(0.02)+angleX*(0.98);//complementary filter
        //the following is just so the calculated angle prints at a reasonable rate
        if(j=1000){
        pc.printf("calc angle: %f.2\n\r",finalAx);
        j=0;
        }else
        j++;
        */
    }
}

inline void MPU6050Values(Serial *Ardu,int16_t *accy,int16_t *accz,int16_t *gyr, float *samplerate, int *k, float *offset,float *pitch, float *angleX){
    uint8_t charArr[6];
    for(int i =0; i<6;i++)
        charArr[i]=Ardu->getc();
    
    (*accy)= charArr[0]<<8|charArr[1];
    (*accz)= charArr[2]<<8|charArr[3];
    (*gyr)= charArr[4]<<8|charArr[5];
    (*pitch) = atan((*accy)/(*accz));
    if(*k){
        (*offset) = (*gyr);//gets the offset or starting point the first time around
        *k=0;
        }
        
    (*angleX) += ((*gyr) - (*offset))/(*samplerate);
    }

inline float filteredAngle(float *pitch, float *angleX){
    return (*pitch)*(0.02)+(*angleX)*(0.98);
    }