WebSVN - FlightCtrl - Diff - Rev 1623 and 1634 - /branches/dongfang_FC


// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + Nur für den privaten Gebrauch
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
// + dass eine Nutzung (auch auszugsweise) nur für den privaten und nicht-kommerziellen Gebrauch zulässig ist.
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
// + bzgl. der Nutzungsbedingungen aufzunehmen.
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
// + Verkauf von Luftbildaufnahmen, usw.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
// + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + eindeutig als Ursprung verlinkt und genannt werden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
// + Benutzung auf eigene Gefahr
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
// + mit unserer Zustimmung zulässig
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
// + this list of conditions and the following disclaimer.
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
// +     from this software without specific prior written permission.
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
// +     for non-commercial use (directly or indirectly)
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
// +     with our written permission
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
// +     clearly linked as origin
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// +  POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
#include <avr/boot.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
 
#include "timer0.h"
#include "timer2.h"
#include "uart0.h"
#include "uart1.h"
#include "output.h"
#include "menu.h"
#include "attitude.h"
#include "flight.h"
#include "controlMixer.h"
#include "rc.h"
#include "analog.h"
#include "configuration.h"
#include "printf_P.h"
#include "twimaster.h"
#ifdef USE_NAVICTRL
#include "spi.h"
#endif
#ifdef USE_MK3MAG
#include "mk3mag.h"
#endif
#include "eeprom.h"
 
int16_t main (void) {
  uint16_t timer;
 
  // disable interrupts global
  cli();
 
  // analyze hardware environment
  CPUType = getCPUType();
  BoardRelease = getBoardRelease();
 
  // disable watchdog
  MCUSR &=~(1<<WDRF);
  WDTCSR |= (1<<WDCE)|(1<<WDE);
  WDTCSR = 0;
 
  // PPM_in[CH_THROTTLE] = 0;
  // Why??? They are already initialized to 0.
  // stickPitch = stickRoll = stickYaw = 0;
 
  RED_OFF;
 
  // initalize modules
  output_init();
  timer0_init();
  timer2_init();
  USART0_Init();
  if(CPUType == ATMEGA644P) USART1_Init();
  RC_Init();
  analog_init();
  I2C_init();
#ifdef USE_NAVICTRL
  SPI_MasterInit();
#endif
#ifdef USE_MK3MAG
  MK3MAG_Init();
#endif
 
  // enable interrupts global
  sei();
 
  printf("\n\r===================================");
  printf("\n\rFlightControl");
  printf("\n\rHardware: Custom");
  printf("\r\n     CPU: Atmega644");
  if(CPUType == ATMEGA644P)
    printf("p");
  printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a');
  printf("\n\r===================================");
 
  // Parameter Set handling
  ParamSet_Init();
 
  // Wait for a short time (otherwise the RC channel check won't work below)
  // timer = SetDelay(500);
  // while(!CheckDelay(timer));
 
  // Instead, while away the time by flashing the 2 outputs:
 
  // First J16, then J17. Makes it easier to see which is which.
  timer = SetDelay(200);
  J16_ON;
  GRN_OFF;
  RED_ON;
  while(!CheckDelay(timer));
 
  timer = SetDelay(200);
  J16_OFF;
  RED_OFF;
  GRN_ON;
  J17_ON;
  while(!CheckDelay(timer));
 
  timer = SetDelay(200);
  while(!CheckDelay(timer));
  J17_OFF;
 
  twi_diagnostics();
 
  printf("\n\r===================================");
 
  /*
  if(staticParams.GlobalConfig & CFG_HEIGHT_CONTROL)
    {
      printf("\n\rCalibrating air pressure sensor..");
      timer = SetDelay(1000);
      SearchAirPressureOffset();
      while (!CheckDelay(timer));
      printf("OK\n\r");
    }
  */
 
#ifdef USE_NAVICTRL
  printf("\n\rSupport for NaviCtrl");
#ifdef USE_RC_DSL
  printf("\r\nSupport for DSL RC at 2nd UART");
#endif
#ifdef USE_RC_SPECTRUM
  printf("\r\nSupport for SPECTRUM RC at 2nd UART");
#endif
#endif
 
#ifdef USE_MK3MAG
  printf("\n\rSupport for MK3MAG Compass");
#endif
 
#if (defined (USE_MK3MAG))
  if(CPUType == ATMEGA644P) printf("\n\rSupport for GPS at 2nd UART");
  else                      printf("\n\rSupport for GPS at 1st UART");
#endif
 
  controlMixer_setNeutral(0);
 
  // Cal. attitude sensors and reset integrals.
  attitude_setNeutral();
 
  Servo_On();
 
  // Init flight parameters
  flight_setNeutral();
 
  // RED_OFF;
 
  beep(2000);
 
  printf("\n\rControl: ");
  if (staticParams.GlobalConfig & CFG_HEADING_HOLD) printf("HeadingHold");
  else printf("Neutral (ACC-Mode)");
 
  printf("\n\n\r");
 
  LCD_Clear();
 
  I2CTimeout = 5000;
 
  while (1) {
    if(runFlightControl && analogDataReady) {      // control interval
      runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
     
      J4HIGH;
      flight_control();
      J4LOW;
     
      sendMotorData(); // the flight control code
     
      RED_OFF;
     
      /*
        Does not belong here. Instead, external control should be ignored in
        controlMixer if there was no new data from there for some time.
      if(externalControlActive) externalControlActive--;
      else {
        externalControl.config = 0;
        externalStickPitch = 0;
        externalStickRoll = 0;
        externalStickYaw = 0;
      }
      */
     
      /*
        Does not belong here.
        if(RC_Quality)  RC_Quality--;
      */
     
      /* Does not belong here. Well since we are not supporting navi right now anyway, leave out.
#ifdef USE_NAVICTRL
      if(NCDataOkay) {
        if(--NCDataOkay == 0) // no data from NC
          {  // set gps control sticks neutral
            GPSStickPitch = 0;
            GPSStickRoll = 0;
            NCSerialDataOkay = 0;
          }
      }
#endif
      */
      if(!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing ot timeout
        RED_ON;
        if(!I2CTimeout) {
          I2C_Reset();
          I2CTimeout = 5;
        }
      } else {
        RED_OFF;
      }
     
 
      // Allow Serial Data Transmit if motors must not updated or motors are not running
      if( !runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) {
        USART0_TransmitTxData();
      }
     
      USART0_ProcessRxData();
     
      if(CheckDelay(timer)) {
        if(UBat < staticParams.LowVoltageWarning) {
          beepBatteryAlarm();
        }
#ifdef USE_NAVICTRL
        SPI_StartTransmitPacket();
        SendSPI = 4;
#endif
        timer = SetDelay(20); // every 20 ms
      }
      output_update();
    }
   
#ifdef USE_NAVICTRL
    if(!SendSPI) {
      // SendSPI is decremented in timer0.c with a rate of 9.765 kHz.
      // within the SPI_TransmitByte() routine the value is set to 4.
      // I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz,
      // and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms.
      SPI_TransmitByte();
    }
#endif
  }
  return (1);
}
 

Rev 1623	Rev 1634
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		52
53	#include <avr/boot.h>	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
135	// Instead, while away the time by flashing the 2 outputs:	136	// Instead, while away the time by flashing the 2 outputs:
136	// First J16, then J17. Makes it easier to see which is which.	-
137		137	// First J16, then J17. Makes it easier to see which is which.
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
156	printf("\n\r===================================");	157	printf("\n\r===================================");
157		158
158	/*	159	/*
159	if(staticParams.GlobalConfig & CFG_HEIGHT_CONTROL)	160	if(staticParams.GlobalConfig & CFG_HEIGHT_CONTROL)
160	{	161	{
161	printf("\n\rCalibrating air pressure sensor..");	162	printf("\n\rCalibrating air pressure sensor..");
162	timer = SetDelay(1000);	163	timer = SetDelay(1000);
163	SearchAirPressureOffset();	164	SearchAirPressureOffset();
164	while (!CheckDelay(timer));	165	while (!CheckDelay(timer));
165	printf("OK\n\r");	166	printf("OK\n\r");
166	}	167	}
167	*/	168	*/
168		169
169	#ifdef USE_NAVICTRL	170	#ifdef USE_NAVICTRL
170	printf("\n\rSupport for NaviCtrl");	171	printf("\n\rSupport for NaviCtrl");
171	#ifdef USE_RC_DSL	172	#ifdef USE_RC_DSL
172	printf("\r\nSupport for DSL RC at 2nd UART");	173	printf("\r\nSupport for DSL RC at 2nd UART");
173	#endif	174	#endif
174	#ifdef USE_RC_SPECTRUM	175	#ifdef USE_RC_SPECTRUM
175	printf("\r\nSupport for SPECTRUM RC at 2nd UART");	176	printf("\r\nSupport for SPECTRUM RC at 2nd UART");
176	#endif	177	#endif
177	#endif	178	#endif
178		179
179	#ifdef USE_MK3MAG	180	#ifdef USE_MK3MAG
180	printf("\n\rSupport for MK3MAG Compass");	181	printf("\n\rSupport for MK3MAG Compass");
181	#endif	182	#endif
182		183
183	#if (defined (USE_MK3MAG))	184	#if (defined (USE_MK3MAG))
184	if(CPUType == ATMEGA644P) printf("\n\rSupport for GPS at 2nd UART");	185	if(CPUType == ATMEGA644P) printf("\n\rSupport for GPS at 2nd UART");
185	else printf("\n\rSupport for GPS at 1st UART");	186	else printf("\n\rSupport for GPS at 1st UART");
186	#endif	187	#endif
187		188
188	controlMixer_setNeutral(0);	189	controlMixer_setNeutral(0);
189		190
190	// Cal. attitude sensors and reset integrals.	191	// Cal. attitude sensors and reset integrals.
191	attitude_setNeutral();	192	attitude_setNeutral();
192		193
193	Servo_On();	194	Servo_On();
194		195
195	// Init flight parameters	196	// Init flight parameters
196	flight_setNeutral();	197	flight_setNeutral();
197		198
198	RED_OFF;	199	// RED_OFF;
199		200
200	beep(2000);	201	beep(2000);
201		202
202	printf("\n\rControl: ");	203	printf("\n\rControl: ");
203	if (staticParams.GlobalConfig & CFG_HEADING_HOLD) printf("HeadingHold");	204	if (staticParams.GlobalConfig & CFG_HEADING_HOLD) printf("HeadingHold");
204	else printf("Neutral (ACC-Mode)");	205	else printf("Neutral (ACC-Mode)");
205		206
206	printf("\n\n\r");	207	printf("\n\n\r");
207		208
208	LCD_Clear();	209	LCD_Clear();
209		210
210	I2CTimeout = 5000;	211	I2CTimeout = 5000;
211		212
212	while (1) {	213	while (1) {
213	if(runFlightControl && analogDataReady) { // control interval	214	if(runFlightControl && analogDataReady) { // control interval
214	runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0	215	runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
215		216
216	J4HIGH;	217	J4HIGH;
217	flight_control();	218	flight_control();
218	J4LOW;	219	J4LOW;
219		220
220	sendMotorData(); // the flight control code	221	sendMotorData(); // the flight control code
221		222
222	RED_OFF;	223	RED_OFF;
223		224
224	/*	225	/*
225	Does not belong here. Instead, external control should be ignored in	226	Does not belong here. Instead, external control should be ignored in
226	controlMixer if there was no new data from there for some time.	227	controlMixer if there was no new data from there for some time.
227	if(externalControlActive) externalControlActive--;	228	if(externalControlActive) externalControlActive--;
228	else {	229	else {
229	externalControl.config = 0;	230	externalControl.config = 0;
230	externalStickPitch = 0;	231	externalStickPitch = 0;
231	externalStickRoll = 0;	232	externalStickRoll = 0;
232	externalStickYaw = 0;	233	externalStickYaw = 0;
233	}	234	}
234	*/	235	*/
235		236
236	/*	237	/*
237	Does not belong here.	238	Does not belong here.
238	if(RC_Quality) RC_Quality--;	239	if(RC_Quality) RC_Quality--;
239	*/	240	*/
240		241
241	/* Does not belong here. Well since we are not supporting navi right now anyway, leave out.	242	/* Does not belong here. Well since we are not supporting navi right now anyway, leave out.
242	#ifdef USE_NAVICTRL	243	#ifdef USE_NAVICTRL
243	if(NCDataOkay) {	244	if(NCDataOkay) {
244	if(--NCDataOkay == 0) // no data from NC	245	if(--NCDataOkay == 0) // no data from NC
245	{ // set gps control sticks neutral	246	{ // set gps control sticks neutral
246	GPSStickPitch = 0;	247	GPSStickPitch = 0;
247	GPSStickRoll = 0;	248	GPSStickRoll = 0;
248	NCSerialDataOkay = 0;	249	NCSerialDataOkay = 0;
249	}	250	}
250	}	251	}
251	#endif	252	#endif
252	*/	253	*/
253	if(!--I2CTimeout \|\| missingMotor) { // try to reset the i2c if motor is missing ot timeout	254	if(!--I2CTimeout \|\| missingMotor) { // try to reset the i2c if motor is missing ot timeout
254	RED_ON;	255	RED_ON;
255	if(!I2CTimeout) {	256	if(!I2CTimeout) {
256	I2C_Reset();	257	I2C_Reset();
257	I2CTimeout = 5;	258	I2CTimeout = 5;
258	}	259	}
259	} else {	260	} else {
260	RED_OFF;	261	RED_OFF;
261	}	262	}
262		263
263	// allow Serial Data Transmit if motors must not updated or motors are not running	-
264	// Why only when that???	264	// Allow Serial Data Transmit if motors must not updated or motors are not running
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

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/main.c @ 2156 - Rev 1623 → 1634