#include <msp430x16x.h>
#include "nrf24l01.h"
#include "USART0.h"
#include "nrf24l01.h"
#include "cap.h"
#include "control.h"

int set_value[3]={0};       //0是方向，1是距离，2是高度
char  RxBuf[32];
int A1[2]={0};                  //油门高低8位
char a[2];
float distance[3];  //超声波距离 0-X，1-height，2-Y
int time_hight=0;
extern int Picth_adj,Roll_adj;
extern int  throttle;                          //油门基础量
extern int  throttle_adj;                 //油门偏移量
uchar En_Location=0;            //PID闭环使能端
float X=0.5,Y=0.5,H=1.0;
int A,D;
char Start_Com=0x00;          //从机启动命令

uint fly_time=0,Time_EN=0;
uchar stop_flag=0;
uchar set_ok_flag=0;

void Task(uchar task_value);
void task_key_scan(void);                  //任务选择函数
void main(void) 
 {
    WDTCTL = WDTPW + WDTHOLD; 
    InitSys();
    Pid_init();
//    init_NRF24L01();
    Usart0_Init();
    init_dvice();
    MiddleValueFilter();
    MiddleValueFilter();
    Usart1_Init();    
//    delay_ms(7000);   
    CCTL0 = CCIE;                             // CCR0 interrupt enabled
    CCR0 = 50000;
    TACTL = TASSEL_2 + MC_1 + ID_3;                  // SMCLK, upmode 
   _EINT(); // interrupt enable
//         Task(1);          //Task 1
//   Start_Com = 'O';
//   delay_ms(2000); 
//   En_Location =1 ;
//   throttle = 255;
   while(1)
    {
  //     MiddleValueFilter();
      task_key_scan();
    }
 }

uchar key_scan(void)
 {
   uchar key,key_in=0;
   key_in = P6IN & 0xc0;
   if(key_in != 0xc0)
    {
      key_in = P6IN & 0xc0;
      if(key_in != 0xc0)
       {
         key = key_in;
         while(key_in != 0xc0)
           key_in = P6IN & 0xc0;
         switch(key)
          {
            case 0x80: key = 2;break;
            case 0x40: key = 1;break;
            default:key = 0;
          }
       }
    }
   else
     key=0;
   return key;
 }