Subversion Repositories FlightCtrl

Rev

Rev 2055 | Rev 2092 | 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
68
  channelMap_readOrDefault();
69
  mixerMatrix_readOrDefault();
70
  paramSet_readOrDefault();
71
 
1864 - 72
  // enable interrupts global
73
  sei();
1612 dongfang 74
 
1864 - 75
  printf("\n\r===================================");
76
  printf("\n\rFlightControl");
77
  printf("\n\rHardware: Custom");
1964 - 78
  printf("\n\r     CPU: Atmega644");
1864 - 79
  if (CPUType == ATMEGA644P)
80
    printf("p");
81
  printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a');
82
  printf("\n\r===================================");
1612 dongfang 83
 
1864 - 84
  // Wait for a short time (otherwise the RC channel check won't work below)
85
  // timer = SetDelay(500);
86
  // while(!CheckDelay(timer));
1612 dongfang 87
 
1864 - 88
  // Instead, while away the time by flashing the 2 outputs:
89
  // First J16, then J17. Makes it easier to see which is which.
1887 - 90
  timer = setDelay(200);
1964 - 91
  outputSet(0,1);
1864 - 92
  GRN_OFF;
93
  RED_ON;
1887 - 94
  while (!checkDelay(timer))
1864 - 95
    ;
1612 dongfang 96
 
1887 - 97
  timer = setDelay(200);
1964 - 98
  outputSet(0,0);
99
  outputSet(1,1);
1864 - 100
  RED_OFF;
101
  GRN_ON;
1887 - 102
  while (!checkDelay(timer))
1864 - 103
    ;
1849 - 104
 
1887 - 105
  timer = setDelay(200);
106
  while (!checkDelay(timer))
1864 - 107
    ;
1964 - 108
  outputSet(1,0);
109
  GRN_OFF;
1612 dongfang 110
 
1864 - 111
  twi_diagnostics();
1634 - 112
 
1864 - 113
  printf("\n\r===================================");
1612 dongfang 114
 
115
#ifdef USE_NAVICTRL
1864 - 116
  printf("\n\rSupport for NaviCtrl");
1612 dongfang 117
#endif
118
 
2039 - 119
#ifdef USE_DIRECT_GPS
120
  printf("\n\rDirect (no NaviCtrl) navigation");
1612 dongfang 121
#endif
122
 
1864 - 123
  controlMixer_setNeutral();
1612 dongfang 124
 
1864 - 125
  // Cal. attitude sensors and reset integrals.
126
  attitude_setNeutral();
1612 dongfang 127
 
1864 - 128
  // Init flight parameters
2055 - 129
  // flight_setNeutral();
1612 dongfang 130
 
1864 - 131
  beep(2000);
1612 dongfang 132
 
1864 - 133
  printf("\n\rControl: ");
1960 - 134
  if (staticParams.bitConfig & CFG_HEADING_HOLD)
2018 - 135
    printf("Heading Hold");
136
    else printf("RTL Mode");
1612 dongfang 137
 
1864 - 138
  printf("\n\n\r");
1612 dongfang 139
 
2018 - 140
  LCD_clear();
1612 dongfang 141
 
1864 - 142
  I2CTimeout = 5000;
1821 - 143
 
1864 - 144
  while (1) {
1869 - 145
    if (runFlightControl) { // control interval
1864 - 146
      runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
1872 - 147
      if (!analogDataReady) {
2018 - 148
        // Analog data should have been ready but is not!!
1955 - 149
        debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER;
1872 - 150
      } else {
1955 - 151
        debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
1821 - 152
 
1872 - 153
        J4HIGH;
2049 - 154
        // This is probably the correct order:
155
        // The attitude computation should not depend on anything from control (except maybe the estimation of control activity level)
156
        // The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc.
157
        // Flight control uses results from both.
2039 - 158
        calculateFlightAttitude();
159
        controlMixer_periodicTask();
2055 - 160
        commands_handleCommands();
1869 - 161
        flight_control();
1872 - 162
        J4LOW;
1821 - 163
 
1872 - 164
        if (!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing or timeout
1869 - 165
          if (!I2CTimeout) {
166
            I2C_Reset();
167
            I2CTimeout = 5;
168
          }
2018 - 169
          beepI2CAlarm();
1864 - 170
        }
1964 - 171
 
1869 - 172
        // Allow Serial Data Transmit if motors must not updated or motors are not running
173
        if (!runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) {
2018 - 174
          usart0_transmitTxData();
1869 - 175
        }
1821 - 176
 
2018 - 177
        usart0_processRxData();
1821 - 178
 
1887 - 179
        if (checkDelay(timer)) {
1869 - 180
          if (UBat <= UBAT_AT_5V) {
181
            // Do nothing. The voltage on the input side of the regulator is <5V;
182
            // we must be running off USB power. Keep it quiet.
1960 - 183
          } else if (UBat < staticParams.batteryVoltageWarning) {
1869 - 184
            beepBatteryAlarm();
185
          }
1821 - 186
 
1612 dongfang 187
#ifdef USE_NAVICTRL
1869 - 188
          SPI_StartTransmitPacket();
189
          SendSPI = 4;
1612 dongfang 190
#endif
1887 - 191
          timer = setDelay(20); // every 20 ms
1869 - 192
        }
193
        output_update();
1864 - 194
      }
1821 - 195
 
1612 dongfang 196
#ifdef USE_NAVICTRL
1869 - 197
      if(!SendSPI) {
198
        // SendSPI is decremented in timer0.c with a rate of 9.765 kHz.
199
        // within the SPI_TransmitByte() routine the value is set to 4.
200
        // I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz,
201
        // and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms.
202
        SPI_TransmitByte();
203
      }
204
#endif
1975 - 205
 
206
          calculateServoValues();
1980 - 207
 
2018 - 208
          if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error.
1980 - 209
        debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER;
210
      } else {
211
        debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;
212
          }      
1864 - 213
    }
214
  }
215
  return (1);
1612 dongfang 216
}