#include "HardwareInfo.c"
#include "JMLib.c"
#include <SetLCDBack.h>
#include <SetLCDClear.h>
#include <SetLCDString.h>
#include <GetButton1.h>
#include <GetButton2.h>
#include <SetCompass.h>
#include <GetCompassB.h>
#include <SetLCD3Char.h>
#include <GetButton3.h>
#include <GetCompassAHRS.h>
#include <SetLCD2Decimal.h>


// define global var
unsigned int BUT1 = 0;
unsigned int BUT2 = 0;
unsigned int BUT3 = 0;
unsigned int ANGLE = 0;
double ROLL = 0;
double PITCH = 0;
double YAW = 0;

int main(void)
{
    X4RCU_Init();
    SetLCDBack(1);
    SetLCDClear(BLACK);
    SetLCDString(68, 148, "BUT3->RETURN", 65504, BLACK);
    SetLCDString(0, 105, "BUT1->COMPASS", 65504, BLACK);
    SetLCDString(136, 105, "BUT2->AHRS", 65504, BLACK);
    while (1)
    {
        BUT1 = GetButton1();
        BUT2 = GetButton2();
        if ( BUT1==1 )
        {
            SetCompass(_COMPASS_1_, 5, 0);
            SetLCDString(0, 0, "ANGLE", YELLOW, BLACK);
            while (1)
            {
                ANGLE = GetCompassB(_COMPASS_1_);
                SetLCD3Char(112, 0, ANGLE, YELLOW, BLACK);
                BUT3 = GetButton3();
                if ( BUT3==1 )
                {
                    SetLCDString(0, 0, "ANGLE", 0, BLACK);
                    SetLCD3Char(112, 0, ANGLE, 0, BLACK);
                    break;
                }
            }
        }
        if ( BUT2==1 )
        {
            SetCompass(_COMPASS_1_, 5, 2);
            SetLCDString(0, 0, "ROLL", YELLOW, BLACK);
            SetLCDString(0, 32, "PITCH", YELLOW, BLACK);
            SetLCDString(0, 64, "YAW", YELLOW, BLACK);
            while (1)
            {
                ROLL = GetCompassAHRS(_COMPASS_1_, 1, 0);
                PITCH = GetCompassAHRS(_COMPASS_1_, 2, 0);
                YAW = GetCompassAHRS(_COMPASS_1_, 3, 0);
                ROLL = ROLL /10;
                PITCH = PITCH/10;
                YAW = YAW /10;
                SetLCD2Decimal(112, 0, ROLL, YELLOW, BLACK);
                SetLCD2Decimal(112, 32, PITCH, YELLOW, BLACK);
                SetLCD2Decimal(112, 64, YAW, YELLOW, BLACK);
                BUT3 = GetButton3();
                if ( BUT3==1 )
                {
                    SetLCDString(0, 0, "ROLL", 0, BLACK);
                    SetLCDString(0, 32, "PITCH", 0, BLACK);
                    SetLCDString(0, 64, "YAW", 0, BLACK);
                    SetLCD2Decimal(112, 0, ROLL, 0, BLACK);
                    SetLCD2Decimal(112, 32, PITCH, 0, BLACK);
                    SetLCD2Decimal(112, 64, YAW, 0, BLACK);
                    break;
                }
            }
        }
    }
    while(1);
}

