Subversion Repositories FlightCtrl

Rev

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

Rev Author Line No. Line
1612 dongfang 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 "uart1.h"
10
#include "output.h"
11
#include "menu.h"
12
#include "attitude.h"
2055 - 13
#include "commands.h"
1612 dongfang 14
#include "flight.h"
15
#include "rc.h"
16
#include "analog.h"
17
#include "configuration.h"
18
#include "printf_P.h"
19
#include "twimaster.h"
1969 - 20
#include "controlMixer.h"
1612 dongfang 21
#ifdef USE_NAVICTRL
22
#include "spi.h"
23
#endif
2052 - 24
#ifdef USE_MK3MAG
1612 dongfang 25
#include "mk3mag.h"
26
#endif
27
#include "eeprom.h"
28
 
1821 - 29
int16_t main(void) {
1864 - 30
  uint16_t timer;
1612 dongfang 31
 
1864 - 32
  // disable interrupts global
33
  cli();
1612 dongfang 34
 
1864 - 35
  // analyze hardware environment
2088 - 36
  setCPUType();
37
  setBoardRelease();
1612 dongfang 38
 
1864 - 39
  // disable watchdog
40
  MCUSR &= ~(1 << WDRF);
41
  WDTCSR |= (1 << WDCE) | (1 << WDE);
42
  WDTCSR = 0;
1612 dongfang 43
 
1969 - 44
// This is strange: It should NOT be necessarty to do. But the call of the same,
45
// in channelMap_readOrDefault (if eeprom read fails) just sets all to 0,0,0,....
46
channelMap_default();
47
 
1864 - 48
  // initalize modules
49
  output_init();
50
  timer0_init();
51
  timer2_init();
2018 - 52
  usart0_init();
2088 - 53
  //if (CPUType == ATMEGA644P);// usart1_Init();
1864 - 54
  RC_Init();
55
  analog_init();
56
  I2C_init();
1612 dongfang 57
#ifdef USE_NAVICTRL
1864 - 58
  SPI_MasterInit();
1612 dongfang 59
#endif
2052 - 60
#ifdef USE_MK3MAG
61
  MK3MAG_init();
62
#endif
2039 - 63
#ifdef USE_DIRECT_GPS
64
  usart1_init();
1612 dongfang 65
#endif
66
 
1969 - 67
  // Parameter Set handling
2092 - 68
  IMUConfig_readOrDefault();
1969 - 69
  channelMap_readOrDefault();
2158 - 70
  motorMixer_readOrDefault();
1969 - 71
  paramSet_readOrDefault();
72
 
1864 - 73
  // enable interrupts global
74
  sei();
1612 dongfang 75
 
1864 - 76
  printf("\n\r===================================");
77
  printf("\n\rFlightControl");
78
  printf("\n\rHardware: Custom");
1964 - 79
  printf("\n\r     CPU: Atmega644");
1864 - 80
  if (CPUType == ATMEGA644P)
81
    printf("p");
82
  printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a');
83
  printf("\n\r===================================");
1612 dongfang 84
 
1864 - 85
  // Wait for a short time (otherwise the RC channel check won't work below)
86
  // timer = SetDelay(500);
87
  // while(!CheckDelay(timer));
1612 dongfang 88
 
1864 - 89
  // Instead, while away the time by flashing the 2 outputs:
90
  // First J16, then J17. Makes it easier to see which is which.
1887 - 91
  timer = setDelay(200);
1964 - 92
  outputSet(0,1);
1864 - 93
  GRN_OFF;
94
  RED_ON;
1887 - 95
  while (!checkDelay(timer))
1864 - 96
    ;
1612 dongfang 97
 
1887 - 98
  timer = setDelay(200);
1964 - 99
  outputSet(0,0);
100
  outputSet(1,1);
1864 - 101
  RED_OFF;
102
  GRN_ON;
1887 - 103
  while (!checkDelay(timer))
1864 - 104
    ;
1849 - 105
 
1887 - 106
  timer = setDelay(200);
107
  while (!checkDelay(timer))
1864 - 108
    ;
1964 - 109
  outputSet(1,0);
110
  GRN_OFF;
1612 dongfang 111
 
1864 - 112
  twi_diagnostics();
1634 - 113
 
1864 - 114
  printf("\n\r===================================");
1612 dongfang 115
 
116
#ifdef USE_NAVICTRL
1864 - 117
  printf("\n\rSupport for NaviCtrl");
1612 dongfang 118
#endif
119
 
2039 - 120
#ifdef USE_DIRECT_GPS
121
  printf("\n\rDirect (no NaviCtrl) navigation");
1612 dongfang 122
#endif
123
 
1864 - 124
  controlMixer_setNeutral();
1612 dongfang 125
 
1864 - 126
  // Cal. attitude sensors and reset integrals.
127
  attitude_setNeutral();
1612 dongfang 128
 
1864 - 129
  // Init flight parameters
2055 - 130
  // flight_setNeutral();
1612 dongfang 131
 
1864 - 132
  beep(2000);
1612 dongfang 133
 
1864 - 134
  printf("\n\rControl: ");
1960 - 135
  if (staticParams.bitConfig & CFG_HEADING_HOLD)
2018 - 136
    printf("Heading Hold");
137
    else printf("RTL Mode");
1612 dongfang 138
 
1864 - 139
  printf("\n\n\r");
1612 dongfang 140
 
2018 - 141
  LCD_clear();
1612 dongfang 142
 
1864 - 143
  I2CTimeout = 5000;
1821 - 144
 
1864 - 145
  while (1) {
1869 - 146
    if (runFlightControl) { // control interval
1864 - 147
      runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
1872 - 148
      if (!analogDataReady) {
2018 - 149
        // Analog data should have been ready but is not!!
1955 - 150
        debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER;
1872 - 151
      } else {
1955 - 152
        debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
1821 - 153
 
1872 - 154
        J4HIGH;
2049 - 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.
2039 - 159
        calculateFlightAttitude();
160
        controlMixer_periodicTask();
2055 - 161
        commands_handleCommands();
1869 - 162
        flight_control();
1872 - 163
        J4LOW;
1821 - 164
 
1872 - 165
        if (!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing or timeout
1869 - 166
          if (!I2CTimeout) {
167
            I2C_Reset();
168
            I2CTimeout = 5;
169
          }
2018 - 170
          beepI2CAlarm();
1864 - 171
        }
1964 - 172
 
2112 - 173
      // Allow serial data transmission if there is still time, or if we are not flying anyway.
174
      if (!runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) {
2018 - 175
          usart0_transmitTxData();
1869 - 176
        }
1821 - 177
 
2018 - 178
        usart0_processRxData();
1821 - 179
 
1887 - 180
        if (checkDelay(timer)) {
1869 - 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.
1960 - 184
          } else if (UBat < staticParams.batteryVoltageWarning) {
1869 - 185
            beepBatteryAlarm();
186
          }
1821 - 187
 
1612 dongfang 188
#ifdef USE_NAVICTRL
1869 - 189
          SPI_StartTransmitPacket();
190
          SendSPI = 4;
1612 dongfang 191
#endif
1887 - 192
          timer = setDelay(20); // every 20 ms
1869 - 193
        }
194
        output_update();
1864 - 195
      }
1821 - 196
 
1612 dongfang 197
#ifdef USE_NAVICTRL
1869 - 198
      if(!SendSPI) {
199
        // SendSPI is decremented in timer0.c with a rate of 9.765 kHz.
200
        // within the SPI_TransmitByte() routine the value is set to 4.
201
        // I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz,
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();
204
      }
205
#endif
1975 - 206
 
207
          calculateServoValues();
1980 - 208
 
2018 - 209
          if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error.
1980 - 210
        debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER;
211
      } else {
212
        debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;
213
          }      
1864 - 214
    }
215
  }
216
  return (1);
1612 dongfang 217
}