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

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/main.c @ 1960 - Rev 1821 → 1849