FlightControl/main.c at master · wuyingnan/FlightControl · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115

#include "msp430f5438.h"
#include "msp430_math.h"
#include "Global.h"
#include "UCS.h"
#include "WDT.h"
#include "TimerA1.h"
#include "UART.h"

#include "I2C.h"
#include "I2C2.h"
#include "IMU.h"

#include "PWM.h"
#include "FC.h"
#include "MMA8452.h"

unsigned long nowTime,startTime;
unsigned char command;
unsigned char flag=0;

void main( void )
{
  WDT_init();

  UCS_init();

  TimerA1_init();

  I2C_init();

  I2C2_init();

  UART_init(UCA1,115200);

  PWM_init();

  _EINT();

  IMU_init();

  IMU_calibrate();

  FC_init();

  UART_sendstr(UCA1,"Ready.");

//  WDTCTL = WDTCNTCL+ WDT_ADLY_1000;

  while(UCA1_GET_CHAR(&command));

  startTime=TimeBase;


  while(1)
  {
//    WDTCTL = WDTCNTCL+ WDT_ADLY_1000;

    if(nowTime!=TimeBase)
    {
      nowTime=TimeBase;

      if(nowTime % 5==0)//&&(nowTime < 5000+startTime))
      {
        if(!IMU_getdata())
          AHRSupdate();
//        IMU_update();
        _NOP();

        if (nowTime % 10 == 0)
        {
          if(flag)
            FC_control();
          _NOP();
        }
      }
    }
    if(!UCA1_GET_CHAR(&command))
    {
      if (command=='g')
      {
        flag=1;
      }
      if (command=='h')
      {
        PWM_1(0);
        PWM_2(0);
        PWM_3(0);
        PWM_4(0);
        signed long kp=0,ki=0,kd=0;
        while(UCA1_GET_CHAR(&command));
        while(command!=' ')
        {
          kp=kp*10+command-'0';
          while(UCA1_GET_CHAR(&command));
        }
        while(UCA1_GET_CHAR(&command));
        while(command!=' ')
        {
          ki=ki*10+command-'0';
          while(UCA1_GET_CHAR(&command));
        }
        while(UCA1_GET_CHAR(&command));
        while(command!=' ')
        {
          kd=kd*10+command-'0';
          while(UCA1_GET_CHAR(&command));
        }
        FC_changePitchPID(kp,ki,kd);
      }
    }

  }

}