WebSVN - FlightCtrl - Diff - Rev 1645 and 1775 - /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);
  OUTPUT_ON(0);
  GRN_OFF;
  RED_ON;
  while(!CheckDelay(timer));
 
  timer = SetDelay(200);
  OUTPUT_OFF(0);
  OUTPUT_ON(1);
  RED_OFF;
  GRN_ON;
 
  while(!CheckDelay(timer));
 
  timer = SetDelay(200);
  while(!CheckDelay(timer));
  OUTPUT_OFF(1);
 
  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 && analogDataReady) {      // control interval
      runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
     
      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();
      }
     
      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 1645	Rev 1775
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
136	// Instead, while away the time by flashing the 2 outputs:	136	// Instead, while away the time by flashing the 2 outputs:
137	// First J16, then J17. Makes it easier to see which is which.	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	OUTPUT_ON(0);
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	OUTPUT_OFF(0);
-		146	OUTPUT_ON(1);
146	RED_OFF;	147	RED_OFF;
147	GRN_ON;	148	GRN_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	OUTPUT_OFF(1);
154		154
155	twi_diagnostics();	155	twi_diagnostics();
156		156
157	printf("\n\r===================================");	157	printf("\n\r===================================");
158		158
159	/*	159	/*
160	if(staticParams.GlobalConfig & CFG_HEIGHT_CONTROL)	160	if(staticParams.GlobalConfig & CFG_HEIGHT_CONTROL)
161	{	161	{
162	printf("\n\rCalibrating air pressure sensor..");	162	printf("\n\rCalibrating air pressure sensor..");
163	timer = SetDelay(1000);	163	timer = SetDelay(1000);
164	SearchAirPressureOffset();	164	SearchAirPressureOffset();
165	while (!CheckDelay(timer));	165	while (!CheckDelay(timer));
166	printf("OK\n\r");	166	printf("OK\n\r");
167	}	167	}
168	*/	168	*/
169		169
170	#ifdef USE_NAVICTRL	170	#ifdef USE_NAVICTRL
171	printf("\n\rSupport for NaviCtrl");	171	printf("\n\rSupport for NaviCtrl");
172	#ifdef USE_RC_DSL	172	#ifdef USE_RC_DSL
173	printf("\r\nSupport for DSL RC at 2nd UART");	173	printf("\r\nSupport for DSL RC at 2nd UART");
174	#endif	174	#endif
175	#ifdef USE_RC_SPECTRUM	175	#ifdef USE_RC_SPECTRUM
176	printf("\r\nSupport for SPECTRUM RC at 2nd UART");	176	printf("\r\nSupport for SPECTRUM RC at 2nd UART");
177	#endif	177	#endif
178	#endif	178	#endif
179		179
180	#ifdef USE_MK3MAG	180	#ifdef USE_MK3MAG
181	printf("\n\rSupport for MK3MAG Compass");	181	printf("\n\rSupport for MK3MAG Compass");
182	#endif	182	#endif
183		183
184	#if (defined (USE_MK3MAG))	184	#if (defined (USE_MK3MAG))
185	if(CPUType == ATMEGA644P) printf("\n\rSupport for GPS at 2nd UART");	185	if(CPUType == ATMEGA644P) printf("\n\rSupport for GPS at 2nd UART");
186	else printf("\n\rSupport for GPS at 1st UART");	186	else printf("\n\rSupport for GPS at 1st UART");
187	#endif	187	#endif
188		188
189	controlMixer_setNeutral(0);	189	controlMixer_setNeutral();
190		190
191	// Cal. attitude sensors and reset integrals.	191	// Cal. attitude sensors and reset integrals.
192	attitude_setNeutral();	192	attitude_setNeutral();
193		193
194	Servo_On();	194	Servo_On();
195		195
196	// Init flight parameters	196	// Init flight parameters
197	flight_setNeutral();	197	flight_setNeutral();
198		198
199	// RED_OFF;	199	// RED_OFF;
200		200
201	beep(2000);	201	beep(2000);
202		202
203	printf("\n\rControl: ");	203	printf("\n\rControl: ");
204	if (staticParams.GlobalConfig & CFG_HEADING_HOLD) printf("HeadingHold");	204	if (staticParams.GlobalConfig & CFG_HEADING_HOLD) printf("HeadingHold");
205	else printf("Neutral (ACC-Mode)");	205	else printf("Neutral (ACC-Mode)");
206		206
207	printf("\n\n\r");	207	printf("\n\n\r");
208		208
209	LCD_Clear();	209	LCD_Clear();
210		210
211	I2CTimeout = 5000;	211	I2CTimeout = 5000;
212		212
213	while (1) {	213	while (1) {
214	if(runFlightControl && analogDataReady) { // control interval	214	if(runFlightControl && analogDataReady) { // control interval
215	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
216		216
217	J4HIGH;	217	J4HIGH;
218	flight_control();	218	flight_control();
219	J4LOW;	219	J4LOW;
-		220
-		221	/*
-		222	* If the motors are running (MKFlags & MKFLAG_MOTOR_RUN in flight.c), transmit
220		223	* the throttle vector just computed. Otherwise, if motor test is engaged, transmit
-		224	* the test throttle vector. If no testing, stop all motors.
-		225	*/
-		226	// Obsoleted.
221	sendMotorData(); // the flight control code	227	// transmitMotorThrottleData();
222		228
223	RED_OFF;	229	RED_OFF;
224		230
225	/*	231	/*
226	Does not belong here. Instead, external control should be ignored in	232	Does not belong here. Instead, external control should be ignored in
227	controlMixer if there was no new data from there for some time.	233	controlMixer if there was no new data from there for some time.
228	if(externalControlActive) externalControlActive--;	234	if(externalControlActive) externalControlActive--;
229	else {	235	else {
230	externalControl.config = 0;	236	externalControl.config = 0;
231	externalStickPitch = 0;	237	externalStickPitch = 0;
232	externalStickRoll = 0;	238	externalStickRoll = 0;
233	externalStickYaw = 0;	239	externalStickYaw = 0;
234	}	240	}
235	*/	241	*/
236		242
237	/*	243	/*
238	Does not belong here.	244	Does not belong here.
239	if(RC_Quality) RC_Quality--;	245	if(RC_Quality) RC_Quality--;
240	*/	246	*/
241		247
242	/* Does not belong here. Well since we are not supporting navi right now anyway, leave out.	248	/* Does not belong here. Well since we are not supporting navi right now anyway, leave out.
243	#ifdef USE_NAVICTRL	249	#ifdef USE_NAVICTRL
244	if(NCDataOkay) {	250	if(NCDataOkay) {
245	if(--NCDataOkay == 0) // no data from NC	251	if(--NCDataOkay == 0) // no data from NC
246	{ // set gps control sticks neutral	252	{ // set gps control sticks neutral
247	GPSStickPitch = 0;	253	GPSStickPitch = 0;
248	GPSStickRoll = 0;	254	GPSStickRoll = 0;
249	NCSerialDataOkay = 0;	255	NCSerialDataOkay = 0;
250	}	256	}
251	}	257	}
252	#endif	258	#endif
253	*/	259	*/
254	if(!--I2CTimeout \|\| missingMotor) { // try to reset the i2c if motor is missing ot timeout	260	if(!--I2CTimeout \|\| missingMotor) { // try to reset the i2c if motor is missing ot timeout
255	RED_ON;	261	RED_ON;
256	if(!I2CTimeout) {	262	if(!I2CTimeout) {
257	I2C_Reset();	263	I2C_Reset();
258	I2CTimeout = 5;	264	I2CTimeout = 5;
259	}	265	}
260	} else {	266	} else {
261	RED_OFF;	267	RED_OFF;
262	}	268	}
263		269
264	// Allow Serial Data Transmit if motors must not updated or motors are not running	270	// Allow Serial Data Transmit if motors must not updated or motors are not running
265	if( !runFlightControl \|\| !(MKFlags & MKFLAG_MOTOR_RUN)) {	271	if( !runFlightControl \|\| !(MKFlags & MKFLAG_MOTOR_RUN)) {
266	usart0_TransmitTxData();	272	usart0_TransmitTxData();
267	}	273	}
268		274
269	usart0_ProcessRxData();	275	usart0_ProcessRxData();
270		276
271	if(CheckDelay(timer)) {	277	if(CheckDelay(timer)) {
-		278	if (UBat <= UBAT_AT_5V) {
-		279	// Do nothing. The voltage on the input side of the regulator is <5V;
-		280	// we must be running off USB power. Keep it quiet.
272	if(UBat < staticParams.LowVoltageWarning) {	281	} else if(UBat < staticParams.LowVoltageWarning) {
273	beepBatteryAlarm();	282	beepBatteryAlarm();
274	}	283	}
-		284
275	#ifdef USE_NAVICTRL	285	#ifdef USE_NAVICTRL
276	SPI_StartTransmitPacket();	286	SPI_StartTransmitPacket();
277	SendSPI = 4;	287	SendSPI = 4;
278	#endif	288	#endif
279	timer = SetDelay(20); // every 20 ms	289	timer = SetDelay(20); // every 20 ms
280	}	290	}
281	output_update();	291	output_update();
282	}	292	}
283		293
284	#ifdef USE_NAVICTRL	294	#ifdef USE_NAVICTRL
285	if(!SendSPI) {	295	if(!SendSPI) {
286	// SendSPI is decremented in timer0.c with a rate of 9.765 kHz.	296	// 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.	297	// 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,	298	// 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.	299	// 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();	300	SPI_TransmitByte();
291	}	301	}
292	#endif	302	#endif
293	}	303	}
294	return (1);	304	return (1);
295	}	305	}
296		306

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/main.c @ 1845 - Rev 1645 → 1775