Subversion Repositories FlightCtrl

Rev

Rev 2158 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2158 Rev 2189
Line 1... Line 1...
1 
#include <avr/boot.h>
 1 
#include <avr/boot.h>
2 
#include <avr/io.h>
 2 
#include <avr/io.h>
3 
#include <avr/interrupt.h>
 3 
#include <avr/interrupt.h>
4 
#include <util/delay.h>
 4 
#include <util/delay.h>
- 
 
 5 
#include <stdio.h>
Line 5... Line 6...
5 
 
 6 
 
6 
#include "timer0.h"
 7 
#include "timer0.h"
7 
#include "timer2.h"
 8 
#include "timer2.h"
8 
#include "uart0.h"
 9 
#include "uart0.h"
9 
#include "uart1.h"
 10 
//#include "uart1.h"
10 
#include "output.h"
 - 
 
11 
#include "menu.h"
 11 
#include "output.h"
12 
#include "attitude.h"
 12 
#include "attitude.h"
13 
#include "commands.h"
 13 
//#include "commands.h"
- 
 
 14 
//#include "flight.h"
14 
#include "flight.h"
 15 
//#include "profiler.h"
15 
#include "rc.h"
 16 
#include "rc.h"
16 
#include "analog.h"
 17 
#include "analog.h"
17 
#include "configuration.h"
 - 
 
18 
#include "printf_P.h"
 18 
#include "configuration.h"
19 
#include "twimaster.h"
 19 
#include "twimaster.h"
20 
#include "controlMixer.h"
 20 
#include "controlMixer.h"
21 
#ifdef USE_NAVICTRL
 21 
#include "eeprom.h"
22 
#include "spi.h"
 - 
 
23 
#endif
 22 
#include "beeper.h"
24 
#ifdef USE_MK3MAG
 23 
#ifdef USE_MK3MAG
25 
#include "mk3mag.h"
 24 
#include "mk3mag.h"
26 
#endif
 - 
 
Line 27... Line 25...
27 
#include "eeprom.h"
 25 
#endif
28 
 
 26 
 
Line 29... Line 27...
29 
int16_t main(void) {
 27 
int16_t main(void) {
30 
  uint16_t timer;
 28 
        uint16_t timer;
Line 31... Line 29...
31 
 
 29 
 
32 
  // disable interrupts global
 30 
        // disable interrupts global
33 
  cli();
 31 
        cli();
- 
 
 32 
 
- 
 
 33 
        // analyze hardware environment
34 
 
 34 
        setCPUType();
35 
  // analyze hardware environment
 35 
        setBoardRelease();
36 
  setCPUType();
 36 
 
37 
  setBoardRelease();
 37 
        fdevopen(uart_putchar, NULL);
38 
 
 38 
 
39 
  // disable watchdog
 - 
 
40 
  MCUSR &= ~(1 << WDRF);
 - 
 
41 
  WDTCSR |= (1 << WDCE) | (1 << WDE);
 - 
 
42 
  WDTCSR = 0;
 - 
 
43 
 
 39 
        // disable watchdog
44 
// This is strange: It should NOT be necessarty to do. But the call of the same,
 40 
        MCUSR &= ~(1 << WDRF);
45 
// in channelMap_readOrDefault (if eeprom read fails) just sets all to 0,0,0,....
 41 
        WDTCSR |= (1 << WDCE) | (1 << WDE);
46 
channelMap_default();
 42 
        WDTCSR = 0;
47 
 
 43 
 
48 
  // initalize modules
 44 
        // initalize modules
49 
  output_init();
 45 
        output_init();
50 
  timer0_init();
 46 
        timer0_init();
51 
  timer2_init();
 - 
 
52 
  usart0_init();
 47 
        // timer2_init();
53 
  //if (CPUType == ATMEGA644P);// usart1_Init();
 - 
 
54 
  RC_Init();
 48 
        usart0_init();
55 
  analog_init();
 - 
 
56 
  I2C_init();
 49 
        //if (CPUType == ATMEGA644P);// usart1_Init();
57 
#ifdef USE_NAVICTRL
 50 
        RC_Init();
58 
  SPI_MasterInit();
 51 
        I2C_init();
59 
#endif
 52 
        analog_init();
60 
#ifdef USE_MK3MAG
 53 
#ifdef USE_MK3MAG
61 
  MK3MAG_init();
 54 
        MK3MAG_init();
Line 62... Line 55...
62 
#endif
 55 
#endif
63 
#ifdef USE_DIRECT_GPS
 56 
#ifdef USE_DIRECT_GPS
64 
  usart1_init();
 57 
        usart1_init();
65 
#endif
 58 
#endif
66 
 
 59 
 
67 
  // Parameter Set handling
 60 
        // Parameter Set handling
68 
  IMUConfig_readOrDefault();
 61 
        IMUConfig_readOrDefault();
69 
  channelMap_readOrDefault();
 62 
        channelMap_readOrDefault();
70 
  motorMixer_readOrDefault();
 63 
        outputMixer_readOrDefault();
71 
  paramSet_readOrDefault();
 64 
        paramSet_readOrDefault();
72 
 
 65 
 
73 
  // enable interrupts global
 66 
        // enable interrupts global
74 
  sei();
 67 
        sei();
75 
 
 68 
 
76 
  printf("\n\r===================================");
 69 
        printf("\n\r===================================");
77 
  printf("\n\rFlightControl");
 70 
        printf("\n\rFlightControl");
- 
 
 71 
        printf("\n\rHardware: Custom");
78 
  printf("\n\rHardware: Custom");
 72 
        printf("\n\r     CPU: Atmega644");
79 
  printf("\n\r     CPU: Atmega644");
 73 
        if (CPUType == ATMEGA644P)
80 
  if (CPUType == ATMEGA644P)
 74 
                printf("p");
81 
    printf("p");
 75 
        printf("\n\rSoftware: V%d.%d%c", VERSION_MAJOR, VERSION_MINOR,
82 
  printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a');
 76 
                        VERSION_PATCH + 'a');
83 
  printf("\n\r===================================");
 77 
        printf("\n\r===================================");
84 
 
 78 
 
85 
  // Wait for a short time (otherwise the RC channel check won't work below)
 79 
        // Wait for a short time (otherwise the RC channel check won't work below)
86 
  // timer = SetDelay(500);
 80 
        // timer = SetDelay(500);
87 
  // while(!CheckDelay(timer));
 81 
        // while(!CheckDelay(timer));
88 
 
 82 
 
89 
  // Instead, while away the time by flashing the 2 outputs:
 83 
        // Instead, while away the time by flashing the 2 outputs:
90 
  // First J16, then J17. Makes it easier to see which is which.
 84 
        // First J16, then J17. Makes it easier to see which is which.
91 
  timer = setDelay(200);
 85 
        timer = setDelay(200);
92 
  outputSet(0,1);
 86 
        output_setLED(0, 1);
93 
  GRN_OFF;
 87 
        GRN_OFF;
94 
  RED_ON;
 88 
        RED_ON;
95 
  while (!checkDelay(timer))
 89 
        while (!checkDelay(timer))
96 
    ;
 90 
                ;
97 
 
 91 
 
98 
  timer = setDelay(200);
 92 
        timer = setDelay(200);
99 
  outputSet(0,0);
 93 
        output_setLED(0, 0);
100 
  outputSet(1,1);
 94 
        output_setLED(1, 1);
101 
  RED_OFF;
 95 
        RED_OFF;
102 
  GRN_ON;
 96 
        GRN_ON;
103 
  while (!checkDelay(timer))
 97 
        while (!checkDelay(timer))
104 
    ;
 98 
                ;
105 
 
 99 
 
Line 106... Line 100...
106 
  timer = setDelay(200);
 100 
        timer = setDelay(200);
Line 107... Line 101...
107 
  while (!checkDelay(timer))
 101 
        while (!checkDelay(timer))
Line 108... Line 102...
108 
    ;
 102 
                ;
109 
  outputSet(1,0);
 103 
        output_setLED(1, 0);
110 
  GRN_OFF;
 104 
        GRN_OFF;
Line 111... Line 105...
111 
 
 105 
 
112 
  twi_diagnostics();
 106 
        twi_diagnostics();
113 
 
 107 
 
Line 114... Line 108...
114 
  printf("\n\r===================================");
 108 
        printf("\n\r===================================");
115 
 
 - 
 
116 
#ifdef USE_NAVICTRL
 - 
 
117 
  printf("\n\rSupport for NaviCtrl");
 - 
 
Line 118... Line -...
118 
#endif
 - 
 
119 
 
 - 
 
120 
#ifdef USE_DIRECT_GPS
 - 
 
121 
  printf("\n\rDirect (no NaviCtrl) navigation");
 - 
 
122 
#endif
 - 
 
123 
 
 - 
 
124 
  controlMixer_setNeutral();
 - 
 
125 
 
 - 
 
126 
  // Cal. attitude sensors and reset integrals.
 - 
 
127 
  attitude_setNeutral();
 - 
 
128 
 
 - 
 
129 
  // Init flight parameters
 - 
 
130 
  // flight_setNeutral();
 - 
 
131 
 
 - 
 
132 
  beep(2000);
 109 
 
133 
 
 - 
 
134 
  printf("\n\rControl: ");
 - 
 
135 
  if (staticParams.bitConfig & CFG_HEADING_HOLD)
 - 
 
136 
    printf("Heading Hold");
 - 
 
137 
    else printf("RTL Mode");
 - 
 
138 
 
 - 
 
139 
  printf("\n\n\r");
 - 
 
140 
 
 - 
 
141 
  LCD_clear();
 - 
 
142 
 
 - 
 
143 
  I2CTimeout = 5000;
 - 
 
144 
 
 - 
 
145 
  while (1) {
 - 
 
146 
    if (runFlightControl) { // control interval
 - 
 
147 
      runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
 - 
 
148 
      if (!analogDataReady) {
 - 
 
149 
        // Analog data should have been ready but is not!!
 - 
 
150 
        debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER;
 - 
 
151 
      } else {
 - 
 
152 
        debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
 - 
 
153 
 
 - 
 
154 
        J4HIGH;
 - 
 
155 
        // This is probably the correct order:
 - 
 
156 
        // The attitude computation should not depend on anything from control (except maybe the estimation of control activity level)
 - 
 
157 
        // The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc.
 - 
 
158 
        // Flight control uses results from both.
 - 
 
159 
        calculateFlightAttitude();
 - 
 
160 
        controlMixer_periodicTask();
 - 
 
161 
        commands_handleCommands();
 - 
 
162 
        flight_control();
 - 
 
163 
        J4LOW;
 - 
 
164 
 
 - 
 
165 
        if (!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing or timeout
 - 
 
166 
          if (!I2CTimeout) {
 - 
 
167 
            I2C_Reset();
 - 
 
168 
            I2CTimeout = 5;
 - 
 
169 
          }
 - 
 
170 
          beepI2CAlarm();
 - 
 
171 
        }
 - 
 
172 
       
 - 
 
173 
      // Allow serial data transmission if there is still time, or if we are not flying anyway.
 - 
 
174 
      if (!runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) {
 - 
 
175 
          usart0_transmitTxData();
 - 
 
176 
        }
 - 
 
177 
 
 - 
 
178 
        usart0_processRxData();
 - 
 
179 
 
 - 
 
180 
        if (checkDelay(timer)) {
 - 
 
181 
          if (UBat <= UBAT_AT_5V) {
 - 
 
182 
            // Do nothing. The voltage on the input side of the regulator is <5V;
 - 
 
183 
            // we must be running off USB power. Keep it quiet.
 - 
 
184 
          } else if (UBat < staticParams.batteryVoltageWarning) {
 - 
 
185 
            beepBatteryAlarm();
 - 
 
186 
          }
 - 
 
187 
 
 - 
 
188 
#ifdef USE_NAVICTRL
 - 
 
189 
          SPI_StartTransmitPacket();
 - 
 
190 
          SendSPI = 4;
 - 
 
191 
#endif
 - 
 
192 
          timer = setDelay(20); // every 20 ms
 - 
 
193 
        }
 - 
 
194 
        output_update();
 - 
 
Line -... Line 110...
- 
 
 110 
#ifdef USE_NAVICTRL
- 
 
 111 
        printf("\n\rSupport for NaviCtrl");
- 
 
 112 
#endif
- 
 
 113 
 
- 
 
 114 
#ifdef USE_DIRECT_GPS
- 
 
 115 
        printf("\n\rDirect (no NaviCtrl) navigation");
- 
 
 116 
#endif
- 
 
 117 
 
- 
 
 118 
        controlMixer_setNeutral();
- 
 
 119 
 
- 
 
 120 
        I2CTimeout = 5000;
- 
 
 121 
 
- 
 
 122 
        // Cal. attitude sensors and reset integrals.
- 
 
 123 
        attitude_setNeutral();
- 
 
 124 
 
- 
 
 125 
        // Init flight parameters
- 
 
 126 
        // flight_setNeutral();
- 
 
 127 
 
- 
 
 128 
        beep(2000);
195 
      }
 129 
        printf("\n\n\r");
196 
 
 130 
 
- 
 
 131 
        timer2_init();
197 
#ifdef USE_NAVICTRL
 132 
       
- 
 
 133 
        /*
198 
      if(!SendSPI) {
 134 
         * Main loop updates attitude and does some nonessential tasks
199 
        // SendSPI is decremented in timer0.c with a rate of 9.765 kHz.
 135 
         * like beeping alarms and computing servo values.
- 
 
 136 
         */
200 
        // within the SPI_TransmitByte() routine the value is set to 4.
 137 
        while (1) {
201 
        // I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz,
 138 
                attitude_update();
202 
        // and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms.
 - 
 
203 
        SPI_TransmitByte();
 139 
 
204 
      }
 140 
        // This is fair to leave here - servo values only need update after a change in attitude anyway.
205 
#endif
 141 
        // calculateServoValues();