#include "HardwareInfo.c"
#include "JMLib.c"
#include <SetLCDBack.h>
#include <SetLCDString.h>
#include <GetButton2.h>
#include <SetInBeep.h>
#include <SetLCD8Char.h>
#include <SetAHRS.h>
#include <GetAHRS.h>

int main(void)
{
    X2RCU_Init();
    unsigned int ButR = 0;
    int Roll = 0;
    int Pitch = 0;
    int Yaw = 0;
    SetLCDBack(1);
    SetLCDString(0, 0, "D12", YELLOW, BLACK);
    SetLCDString(0, 16, "Euler", YELLOW, BLACK);
    SetLCDString(0, 32, "Roll:", YELLOW, BLACK);
    SetLCDString(0, 48, "Pitch:", YELLOW, BLACK);
    SetLCDString(0, 64, "Yaw:", YELLOW, BLACK);
    while (1)
    {
        ButR = GetButton2();
        if ( ButR==1 )
        {
            SetInBeep(1);
            SetLCD8Char(48, 32, 8888, YELLOW, BLACK);
            SetLCD8Char(48, 48, 8888, YELLOW, BLACK);
            SetLCD8Char(48, 64, 8888, YELLOW, BLACK);
            //校正一次水平姿态方位
            SetAHRS(_AHRS_D12_, 1);
        }
        SetInBeep(0);
        Roll = GetAHRS(_AHRS_D12_, 1, 0);
        Pitch = GetAHRS(_AHRS_D12_, 2, 0);
        Yaw = GetAHRS(_AHRS_D12_, 3, 0);
        SetLCD8Char(48, 32, Roll, YELLOW, BLACK);
        SetLCD8Char(48, 48, Pitch, YELLOW, BLACK);
        SetLCD8Char(48, 64, Yaw, YELLOW, BLACK);
    }
    return 1;
}

