MPU6050 driver for Proton basic

[towlerg]

MPU6050 driver for Proton basic was posted on Sonsivri by FoFo. Think it might be tweaked version of a MPU6050 demo posted a few years ago. If anybody need this and doesn't have Sonsivri access I can upload.

[midali]

Hi Towlerg,

The code is welcome, I need it . I saw that FoFo have a PID example writed in Proton Basic . May you upload that code , please ?

Thank you !

[Mapo]

I remembered that I had a sonsivri account, here are the files you were looking for

[midali]

Thank you Mapo !

[Dompie]

Hiiiii In the PID example,

Code Select Expand

' Written by Les Johnson (14-11-11)So it is Les his stuf.

Johan

[ANDY WONG]

I ported DMPMPU6050.zip to 18F458; it compiled but did not work at all. SDA, SCL pins no signal. I only got SDA SCL signals when I deleted from line 146 down leaves only the validation check.

[top204]

I created a library for the MPU6050 a few years ago.

Below is the include file's listing. Name it "MPU6050.inc"

Code Select Expand

$ifndef _MPU_INC_
$define _MPU_INC_
'
'   /\\\\\\\\\
'  /\\\///////\\\
'  \/\\\     \/\\\                                                 /\\\          /\\\
'   \/\\\\\\\\\\\/        /\\\\\     /\\\\\\\\\\     /\\\\\\\\   /\\\\\\\\\\\  /\\\\\\\\\\\  /\\\\\\\\\
'    \/\\\//////\\\      /\\\///\\\  \/\\\//////    /\\\/////\\\ \////\\\////  \////\\\////  \////////\\\
'     \/\\\    \//\\\    /\\\  \//\\\ \/\\\\\\\\\\  /\\\\\\\\\\\     \/\\\         \/\\\        /\\\\\\\\\\
'      \/\\\     \//\\\  \//\\\  /\\\  \////////\\\ \//\\///////      \/\\\ /\\     \/\\\ /\\   /\\\/////\\\
'       \/\\\      \//\\\  \///\\\\\/    /\\\\\\\\\\  \//\\\\\\\\\\    \//\\\\\      \//\\\\\   \//\\\\\\\\/\\
'        \///        \///     \/////     \//////////    \//////////      \/////        \/////     \////////\//
'                                  Let's find out together what makes a PIC Tick!
'
' MPU6050 Accelerometer and Gyroscope Sensor library procedures
' Written for the Positron8 BASIC compiler by Les Johnson
'
' Create default pins for the MPU6050's I2C interface
'
$ifndef MPU_SDA_Pin                         ' Has the main program set what pin to use for SDA?
    $define MPU_SDA_Pin PORTC.4             ' No. So create a default pin for the MPU6050 SDA line
$endif
$ifndef MPU_SCL_Pin                         ' Has the main program set what pin to use for SCL?
    $define MPU_SCL_Pin PORTC.3             ' No. So create a default pin for the MPU6050 SCL line
$endif

$define cMPU_ADDRESS $D0                    ' The I2C address of the MPU6050 ($68 << 1)
'
' Create command register names for the MPU6050
'
$define cMPU_XG_OFFS_TC         $00
$define cMPU_YG_OFFS_TC         $01
$define cMPU_ZG_OFFS_TC         $02
$define cMPU_X_FINE_GAIN        $03
$define cMPU_Y_FINE_GAIN        $04
$define cMPU_Z_FINE_GAIN        $05
$define cMPU_XA_OFFS_H          $06
$define cMPU_XA_OFFS_L_TC       $07
$define cMPU_YA_OFFS_H          $08
$define cMPU_YA_OFFS_L_TC       $09
$define cMPU_ZA_OFFS_H          $0A
$define cMPU_ZA_OFFS_L_TC       $0B
$define cMPU_XG_OFFS_USRH       $13
$define cMPU_XG_OFFS_USRL       $14
$define cMPU_YG_OFFS_USRH       $15
$define cMPU_YG_OFFS_USRL       $16
$define cMPU_ZG_OFFS_USRH       $17
$define cMPU_ZG_OFFS_USRL       $18
$define cMPU_SMPLRT_DIV         $19
$define cMPU_CONFIG             $1A
$define cMPU_GYRO_CONFIG        $1B
$define cMPU_ACCEL_CONFIG       $1C
$define cMPU_FF_THR             $1D
$define cMPU_FF_DUR             $1E
$define cMPU_MOT_THR            $1F
$define cMPU_MOT_DUR            $20
$define cMPU_ZRMOT_THR          $21
$define cMPU_ZRMOT_DUR          $22
$define cMPU_FIFO_EN            $23
$define cMPU_I2C_MST_CTRL       $24
$define cMPU_I2C_SLV0_ADDR      $25
$define cMPU_I2C_SLV0_REG       $26
$define cMPU_I2C_SLV0_CTRL      $27
$define cMPU_I2C_SLV1_ADDR      $28
$define cMPU_I2C_SLV1_REG       $29
$define cMPU_I2C_SLV1_CTRL      $2A
$define cMPU_I2C_SLV2_ADDR      $2B
$define cMPU_I2C_SLV2_REG       $2C
$define cMPU_I2C_SLV2_CTRL      $2D
$define cMPU_I2C_SLV3_ADDR      $2E
$define cMPU_I2C_SLV3_REG       $2F
$define cMPU_I2C_SLV3_CTRL      $30
$define cMPU_I2C_SLV4_ADDR      $31
$define cMPU_I2C_SLV4_REG       $32
$define cMPU_I2C_SLV4_DO        $33
$define cMPU_I2C_SLV4_CTRL      $34
$define cMPU_I2C_SLV4_DI        $35
$define cMPU_I2C_MST_STATUS     $36
$define cMPU_INT_PIN_CFG        $37
$define cMPU_INT_ENABLE         $38
$define cMPU_DMP_INT_STATUS     $39
$define cMPU_INT_STATUS         $3A
$define cMPU_ACCEL_XOUT_H       $3B
$define cMPU_ACCEL_XOUT_L       $3C
$define cMPU_ACCEL_YOUT_H       $3D
$define cMPU_ACCEL_YOUT_L       $3E
$define cMPU_ACCEL_ZOUT_H       $3F
$define cMPU_ACCEL_ZOUT_L       $40
$define cMPU_TEMP_OUT_H         $41
$define cMPU_TEMP_OUT_L         $42
$define cMPU_GYRO_XOUT_H        $43
$define cMPU_GYRO_XOUT_L        $44
$define cMPU_GYRO_YOUT_H        $45
$define cMPU_GYRO_YOUT_L        $46
$define cMPU_GYRO_ZOUT_H        $47
$define cMPU_GYRO_ZOUT_L        $48
$define cMPU_EXT_SENS_DATA_00   $49
$define cMPU_EXT_SENS_DATA_01   $4A
$define cMPU_EXT_SENS_DATA_02   $4B
$define cMPU_EXT_SENS_DATA_03   $4C
$define cMPU_EXT_SENS_DATA_04   $4D
$define cMPU_EXT_SENS_DATA_05   $4E
$define cMPU_EXT_SENS_DATA_06   $4F
$define cMPU_EXT_SENS_DATA_07   $50
$define cMPU_EXT_SENS_DATA_08   $51
$define cMPU_EXT_SENS_DATA_09   $52
$define cMPU_EXT_SENS_DATA_10   $53
$define cMPU_EXT_SENS_DATA_11   $54
$define cMPU_EXT_SENS_DATA_12   $55
$define cMPU_EXT_SENS_DATA_13   $56
$define cMPU_EXT_SENS_DATA_14   $57
$define cMPU_EXT_SENS_DATA_15   $58
$define cMPU_EXT_SENS_DATA_16   $59
$define cMPU_EXT_SENS_DATA_17   $5A
$define cMPU_EXT_SENS_DATA_18   $5B
$define cMPU_EXT_SENS_DATA_19   $5C
$define cMPU_EXT_SENS_DATA_20   $5D
$define cMPU_EXT_SENS_DATA_21   $5E
$define cMPU_EXT_SENS_DATA_22   $5F
$define cMPU_EXT_SENS_DATA_23   $60
$define cMPU_MOT_DETECT_STATUS  $61
$define cMPU_I2C_SLV0_DO        $63
$define cMPU_I2C_SLV1_DO        $64
$define cMPU_I2C_SLV2_DO        $65
$define cMPU_I2C_SLV3_DO        $66
$define cMPU_I2C_MST_DELAY_CTRL $67
$define cMPU_SIGNAL_PATH_RESET  $68
$define cMPU_MOT_DETECT_CTRL    $69
$define cMPU_USER_CTRL          $6A
$define cMPU_PWR_MGMT1          $6B
$define cMPU_PWR_MGMT2          $6C
$define cMPU_BANK_SEL           $6D
$define cMPU_MEM_START_ADDR     $6E
$define cMPU_MEM_R_W            $6F
$define cMPU_DMP_CFG_1          $70
$define cMPU_DMP_CFG_2          $71
$define cMPU_FIFO_COUNTH        $72
$define cMPU_FIFO_COUNTL        $73
$define cMPU_FIFO_R_W           $74
$define cMPU_WHO_AM_I           $75
'
' Bit names for some above registers
'
' Bits of MPU_PWR_MGMT2
'
$define cMPU_PWR_MGMT2_LP_WAKE_CTRLH 128    ' \ 2-bit unsigned value. Specifies the frequency of wake-ups during Accelerometer Only Low Power Mode.
$define cMPU_PWR_MGMT2_LP_WAKE_CTRLL 64     ' /
$define cMPU_PWR_MGMT2_STBY_XA 32           ' When set to 1, this bit puts the X axis accelerometer into standby mode.
$define cMPU_PWR_MGMT2_STBY_YA 16           ' When set to 1, this bit puts the Y axis accelerometer into standby mode.
$define cMPU_PWR_MGMT2_STBY_ZA 8            ' When set to 1, this bit puts the Z axis accelerometer into standby mode.
$define cMPU_PWR_MGMT2_STBY_XG 4            ' When set to 1, this bit puts the X axis gyroscope into standby mode.
$define cMPU_PWR_MGMT2_STBY_YG 2            ' When set to 1, this bit puts the Y axis gyroscope into standby mode.
$define cMPU_PWR_MGMT2_STBY_ZG 1            ' When set to 1, this bit puts the Z axis gyroscope into standby mode.
'
' Create global variables for the MPU6050 library routines
'
    Dim MPU_bReg As Byte Access

    Dim MPU_wAccel_X As SWord    ' Holds the X Axis from the accelerometer
    Dim MPU_wAccel_Y As SWord    ' Holds the Y Axis from the accelerometer
    Dim MPU_wAccel_Z As SWord    ' Holds the Z Axis from the accelerometer

    Dim MPU_wGyro_X As SWord     ' Holds the X Axis from the gyroscope
    Dim MPU_wGyro_Y As SWord     ' Holds the Y Axis from the gyroscope
    Dim MPU_wGyro_Z As SWord     ' Holds the Z Axis from the gyroscope

    Dim MPU_wTemperature As SWord       ' Holds the temperature in degrees Centigrade

'-----------------------------------------------------------------------------------
' Write 8-bits to a register on the MPU6050 device
' Input     : pReg holds the register to write
'           : pValue holds the value to send to the register
' Output    : None
' Notes     : None
'
Proc MPU_WriteByte(pReg As MPU_bReg, pValue As Byte)
    I2COut MPU_SDA_Pin, MPU_SCL_Pin, cMPU_ADDRESS, pReg, [pValue]
EndProc

'-----------------------------------------------------------------------------------
' Write 16-bits to a register on the MPU6050 device
' Input     : pReg holds the register to write
'           : pValue holds the value to send to the register
' Output    : None
' Notes     : None
'
Proc MPU_WriteWord(pReg As MPU_bReg, pValue As Word)
    I2COut MPU_SDA_Pin, MPU_SCL_Pin, cMPU_ADDRESS, pReg, [pValue.HighByte, pValue.LowByte]
EndProc

'-----------------------------------------------------------------------------------
' Read an 8-bit value from a register on the MPU6050 device
' Input     : pReg holds the register to read from
' Output    : Returns the byte read from the MPU6050 device
' Notes     : None
'
Proc MPU_ReadByte(pReg As MPU_bReg), Byte
    I2CIn MPU_SDA_Pin, MPU_SCL_Pin, cMPU_ADDRESS, pReg, [Result]
EndProc

'-----------------------------------------------------------------------------------
' Read a 16-bit value from a register on the MPU6050 device
' Input     : pReg holds the register to read from
' Output    : Returns the word read from the MPU6050 device
' Notes     : None
'
Proc MPU_ReadWord(pReg As MPU_bReg), SWord
    I2CIn MPU_SDA_Pin, MPU_SCL_Pin, cMPU_ADDRESS, pReg, [Result.HighByte, Result.LowByte]
EndProc

'-----------------------------------------------------------------------------------
' Read the Accelerometer, Gyro and Temperature from MPU6050 chip
' Input     : None
' Output    : MPU_wAccel_X holds the X Axis from the accelerometer
'           : MPU_wAccel_Y holds the Y Axis from the accelerometer
'           : MPU_wAccel_Z holds the Z Axis from the accelerometer
'           : MPU_wGyro_X holds the X Axis from the gyroscope
'           : MPU_wGyro_Y holds the Y Axis from the gyroscope
'           : MPU_wGyro_Z holds the Z Axis from the gyroscope
'           : MPU_wTemperature holds the raw temperature value
' Notes     : None
'
Proc MPU_ReadAll()
    I2CIn MPU_SDA_Pin, MPU_SCL_Pin, cMPU_ADDRESS, cMPU_ACCEL_XOUT_H, [MPU_wAccel_X.HighByte, MPU_wAccel_X.LowByte,_
                                                                      MPU_wAccel_Y.HighByte, MPU_wAccel_Y.LowByte,_
                                                                      MPU_wAccel_Z.HighByte, MPU_wAccel_Z.LowByte,_
                                                                      MPU_wTemperature.HighByte, MPU_wTemperature.LowByte,_
                                                                      MPU_wGyro_X.HighByte, MPU_wGyro_X.LowByte,_
                                                                      MPU_wGyro_Y.HighByte, MPU_wGyro_Y.LowByte,_
                                                                      MPU_wGyro_Z.HighByte, MPU_wGyro_Z.LowByte]
EndProc

'-----------------------------------------------------------------------------------
' Read the Accelerometer X, Y, and Z from MPU6050 chip
' Input     : None
' Output    : MPU_wAccel_X holds the X Axis from the accelerometer
'           : MPU_wAccel_Y holds the Y Axis from the accelerometer
'           : MPU_wAccel_Z holds the Z Axis from the accelerometer
' Notes     : None
'
Proc MPU_ReadAccel()
    I2CIn MPU_SDA_Pin, MPU_SCL_Pin, cMPU_ADDRESS, cMPU_ACCEL_XOUT_H, [MPU_wAccel_X.HighByte, MPU_wAccel_X.LowByte,_
                                                                      MPU_wAccel_Y.HighByte, MPU_wAccel_Y.LowByte,_
                                                                      MPU_wAccel_Z.HighByte, MPU_wAccel_Z.LowByte]
EndProc

'-----------------------------------------------------------------------------------
' Read the Gyro X, Y, and Z from MPU6050 chip
' Input     : None
'           : MPU_wGyro_X holds the X Axis from the gyroscope
'           : MPU_wGyro_Y holds the Y Axis from the gyroscope
'           : MPU_wGyro_Z holds the Z Axis from the gyroscope
' Notes     : None
'
Proc MPU_ReadGyro()
    I2CIn MPU_SDA_Pin, MPU_SCL_Pin, cMPU_ADDRESS, cMPU_GYRO_XOUT_H, [MPU_wGyro_X.HighByte, MPU_wGyro_X.LowByte,_
                                                                     MPU_wGyro_Y.HighByte, MPU_wGyro_Y.LowByte,_
                                                                     MPU_wGyro_Z.HighByte, MPU_wGyro_Z.LowByte]
EndProc

'-----------------------------------------------------------------------------------
' Read the 16-bit Accelerometer X value
' Input     : None
' Output    : Returns the 16-bit Accererometer X value
' Notes     : None
'
$define MPU_Get_AccelX() MPU_ReadWord(cMPU_ACCEL_XOUT_H)

'-----------------------------------------------------------------------------------
' Read the 16-bit Accelerometer Y value
' Input     : None
' Output    : Returns the 16-bit Accererometer Y value
' Notes     : None
'
$define MPU_Get_AccelY() MPU_ReadWord(cMPU_ACCEL_YOUT_H)

'-----------------------------------------------------------------------------------
' Read the 16-bit Accelerometer Z value
' Input     : None
' Output    : Returns the 16-bit Accererometer Z value
' Notes     : None
'
$define MPU_Get_AccelZ() MPU_ReadWord(cMPU_ACCEL_ZOUT_H)

'-----------------------------------------------------------------------------------
' Read the 16-bit Gyro X value
' Input     : None
' Output    : Returns the 16-bit Gyro X value
' Notes     : None
'
$define MPU_Get_GyroX() MPU_ReadWord(cMPU_GYRO_XOUT_H)

'-----------------------------------------------------------------------------------
' Read the 16-bit Gyro Y value
' Input     : None
' Output    : Returns the 16-bit Gyro Y value
' Notes     : None
'
$define MPU_Get_GyroY() MPU_ReadWord(cMPU_GYRO_YOUT_H)

'-----------------------------------------------------------------------------------
' Read the 16-bit Gyro Z value
' Input     : None
' Output    : Returns the 16-bit Gyro Z value
' Notes     : None
'
$define MPU_Get_GyroZ() MPU_ReadWord(cMPU_GYRO_ZOUT_H)

'-----------------------------------------------------------------------------------
' Read the 16-bit temperature value
' Input     : None
' Output    : Returns the 16-bit temperature value
' Notes     : None
'
$define MPU_Get_Temperature() MPU_ReadWord(cMPU_TEMP_OUT_H)

'-----------------------------------------------------------------------------------
' Convert the 16-bit temperature value into degrees Centigrade
' Input     : pTempValue holds the 16-bit temperature value read from the MPU6050 device
' Output    : Returns the temperature value in degrees Centigrade
' Notes     : None
'
Proc MPU_ConvToDeg(pTempValue As SWord), SWord
    Result = (pTempValue + 12421) / 340       ' Convert the temperature value into degrees C
EndProc

'-----------------------------------------------------------------------------------
' Initialise the MPU6050 device
' Input     : None
' Output    : None
' Notes     : None
'
Proc MPU_Init()
    MPU_WriteByte(cMPU_PWR_MGMT1, 1)        ' Power on
    MPU_WriteByte(cMPU_CONFIG, 0)           ' Output Low Pass Filter
    MPU_WriteByte(cMPU_GYRO_CONFIG, 0)      ' Set Gyro for ±250°/s. LSB Sensitivity is 131 LSB/°/s
    MPU_WriteByte(cMPU_ACCEL_CONFIG, 0)     ' Set Accel for ±2g. LSB Sensitivity is 16384 LSB/g
EndProc

'-----------------------------------------------------------------------------------
MPU_Main:

$endif  ' _MPU_INC_

Below is a demo program for the above library, that also has filtering in it for better stability:

Code Select Expand

'
'   /\\\\\\\\\
'  /\\\///////\\\
'  \/\\\     \/\\\                                                 /\\\          /\\\
'   \/\\\\\\\\\\\/        /\\\\\     /\\\\\\\\\\     /\\\\\\\\   /\\\\\\\\\\\  /\\\\\\\\\\\  /\\\\\\\\\
'    \/\\\//////\\\      /\\\///\\\  \/\\\//////    /\\\/////\\\ \////\\\////  \////\\\////  \////////\\\
'     \/\\\    \//\\\    /\\\  \//\\\ \/\\\\\\\\\\  /\\\\\\\\\\\     \/\\\         \/\\\        /\\\\\\\\\\
'      \/\\\     \//\\\  \//\\\  /\\\  \////////\\\ \//\\///////      \/\\\ /\\     \/\\\ /\\   /\\\/////\\\
'       \/\\\      \//\\\  \///\\\\\/    /\\\\\\\\\\  \//\\\\\\\\\\    \//\\\\\      \//\\\\\   \//\\\\\\\\/\\
'        \///        \///     \/////     \//////////    \//////////      \/////        \/////     \////////\//
'                                  Let's find out together what makes a PIC Tick!
'
' Serially transmit angle values at 9600 Baud 8N1, which are read from an MPU6050 Accelerometer and Gyroscope Sensor
'
' Written for the Positron8 BASIC compiler by Les Johnson
'
    Device = 18F26K22
    Declare Xtal = 40                                       ' The code is operating at 40 MHz
'
' Setup USART1
'
    Declare Hserial_Baud = 9600                             ' Set the Baud rate for USART1 to 9600 Baud
    Declare HRSOut1_Pin  = PORTC.6                          ' Select the pin used by USART1 TX
'
' Set the pins for the I2C interface to the MPU6050 device
'
    $define MPU_SDA_Pin PORTC.4                             ' Set the pin used for the SDA line to the MPU6050 chip
    $define MPU_SCL_Pin PORTC.3                             ' Set the pin used for the SCL line to the MPU6050 chip

    Include "MPU6050.inc"                                   ' Load the MPU6050 accelerometer routines into the program
'
' Create variables
'
    Dim wAngleValue    As SWord                             ' Holds the angle value transmitted serially
    Dim wFilterValue   As SWord                             ' Holds the filter value that is transferred to Filter, and altered by Filter
'
' Create some constants for the Fixed Point and Float calculations
'
$define cPI 3.141592653

Symbol cFilter_Float_A  = 0.95
Symbol cFilter_FixP_DT  = (0.005 * 16384)
Symbol cFilter_FixP_A   = (0.95 * 16384)
Symbol cFilter_FixP_1_A = (16384 - cFilter_FixP_A)
Symbol cFilter_K_Rad_2  = ((65536.0 / 500.0) * (180.0 / cPI))
Symbol cFilter_K_Deg_2  = (500.0 / 65536.0)

$ifndef False
    $define False 0
$endif
$ifndef True
    $define True 1
$endif

'-----------------------------------------------------------------------------------
' The main program starts here
' It serially transmits the angle value
'
Main:
    Prog_Setup()                                                        ' Setup the program and the peripherals
'
' Perform an initial reading of the MPU6050 with filtering
'
    MPU_GetSamples()
'
' This is the main loop for transmitting the MPU6050 angle value
'
    Do                                                                  ' Create an infinite loop
        wAngleValue = MPU_GetSamples()                                  ' Filter the results from the MPU6050 and get the angle value
        HRSOutLn SDec wAngleValue                                       ' Transmit the angle value
        DelayMS 100                                                     ' Create a delay after the value is transmitted (Can be changed to any delay value)
    Loop                                                                ' Do it forever

'-----------------------------------------------------------------------------------
' Fixed point multiply
' Input     : pValue1 holds the multiplicand value
'           : pValue2 holds the muliplier value
' Output    : Returns the fixed point result of pValue1 x pValue2
' Notes     : Only used within the MPU_Filter procedure
'
Proc FixPoint_Mul(pValue1 As SWord, pValue2 As SWord), SDword
    Result = pValue1 * pValue2
    Result = Result / 16384
EndProc

'-----------------------------------------------------------------------------------
' Filter the raw values from the MPU6050 device
' Returns a positive or negative value that represents the angle of the device
' Input     : pFilAngleAddr holds the address of the filter value from the previous filtering
'           : MPU_wAccel_X, MPU_wAccel_Y, MPU_wAccel_Z, and MPU_wGyro_X hold the global values from the MPU6050 device
' Output    : Returns the calculated Angle value
' Notes     : The routine also operates as a Low Pass Accumulative filter
'
Proc MPU_Filter(ByRef pFilAngleAddr As Word), SWord

    Dim dAccel_X_2    As SDword
    Dim dAccel_Z_2    As SDword
    Dim wAng_Accel_X  As SWord
    Dim fMag_Accel_XZ As Float
    Dim wFilterAngle  As SWord

    wFilterAngle = Ptr16(pFilAngleAddr)                             ' Load wFilterAngle with the value from pFilAngleAddr

    dAccel_X_2 = MPU_wAccel_X * MPU_wAccel_X                        ' Calculate the square of MPU_wAccel_X
    dAccel_Z_2 = MPU_wAccel_Z * MPU_wAccel_Z                        ' Calculate the square of MPU_wAccel_Z
    fMag_Accel_XZ = Sqr(dAccel_X_2 + dAccel_Z_2)                    ' Square the values
    If fMag_Accel_XZ = 0.0 Then ExitProc                            ' Prevent a division of 0

    wAng_Accel_X = fRound(cFilter_K_Rad_2 * ATan(MPU_wAccel_Y / fMag_Accel_XZ))

    wFilterAngle = FixPoint_Mul(cFilter_FixP_DT, MPU_wGyro_X) + wFilterAngle
    wFilterAngle = FixPoint_Mul(cFilter_FixP_A, wFilterAngle)
    wFilterAngle = wFilterAngle + FixPoint_Mul(cFilter_FixP_1_A, wAng_Accel_X)
    Ptr16(pFilAngleAddr) = wFilterAngle                             ' Load the address held in pFilAngleAddr with the contents of wFilterAngle
    Result = wFilterValue / 131                                     ' Calculate the angle value
EndProc

'-----------------------------------------------------------------------------------
' Read the MPU6050 device multiple times and filter it
' Input     : None
' Output    : Returns the calculated Angle value
' Notes     : If the results need to be faster in changing
'           : Change the amount of loops held in bSampeLoop
'           : But please note that this will also slow down the response a little
'
Proc MPU_GetSamples(), SWord
    Dim bSampeLoop As Byte                                      ' Used to initially load the MPU_Filter procedure

    For bSampeLoop = 7 To 0 Step -1                             ' Create a small loop
        MPU_ReadAll()                                           ' Read data from the MPU6050 device
        MPU_Filter(wFilterValue)                                ' Run it through the filter
    Next
    Result = MPU_Filter(wFilterValue)                           ' Return the value from the filter
EndProc

'-----------------------------------------------------------------------------------
' Setup the program
' Input     : None
' Output    : None
' Notes     : None
'
Proc Prog_Setup()
    wFilterValue = 0
    MPU_Init()                                                  ' Initialise the MPU6050 device
EndProc

'-----------------------------------------------------------------------------------
' Set the config fuses for 4xPLL enabled.
'
Config_Start
    FOSC = HSHP             ' HS oscillator (high power > 16 MHz)
    PLLCFG = On             ' Oscillator multiplied by 4
    WDTEN = Off             ' WDT Disabled
    Debug = Off             ' Background debugger disabled; RB6 and RB7 configured as general purpose I/O pins
    XINST = Off             ' Set to standard mode
    FCMEN = On              ' Fail-Safe Clock Monitor enabled
    IESO = Off              ' Oscillator Switchover mode disabled
    PWRTEN = On             ' PWRT enabled
    BOREN = On              ' Brown-out Reset enabled and controlled by software (SBOREN is enabled)
    BORV = 190              ' VBOR set to 1.9 V nominal
    WDTPS = 32768           ' Watchdog postscaler is 1:32768 (approx 30 seconds)
    MCLRE = EXTMCLR         ' MCLR pin enabled, RE3 input pin disabled
    HFOFST = Off            ' The system clock is held off until the HF-INTOSC is stable.
    PRICLKEN = On           ' Primary clock enabled
    PBADEN = Off            ' PORTB<4:0> pins are configured as digital I/O on Reset
    CCP2MX = PORTC1         ' CCP2 input/output is multiplexed with RC1
    CCP3MX = PORTB5         ' P3A/CCP3 input/output is multiplexed with RB5
    T3CMX = PORTC0          ' T3CKI is on RC0
    P2BMX = PORTB5          ' P2B is on RB5
    STVREN = On             ' Stack full/underflow will cause Reset
    LVP = Off               ' Single-Supply ICSP disabled
    Cp0 = Off               ' Block 0 (000800-001FFFh) not code-protected
    CP1 = Off               ' Block 1 (002000-003FFFh) not code-protected
    CPB = Off               ' Boot block (000000-0007FFh) not code-protected
    CPD = Off               ' Data EEPROM not code-protected
    WRT0 = Off              ' Block 0 (000800-001FFFh) not write-protected
    WRT1 = Off              ' Block 1 (002000-003FFFh) not write-protected
    WRTB = Off              ' Boot block (000000-0007FFh) not write-protected
    WRTC = Off              ' Configuration registers (300000-3000FFh) not write-protected
    WRTD = Off              ' Data EEPROM not write-protected
    EBTR0 = Off             ' Block 0 (000800-001FFFh) not protected from table reads executed in other blocks
    EBTR1 = Off             ' Block 1 (002000-003FFFh) not protected from table reads executed in other blocks
    EBTRB = Off             ' Boot block (000000-0007FFh) not protected from table reads executed in other blocks
Config_End