Subversion Repositories FlightCtrl

Rev

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

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