Subversion Repositories FlightCtrl

Rev

Rev 2108 | Rev 2110 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
2108 - 1
#include <avr/boot.h>
2
#include <avr/io.h>
3
#include <avr/interrupt.h>
4
#include <util/delay.h>
5
 
6
#include "timer0.h"
7
#include "timer2.h"
8
#include "uart0.h"
9
#include "output.h"
10
#include "attitude.h"
11
#include "commands.h"
12
#include "flight.h"
13
#include "rc.h"
14
#include "analog.h"
15
#include "configuration.h"
16
#include "controlMixer.h"
17
#include "eeprom.h"
18
#include "printf_P.h"
19
 
20
int16_t main(void) {
21
  uint16_t timer;
22
 
23
  // disable interrupts global
24
  cli();
25
 
26
  // disable watchdog
27
  MCUSR &= ~(1 << WDRF);
28
  WDTCSR |= (1 << WDCE) | (1 << WDE);
29
  WDTCSR = 0;
30
 
31
// This is strange: It should NOT be necessarty to do. But the call of the same,
32
// in channelMap_readOrDefault (if eeprom read fails) just sets all to 0,0,0,....
2109 - 33
  channelMap_default();
2108 - 34
 
35
  // initalize modules
36
  output_init();
37
  timer0_init();
38
  timer2_init();
39
  usart0_init();
40
  RC_Init();
41
  analog_init();
42
 
43
  // Parameter Set handling
44
  IMUConfig_readOrDefault();
45
  channelMap_readOrDefault();
46
  paramSet_readOrDefault();
47
 
48
  // enable interrupts global
49
  sei();
50
 
51
  printf("\n\r===================================");
52
  printf("\n\rFlightControl");
53
  printf("\n\rHardware: Custom");
54
  printf("\n\r     CPU: Atmega328");
2109 - 55
  printf("\n\rSoftware: V%d.%d%c",
56
      VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a');
2108 - 57
  printf("\n\r===================================");
58
 
59
  // Wait for a short time (otherwise the RC channel check won't work below)
60
  // timer = SetDelay(500);
61
  // while(!CheckDelay(timer));
62
 
63
  // Instead, while away the time by flashing the 2 outputs:
64
  // First J16, then J17. Makes it easier to see which is which.
65
  timer = setDelay(200);
2109 - 66
  outputSet(0, 1);
2108 - 67
  GRN_OFF;
68
  RED_ON;
69
  while (!checkDelay(timer))
70
    ;
71
 
72
  timer = setDelay(200);
2109 - 73
  outputSet(0, 0);
74
  outputSet(1, 1);
2108 - 75
  RED_OFF;
76
  GRN_ON;
77
  while (!checkDelay(timer))
78
    ;
79
 
80
  timer = setDelay(200);
81
  while (!checkDelay(timer))
82
    ;
2109 - 83
  outputSet(1, 0);
2108 - 84
  GRN_OFF;
85
 
2109 - 86
  printf("\n\r===================================");
2108 - 87
 
88
#ifdef USE_NAVICTRL
89
  printf("\n\rSupport for NaviCtrl");
90
#endif
91
 
92
#ifdef USE_DIRECT_GPS
93
  printf("\n\rDirect (no NaviCtrl) navigation");
94
#endif
95
 
96
  controlMixer_setNeutral();
97
 
98
  // Cal. attitude sensors and reset integrals.
99
  attitude_setNeutral();
100
 
101
  // Init flight parameters
102
  // flight_setNeutral();
103
 
104
  beep(2000);
105
 
106
  printf("\n\n\r");
107
 
2109 - 108
  while (1) {
2108 - 109
    if (runFlightControl) { // control interval
110
      runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
111
      if (sensorDataReady != ALL_DATA_READY) {
112
        // Analog data should have been ready but is not!!
113
        debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER;
114
      } else {
115
        debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
2109 - 116
      }
117
      J4HIGH;
118
      // This is probably the correct order:
119
      // The attitude computation should not depend on anything from control (except maybe the estimation of control activity level)
120
      // The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc.
121
      // Flight control uses results from both.
122
      calculateFlightAttitude();
123
      controlMixer_periodicTask();
124
      commands_handleCommands();
125
      flight_control();
126
      J4LOW;
2108 - 127
 
2109 - 128
      // Allow Serial Data Transmit if motors must not updated or motors are not running
129
      if (!runFlightControl || !isFlying) {
130
        usart0_transmitTxData();
131
      }
2108 - 132
 
2109 - 133
      usart0_processRxData();
2108 - 134
 
2109 - 135
      if (checkDelay(timer)) {
136
        // Do nothing. The voltage on the input side of the regulator is <5V;
137
        // we must be running off USB power. Keep it quiet.
138
      } else if (UBat < staticParams.batteryVoltageWarning && UBat > UBAT_AT_5V) {
139
        beepBatteryAlarm();
2108 - 140
      }
141
 
2109 - 142
      timer = setDelay(20); // every 20 ms
143
      output_update();
144
    }
2108 - 145
 
2109 - 146
    calculateFeaturedServoValues();
147
 
148
    if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error.
149
      debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER;
150
    } else {
151
      debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;
2108 - 152
    }
153
  }
154
  return (1);
155
}