Subversion Repositories FlightCtrl

Rev

Rev 1612 | Rev 1634 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

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