WebSVN - FlightCtrl - Diff - Rev 1869 and 1870 - /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 example: 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);
  OUTPUT_SET(0,1);
  GRN_OFF;
  RED_ON;
  while (!CheckDelay(timer))
    ;
 
  timer = SetDelay(200);
  OUTPUT_SET(0,0);
  OUTPUT_SET(1,1);
  RED_OFF;
  GRN_ON;
  while (!CheckDelay(timer))
    ;
 
  timer = SetDelay(200);
  while (!CheckDelay(timer))
    ;
  OUTPUT_SET(1,0);
 
  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();
 
  // 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) { // control interval
      runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
 
      DebugOut.Digital[0] &= ~DEBUG_MAINLOOP_TIMER;
      DebugOut.Digital[1] &= ~DEBUG_MAINLOOP_TIMER;
 
      if (analogDataReady) {
        // J4HIGH;
        flight_control();
        // J4LOW;
 
        /*
         * If the motors are running (MKFlags & MKFLAG_MOTOR_RUN in flight.c), transmit
         * the throttle vector just computed. Otherwise, if motor test is engaged, transmit
         * the test throttle vector. If no testing, stop all motors.
         */
        // Obsoleted.
        // transmitMotorThrottleData();
 
        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();
        } else {
          DebugOut.Digital[1] |= DEBUG_MAINLOOP_TIMER;
        }
 
        usart0_ProcessRxData();
 
        if (CheckDelay(timer)) {
          if (UBat <= UBAT_AT_5V) {
            // Do nothing. The voltage on the input side of the regulator is <5V;
            // we must be running off USB power. Keep it quiet.
          } else 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 1869	Rev 1870
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 example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted	35	// + Commercial use (for example: 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)	106	if (CPUType == ATMEGA644P)
107	usart1_Init();	107	usart1_Init();
108	RC_Init();	108	RC_Init();
109	analog_init();	109	analog_init();
110	I2C_init();	110	I2C_init();
111	#ifdef USE_NAVICTRL	111	#ifdef USE_NAVICTRL
112	SPI_MasterInit();	112	SPI_MasterInit();
113	#endif	113	#endif
114	#ifdef USE_MK3MAG	114	#ifdef USE_MK3MAG
115	MK3MAG_Init();	115	MK3MAG_Init();
116	#endif	116	#endif
117		117
118	// enable interrupts global	118	// enable interrupts global
119	sei();	119	sei();
120		120
121	printf("\n\r===================================");	121	printf("\n\r===================================");
122	printf("\n\rFlightControl");	122	printf("\n\rFlightControl");
123	printf("\n\rHardware: Custom");	123	printf("\n\rHardware: Custom");
124	printf("\r\n CPU: Atmega644");	124	printf("\r\n CPU: Atmega644");
125	if (CPUType == ATMEGA644P)	125	if (CPUType == ATMEGA644P)
126	printf("p");	126	printf("p");
127	printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a');	127	printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a');
128	printf("\n\r===================================");	128	printf("\n\r===================================");
129		129
130	// Parameter Set handling	130	// Parameter Set handling
131	ParamSet_Init();	131	ParamSet_Init();
132		132
133	// Wait for a short time (otherwise the RC channel check won't work below)	133	// Wait for a short time (otherwise the RC channel check won't work below)
134	// timer = SetDelay(500);	134	// timer = SetDelay(500);
135	// while(!CheckDelay(timer));	135	// while(!CheckDelay(timer));
136		136
137	// Instead, while away the time by flashing the 2 outputs:	137	// Instead, while away the time by flashing the 2 outputs:
138	// First J16, then J17. Makes it easier to see which is which.	138	// First J16, then J17. Makes it easier to see which is which.
139	timer = SetDelay(200);	139	timer = SetDelay(200);
140	OUTPUT_SET(0,1);	140	OUTPUT_SET(0,1);
141	GRN_OFF;	141	GRN_OFF;
142	RED_ON;	142	RED_ON;
143	while (!CheckDelay(timer))	143	while (!CheckDelay(timer))
144	;	144	;
145		145
146	timer = SetDelay(200);	146	timer = SetDelay(200);
147	OUTPUT_SET(0,0);	147	OUTPUT_SET(0,0);
148	OUTPUT_SET(1,1);	148	OUTPUT_SET(1,1);
149	RED_OFF;	149	RED_OFF;
150	GRN_ON;	150	GRN_ON;
151	while (!CheckDelay(timer))	151	while (!CheckDelay(timer))
152	;	152	;
153		153
154	timer = SetDelay(200);	154	timer = SetDelay(200);
155	while (!CheckDelay(timer))	155	while (!CheckDelay(timer))
156	;	156	;
157	OUTPUT_SET(1,0);	157	OUTPUT_SET(1,0);
158		158
159	twi_diagnostics();	159	twi_diagnostics();
160		160
161	printf("\n\r===================================");	161	printf("\n\r===================================");
162		162
163	/*	163	/*
164	if(staticParams.GlobalConfig & CFG_HEIGHT_CONTROL)	164	if(staticParams.GlobalConfig & CFG_HEIGHT_CONTROL)
165	{	165	{
166	printf("\n\rCalibrating air pressure sensor..");	166	printf("\n\rCalibrating air pressure sensor..");
167	timer = SetDelay(1000);	167	timer = SetDelay(1000);
168	SearchAirPressureOffset();	168	SearchAirPressureOffset();
169	while (!CheckDelay(timer));	169	while (!CheckDelay(timer));
170	printf("OK\n\r");	170	printf("OK\n\r");
171	}	171	}
172	*/	172	*/
173		173
174	#ifdef USE_NAVICTRL	174	#ifdef USE_NAVICTRL
175	printf("\n\rSupport for NaviCtrl");	175	printf("\n\rSupport for NaviCtrl");
176	#ifdef USE_RC_DSL	176	#ifdef USE_RC_DSL
177	printf("\r\nSupport for DSL RC at 2nd UART");	177	printf("\r\nSupport for DSL RC at 2nd UART");
178	#endif	178	#endif
179	#ifdef USE_RC_SPECTRUM	179	#ifdef USE_RC_SPECTRUM
180	printf("\r\nSupport for SPECTRUM RC at 2nd UART");	180	printf("\r\nSupport for SPECTRUM RC at 2nd UART");
181	#endif	181	#endif
182	#endif	182	#endif
183		183
184	#ifdef USE_MK3MAG	184	#ifdef USE_MK3MAG
185	printf("\n\rSupport for MK3MAG Compass");	185	printf("\n\rSupport for MK3MAG Compass");
186	#endif	186	#endif
187		187
188	#if (defined (USE_MK3MAG))	188	#if (defined (USE_MK3MAG))
189	if(CPUType == ATMEGA644P) printf("\n\rSupport for GPS at 2nd UART");	189	if(CPUType == ATMEGA644P) printf("\n\rSupport for GPS at 2nd UART");
190	else printf("\n\rSupport for GPS at 1st UART");	190	else printf("\n\rSupport for GPS at 1st UART");
191	#endif	191	#endif
192		192
193	controlMixer_setNeutral();	193	controlMixer_setNeutral();
194		194
195	// Cal. attitude sensors and reset integrals.	195	// Cal. attitude sensors and reset integrals.
196	attitude_setNeutral();	196	attitude_setNeutral();
197		197
198	Servo_On();	198	Servo_On();
199		199
200	// Init flight parameters	200	// Init flight parameters
201	flight_setNeutral();	201	flight_setNeutral();
202		202
203	// RED_OFF;	203	// RED_OFF;
204		204
205	beep(2000);	205	beep(2000);
206		206
207	printf("\n\rControl: ");	207	printf("\n\rControl: ");
208	if (staticParams.GlobalConfig & CFG_HEADING_HOLD)	208	if (staticParams.GlobalConfig & CFG_HEADING_HOLD)
209	printf("HeadingHold");	209	printf("HeadingHold");
210	else printf("Neutral (ACC-Mode)");	210	else printf("Neutral (ACC-Mode)");
211		211
212	printf("\n\n\r");	212	printf("\n\n\r");
213		213
214	LCD_Clear();	214	LCD_Clear();
215		215
216	I2CTimeout = 5000;	216	I2CTimeout = 5000;
217		217
218	while (1) {	218	while (1) {
219	if (runFlightControl) { // control interval	219	if (runFlightControl) { // control interval
220	runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0	220	runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
221		221
222	DebugOut.Digital[0] &= ~DEBUG_MAINLOOP_TIMER;	222	DebugOut.Digital[0] &= ~DEBUG_MAINLOOP_TIMER;
223	DebugOut.Digital[1] &= ~DEBUG_MAINLOOP_TIMER;	223	DebugOut.Digital[1] &= ~DEBUG_MAINLOOP_TIMER;
224		224
225	if (analogDataReady) {	225	if (analogDataReady) {
226	J4HIGH;	226	// J4HIGH;
227	flight_control();	227	flight_control();
228	J4LOW;	228	// J4LOW;
229		229
230	/*	230	/*
231	* If the motors are running (MKFlags & MKFLAG_MOTOR_RUN in flight.c), transmit	231	* If the motors are running (MKFlags & MKFLAG_MOTOR_RUN in flight.c), transmit
232	* the throttle vector just computed. Otherwise, if motor test is engaged, transmit	232	* the throttle vector just computed. Otherwise, if motor test is engaged, transmit
233	* the test throttle vector. If no testing, stop all motors.	233	* the test throttle vector. If no testing, stop all motors.
234	*/	234	*/
235	// Obsoleted.	235	// Obsoleted.
236	// transmitMotorThrottleData();	236	// transmitMotorThrottleData();
237		237
238	RED_OFF;	238	RED_OFF;
239		239
240	/*	240	/*
241	Does not belong here. Instead, external control should be ignored in	241	Does not belong here. Instead, external control should be ignored in
242	controlMixer if there was no new data from there for some time.	242	controlMixer if there was no new data from there for some time.
243	if(externalControlActive) externalControlActive--;	243	if(externalControlActive) externalControlActive--;
244	else {	244	else {
245	externalControl.config = 0;	245	externalControl.config = 0;
246	externalStickPitch = 0;	246	externalStickPitch = 0;
247	externalStickRoll = 0;	247	externalStickRoll = 0;
248	externalStickYaw = 0;	248	externalStickYaw = 0;
249	}	249	}
250	*/	250	*/
251		251
252	/*	252	/*
253	Does not belong here.	253	Does not belong here.
254	if(RC_Quality) RC_Quality--;	254	if(RC_Quality) RC_Quality--;
255	*/	255	*/
256		256
257	/* Does not belong here. Well since we are not supporting navi right now anyway, leave out.	257	/* Does not belong here. Well since we are not supporting navi right now anyway, leave out.
258	#ifdef USE_NAVICTRL	258	#ifdef USE_NAVICTRL
259	if(NCDataOkay) {	259	if(NCDataOkay) {
260	if(--NCDataOkay == 0) // no data from NC	260	if(--NCDataOkay == 0) // no data from NC
261	{ // set gps control sticks neutral	261	{ // set gps control sticks neutral
262	GPSStickPitch = 0;	262	GPSStickPitch = 0;
263	GPSStickRoll = 0;	263	GPSStickRoll = 0;
264	NCSerialDataOkay = 0;	264	NCSerialDataOkay = 0;
265	}	265	}
266	}	266	}
267	#endif	267	#endif
268	*/	268	*/
269	if (!--I2CTimeout \|\| missingMotor) { // try to reset the i2c if motor is missing ot timeout	269	if (!--I2CTimeout \|\| missingMotor) { // try to reset the i2c if motor is missing ot timeout
270	RED_ON;	270	RED_ON;
271	if (!I2CTimeout) {	271	if (!I2CTimeout) {
272	I2C_Reset();	272	I2C_Reset();
273	I2CTimeout = 5;	273	I2CTimeout = 5;
274	}	274	}
275	} else {	275	} else {
276	RED_OFF;	276	RED_OFF;
277	}	277	}
278		278
279	// Allow Serial Data Transmit if motors must not updated or motors are not running	279	// Allow Serial Data Transmit if motors must not updated or motors are not running
280	if (!runFlightControl \|\| !(MKFlags & MKFLAG_MOTOR_RUN)) {	280	if (!runFlightControl \|\| !(MKFlags & MKFLAG_MOTOR_RUN)) {
281	usart0_TransmitTxData();	281	usart0_TransmitTxData();
282	} else {	282	} else {
283	DebugOut.Digital[1] \|= DEBUG_MAINLOOP_TIMER;	283	DebugOut.Digital[1] \|= DEBUG_MAINLOOP_TIMER;
284	}	284	}
285		285
286	usart0_ProcessRxData();	286	usart0_ProcessRxData();
287		287
288	if (CheckDelay(timer)) {	288	if (CheckDelay(timer)) {
289	if (UBat <= UBAT_AT_5V) {	289	if (UBat <= UBAT_AT_5V) {
290	// Do nothing. The voltage on the input side of the regulator is <5V;	290	// Do nothing. The voltage on the input side of the regulator is <5V;
291	// we must be running off USB power. Keep it quiet.	291	// we must be running off USB power. Keep it quiet.
292	} else if (UBat < staticParams.LowVoltageWarning) {	292	} else if (UBat < staticParams.LowVoltageWarning) {
293	beepBatteryAlarm();	293	beepBatteryAlarm();
294	}	294	}
295		295
296	#ifdef USE_NAVICTRL	296	#ifdef USE_NAVICTRL
297	SPI_StartTransmitPacket();	297	SPI_StartTransmitPacket();
298	SendSPI = 4;	298	SendSPI = 4;
299	#endif	299	#endif
300	timer = SetDelay(20); // every 20 ms	300	timer = SetDelay(20); // every 20 ms
301	}	301	}
302	output_update();	302	output_update();
303	}	303	}
304		304
305	#ifdef USE_NAVICTRL	305	#ifdef USE_NAVICTRL
306	if(!SendSPI) {	306	if(!SendSPI) {
307	// SendSPI is decremented in timer0.c with a rate of 9.765 kHz.	307	// SendSPI is decremented in timer0.c with a rate of 9.765 kHz.
308	// within the SPI_TransmitByte() routine the value is set to 4.	308	// within the SPI_TransmitByte() routine the value is set to 4.
309	// I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz,	309	// I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz,
310	// and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms.	310	// and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms.
311	SPI_TransmitByte();	311	SPI_TransmitByte();
312	}	312	}
313	#endif	313	#endif
314	}	314	}
315	}	315	}
316	return (1);	316	return (1);
317	}	317	}
318		318

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/main.c - Rev 1869 → 1870