Subversion Repositories FlightCtrl

Rev

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

Rev Author Line No. Line
1612 dongfang 1
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2
// + Copyright (c) 04.2007 Holger Buss
1868 - 3
// + Nur für den privaten Gebrauch
1612 dongfang 4
// + www.MikroKopter.com
5
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1868 - 6
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
7
// + dass eine Nutzung (auch auszugsweise) nur für den privaten und nicht-kommerziellen Gebrauch zulässig ist.
1612 dongfang 8
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
9
// + bzgl. der Nutzungsbedingungen aufzunehmen.
1868 - 10
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
1612 dongfang 11
// + Verkauf von Luftbildaufnahmen, usw.
12
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1868 - 13
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
14
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
1612 dongfang 15
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
16
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
1868 - 17
// + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
1612 dongfang 18
// + eindeutig als Ursprung verlinkt und genannt werden
19
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1868 - 20
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
1612 dongfang 21
// + Benutzung auf eigene Gefahr
1868 - 22
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
1612 dongfang 23
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
1868 - 25
// + mit unserer Zustimmung zulässig
1612 dongfang 26
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
27
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
28
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
29
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
30
// + this list of conditions and the following disclaimer.
31
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
32
// +     from this software without specific prior written permission.
33
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
34
// +     for non-commercial use (directly or indirectly)
1868 - 35
// +     Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
1612 dongfang 36
// +     with our written permission
37
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
38
// +     clearly linked as origin
39
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
40
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
41
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
42
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
43
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
44
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
45
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
46
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
47
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
48
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
49
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
50
// +  POSSIBILITY OF SUCH DAMAGE.
51
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1623 - 52
 
1612 dongfang 53
#include <avr/boot.h>
54
#include <avr/io.h>
55
#include <avr/interrupt.h>
56
#include <util/delay.h>
57
 
58
#include "timer0.h"
59
#include "timer2.h"
60
#include "uart0.h"
61
#include "uart1.h"
62
#include "output.h"
63
#include "menu.h"
64
#include "attitude.h"
65
#include "flight.h"
66
#include "controlMixer.h"
67
#include "rc.h"
68
#include "analog.h"
69
#include "configuration.h"
70
#include "printf_P.h"
71
#include "twimaster.h"
72
#ifdef USE_NAVICTRL
73
#include "spi.h"
74
#endif
75
#ifdef USE_MK3MAG
76
#include "mk3mag.h"
77
#endif
78
#include "eeprom.h"
79
 
1821 - 80
int16_t main(void) {
1864 - 81
  uint16_t timer;
1612 dongfang 82
 
1864 - 83
  // disable interrupts global
84
  cli();
1612 dongfang 85
 
1864 - 86
  // analyze hardware environment
87
  CPUType = getCPUType();
88
  BoardRelease = getBoardRelease();
1612 dongfang 89
 
1864 - 90
  // disable watchdog
91
  MCUSR &= ~(1 << WDRF);
92
  WDTCSR |= (1 << WDCE) | (1 << WDE);
93
  WDTCSR = 0;
1612 dongfang 94
 
1864 - 95
  // PPM_in[CH_THROTTLE] = 0;
96
  // Why??? They are already initialized to 0.
97
  // stickPitch = stickRoll = stickYaw = 0;
1612 dongfang 98
 
1864 - 99
  RED_OFF;
1612 dongfang 100
 
1864 - 101
  // initalize modules
102
  output_init();
103
  timer0_init();
104
  timer2_init();
105
  usart0_Init();
106
  if (CPUType == ATMEGA644P)
107
    usart1_Init();
108
  RC_Init();
109
  analog_init();
110
  I2C_init();
1612 dongfang 111
#ifdef USE_NAVICTRL
1864 - 112
  SPI_MasterInit();
1612 dongfang 113
#endif
114
#ifdef USE_MK3MAG
1864 - 115
  MK3MAG_Init();
1612 dongfang 116
#endif
117
 
1864 - 118
  // enable interrupts global
119
  sei();
1612 dongfang 120
 
1864 - 121
  printf("\n\r===================================");
122
  printf("\n\rFlightControl");
123
  printf("\n\rHardware: Custom");
124
  printf("\r\n     CPU: Atmega644");
125
  if (CPUType == ATMEGA644P)
126
    printf("p");
127
  printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a');
128
  printf("\n\r===================================");
1612 dongfang 129
 
1864 - 130
  // Parameter Set handling
131
  ParamSet_Init();
1634 - 132
 
1864 - 133
  // Wait for a short time (otherwise the RC channel check won't work below)
134
  // timer = SetDelay(500);
135
  // while(!CheckDelay(timer));
1612 dongfang 136
 
1864 - 137
  // Instead, while away the time by flashing the 2 outputs:
138
  // First J16, then J17. Makes it easier to see which is which.
1887 - 139
  timer = setDelay(200);
1864 - 140
  OUTPUT_SET(0,1);
141
  GRN_OFF;
142
  RED_ON;
1887 - 143
  while (!checkDelay(timer))
1864 - 144
    ;
1612 dongfang 145
 
1887 - 146
  timer = setDelay(200);
1864 - 147
  OUTPUT_SET(0,0);
148
  OUTPUT_SET(1,1);
149
  RED_OFF;
150
  GRN_ON;
1887 - 151
  while (!checkDelay(timer))
1864 - 152
    ;
1849 - 153
 
1887 - 154
  timer = setDelay(200);
155
  while (!checkDelay(timer))
1864 - 156
    ;
157
  OUTPUT_SET(1,0);
1612 dongfang 158
 
1864 - 159
  twi_diagnostics();
1634 - 160
 
1864 - 161
  printf("\n\r===================================");
1612 dongfang 162
 
1864 - 163
  /*
164
   if(staticParams.GlobalConfig & CFG_HEIGHT_CONTROL)
165
   {
166
   printf("\n\rCalibrating air pressure sensor..");
167
   timer = SetDelay(1000);
168
   SearchAirPressureOffset();
169
   while (!CheckDelay(timer));
170
   printf("OK\n\r");
171
   }
172
   */
1612 dongfang 173
 
174
#ifdef USE_NAVICTRL
1864 - 175
  printf("\n\rSupport for NaviCtrl");
1612 dongfang 176
#ifdef USE_RC_DSL
1864 - 177
  printf("\r\nSupport for DSL RC at 2nd UART");
1612 dongfang 178
#endif
179
#ifdef USE_RC_SPECTRUM
1864 - 180
  printf("\r\nSupport for SPECTRUM RC at 2nd UART");
1612 dongfang 181
#endif
182
#endif
183
 
184
#ifdef USE_MK3MAG
1864 - 185
  printf("\n\rSupport for MK3MAG Compass");
1612 dongfang 186
#endif
187
 
188
#if (defined (USE_MK3MAG))
1887 - 189
  if(CPUType == ATMEGA644P) printf("\n\r2 UART");
190
  else printf("\n\r1 UART");
1612 dongfang 191
#endif
192
 
1864 - 193
  controlMixer_setNeutral();
1612 dongfang 194
 
1864 - 195
  // Cal. attitude sensors and reset integrals.
196
  attitude_setNeutral();
1612 dongfang 197
 
1864 - 198
  Servo_On();
1612 dongfang 199
 
1864 - 200
  // Init flight parameters
201
  flight_setNeutral();
1612 dongfang 202
 
1864 - 203
  // RED_OFF;
1612 dongfang 204
 
1864 - 205
  beep(2000);
1612 dongfang 206
 
1864 - 207
  printf("\n\rControl: ");
208
  if (staticParams.GlobalConfig & CFG_HEADING_HOLD)
209
    printf("HeadingHold");
210
    else printf("Neutral (ACC-Mode)");
1612 dongfang 211
 
1864 - 212
  printf("\n\n\r");
1612 dongfang 213
 
1864 - 214
  LCD_Clear();
1612 dongfang 215
 
1864 - 216
  I2CTimeout = 5000;
1821 - 217
 
1864 - 218
  while (1) {
1869 - 219
    if (runFlightControl) { // control interval
1864 - 220
      runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
1872 - 221
      if (!analogDataReady) {
222
        DebugOut.Digital[0] |= DEBUG_MAINLOOP_TIMER;
223
      } else {
224
        DebugOut.Digital[0] &= ~DEBUG_MAINLOOP_TIMER;
1821 - 225
 
1872 - 226
        J4HIGH;
1869 - 227
        flight_control();
1872 - 228
        J4LOW;
1821 - 229
 
1872 - 230
        if (!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing or timeout
1869 - 231
          RED_ON;
232
          if (!I2CTimeout) {
233
            I2C_Reset();
234
            I2CTimeout = 5;
235
          }
236
        } else {
237
          RED_OFF;
1864 - 238
        }
1821 - 239
 
1869 - 240
        // Allow Serial Data Transmit if motors must not updated or motors are not running
241
        if (!runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) {
242
          usart0_TransmitTxData();
243
        }
1821 - 244
 
1869 - 245
        usart0_ProcessRxData();
1821 - 246
 
1887 - 247
        if (checkDelay(timer)) {
1869 - 248
          if (UBat <= UBAT_AT_5V) {
249
            // Do nothing. The voltage on the input side of the regulator is <5V;
250
            // we must be running off USB power. Keep it quiet.
251
          } else if (UBat < staticParams.LowVoltageWarning) {
252
            beepBatteryAlarm();
253
          }
1821 - 254
 
1612 dongfang 255
#ifdef USE_NAVICTRL
1869 - 256
          SPI_StartTransmitPacket();
257
          SendSPI = 4;
1612 dongfang 258
#endif
1887 - 259
          timer = setDelay(20); // every 20 ms
1869 - 260
        }
261
        output_update();
1864 - 262
      }
1821 - 263
 
1612 dongfang 264
#ifdef USE_NAVICTRL
1869 - 265
      if(!SendSPI) {
266
        // SendSPI is decremented in timer0.c with a rate of 9.765 kHz.
267
        // within the SPI_TransmitByte() routine the value is set to 4.
268
        // I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz,
269
        // and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms.
270
        SPI_TransmitByte();
271
      }
272
#endif
1864 - 273
    }
274
  }
275
  return (1);
1612 dongfang 276
}