WebSVN - FlightCtrl - Diff - Rev 2137 and 2142 - /branches/dongfang_FC


#include <avr/boot.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/wdt.h>
#include <util/delay.h>
 
#include "timer0.h"
#include "timer2.h"
#include "uart0.h"
#include "output.h"
#include "attitude.h"
#include "commands.h"
#include "flight.h"
#include "rc.h"
#include "analog.h"
#include "configuration.h"
#include "controlMixer.h"
#include "eeprom.h"
#include "printf_P.h"
 
uint8_t resetFlag = 0;
 
void reset(void) {
        resetFlag = 1;
        wdt_enable(WDTO_15MS);
        while (1)
                ;
}
 
int16_t main(void) {
        uint16_t timer = 0;
 
#ifdef DO_PROFILE
        static uint8_t profileTimer;
#endif
 
        // disable interrupts global
        cli();
 
        // wdt_enable(WDTO_120MS);
        wdt_disable();
 
        // analyze hardware environment
        setCPUType();
        setBoardRelease();
 
        // initalize modules
        output_init();
        timer0_init();
        timer2_init();
        usart0_init();
        //if (CPUType == ATMEGA644P);// usart1_Init();
        RC_Init();
        analog_init();
 
        // Parameter Set handling
        IMUConfig_readOrDefault();
        channelMap_readOrDefault();
        rcTrim_readOrDefault();
        paramSet_readOrDefault();
 
        // enable interrupts global
        sei();
 
        controlMixer_setNeutral();
 
        // Cal. attitude sensors and reset integrals.
        attitude_setNeutral();
 
        // Init flight parameters
        // flight_setNeutral();
 
        // This is not a busy-wait operation and should be OK.
        beep(2000);
 
        while (1) {
                if (runFlightControl) { // control interval
                        runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
 
                        if (!resetFlag) {
                                wdt_reset();
                        }
 
                        if (!analogDataReady) {
                                // Analog data should have been ready but is not!!
                                debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER;
                        } else {
                                debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
                        }
                        // This is probably the correct order:
                        // The attitude computation should not depend on anything from control (except maybe the estimation of control activity level)
                        // The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc.
                        // Flight control uses results from both.
#ifdef DO_PROFILE
                        startProfileTimer(ATTITUDE);
#endif
                        calculateFlightAttitude();
#ifdef DO_PROFILE
                        stopProfileTimer(ATTITUDE);
#endif
 
#ifdef DO_PROFILE
                        startProfileTimer(CONTROLMIXER);
#endif
                        controlMixer_periodicTask();
#ifdef DO_PROFILE
                        stopProfileTimer(CONTROLMIXER);
#endif
 
#ifdef DO_PROFILE
                        startProfileTimer(COMMANDS);
#endif
                        commands_handleCommands();
#ifdef DO_PROFILE
                        stopProfileTimer(COMMANDS);
#endif
 
#ifdef DO_PROFILE
                        startProfileTimer(FLIGHT);
#endif
                        flight_control();
#ifdef DO_PROFILE
                        stopProfileTimer(FLIGHT);
#endif
 
                        // Allow Serial Data Transmit if motors must not updated or motors are not running
                        if (!runFlightControl || !isFlying) {
                                usart0_transmitTxData();
                        }
 
                        usart0_processRxData();
 
                        static uint8_t aboveWarningLimitVoltageSeen = 0;
 
                        if (checkDelay(timer)) {
                                if (UBat >= staticParams.batteryWarningVoltage) {
                                        aboveWarningLimitVoltageSeen = 1;
                                } else { // If we are above USB voltage, or if we have once been above warning voltage
                                        if (aboveWarningLimitVoltageSeen || UBat > UBAT_AT_5V) {
                                                beepBatteryAlarm();
                                        }
                                }
                                //calculateFeaturedServoValues();
                                timer = setDelay(20); // every 20 ms
                        }
 
                        output_update();
 
                        if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error.
                                debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER;
                        } else {
                                debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;
                        }
                }
        }
        return (1);
}
 

Rev 2137	Rev 2142
1	#include <avr/boot.h>	1	#include <avr/boot.h>
2	#include <avr/io.h>	2	#include <avr/io.h>
3	#include <avr/interrupt.h>	3	#include <avr/interrupt.h>
4	#include <avr/wdt.h>	4	#include <avr/wdt.h>
5	#include <util/delay.h>	5	#include <util/delay.h>
6		6
7	#include "timer0.h"	7	#include "timer0.h"
8	#include "timer2.h"	8	#include "timer2.h"
9	#include "uart0.h"	9	#include "uart0.h"
10	#include "output.h"	10	#include "output.h"
11	#include "attitude.h"	11	#include "attitude.h"
12	#include "commands.h"	12	#include "commands.h"
13	#include "flight.h"	13	#include "flight.h"
14	#include "rc.h"	14	#include "rc.h"
15	#include "analog.h"	15	#include "analog.h"
16	#include "configuration.h"	16	#include "configuration.h"
17	#include "controlMixer.h"	17	#include "controlMixer.h"
18	#include "eeprom.h"	18	#include "eeprom.h"
19	#include "printf_P.h"	19	#include "printf_P.h"
20		20
21	uint8_t resetFlag = 0;	21	uint8_t resetFlag = 0;
22		22
23	void reset(void) {	23	void reset(void) {
24	resetFlag = 1;	24	resetFlag = 1;
25	wdt_enable(WDTO_15MS);	25	wdt_enable(WDTO_15MS);
26	while (1)	26	while (1)
27	;	27	;
28	}	28	}
29		29
30	int16_t main(void) {	30	int16_t main(void) {
31	uint16_t timer = 0;	31	uint16_t timer = 0;
32		32
33	#ifdef DO_PROFILE	33	#ifdef DO_PROFILE
34	static uint8_t profileTimer;	34	static uint8_t profileTimer;
35	#endif	35	#endif
36		36
37	// disable interrupts global	37	// disable interrupts global
38	cli();	38	cli();
39		39
40	// wdt_enable(WDTO_120MS);	40	// wdt_enable(WDTO_120MS);
41	wdt_disable();	41	wdt_disable();
42		42
43	// analyze hardware environment	43	// analyze hardware environment
44	setCPUType();	44	setCPUType();
45	setBoardRelease();	45	setBoardRelease();
46		46
47	// initalize modules	47	// initalize modules
48	output_init();	48	output_init();
49	timer0_init();	49	timer0_init();
50	timer2_init();	50	timer2_init();
51	usart0_init();	51	usart0_init();
52	//if (CPUType == ATMEGA644P);// usart1_Init();	52	//if (CPUType == ATMEGA644P);// usart1_Init();
53	RC_Init();	53	RC_Init();
54	analog_init();	54	analog_init();
55		55
56	// Parameter Set handling	56	// Parameter Set handling
57	IMUConfig_readOrDefault();	57	IMUConfig_readOrDefault();
58	channelMap_readOrDefault();	58	channelMap_readOrDefault();
59	rcTrim_readOrDefault();	59	rcTrim_readOrDefault();
60	paramSet_readOrDefault();	60	paramSet_readOrDefault();
61		61
62	// enable interrupts global	62	// enable interrupts global
63	sei();	63	sei();
64		64
65	controlMixer_setNeutral();	65	controlMixer_setNeutral();
66		66
67	// Cal. attitude sensors and reset integrals.	67	// Cal. attitude sensors and reset integrals.
68	attitude_setNeutral();	68	attitude_setNeutral();
69		69
70	// Init flight parameters	70	// Init flight parameters
71	// flight_setNeutral();	71	// flight_setNeutral();
72		72
73	// This is not a busy-wait operation and should be OK.	73	// This is not a busy-wait operation and should be OK.
74	beep(2000);	74	beep(2000);
75		75
76	while (1) {	76	while (1) {
77	if (runFlightControl) { // control interval	77	if (runFlightControl) { // control interval
78	runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0	78	runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
79		79
80	if (!resetFlag) {	80	if (!resetFlag) {
81	wdt_reset();	81	wdt_reset();
82	}	82	}
83		83
84	if (!analogDataReady) {	84	if (!analogDataReady) {
85	// Analog data should have been ready but is not!!	85	// Analog data should have been ready but is not!!
86	debugOut.digital[0] \|= DEBUG_MAINLOOP_TIMER;	86	debugOut.digital[0] \|= DEBUG_MAINLOOP_TIMER;
87	} else {	87	} else {
88	debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;	88	debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
89	}	89	}
90	// This is probably the correct order:	90	// This is probably the correct order:
91	// The attitude computation should not depend on anything from control (except maybe the estimation of control activity level)	91	// The attitude computation should not depend on anything from control (except maybe the estimation of control activity level)
92	// The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc.	92	// The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc.
93	// Flight control uses results from both.	93	// Flight control uses results from both.
94	#ifdef DO_PROFILE	94	#ifdef DO_PROFILE
95	startProfileTimer(ATTITUDE);	95	startProfileTimer(ATTITUDE);
96	#endif	96	#endif
97	calculateFlightAttitude();	97	calculateFlightAttitude();
98	#ifdef DO_PROFILE	98	#ifdef DO_PROFILE
99	stopProfileTimer(ATTITUDE);	99	stopProfileTimer(ATTITUDE);
100	#endif	100	#endif
101		101
102	#ifdef DO_PROFILE	102	#ifdef DO_PROFILE
103	startProfileTimer(CONTROLMIXER);	103	startProfileTimer(CONTROLMIXER);
104	#endif	104	#endif
105	controlMixer_periodicTask();	105	controlMixer_periodicTask();
106	#ifdef DO_PROFILE	106	#ifdef DO_PROFILE
107	stopProfileTimer(CONTROLMIXER);	107	stopProfileTimer(CONTROLMIXER);
108	#endif	108	#endif
109		109
110	#ifdef DO_PROFILE	110	#ifdef DO_PROFILE
111	startProfileTimer(COMMANDS);	111	startProfileTimer(COMMANDS);
112	#endif	112	#endif
113	commands_handleCommands();	113	commands_handleCommands();
114	#ifdef DO_PROFILE	114	#ifdef DO_PROFILE
115	stopProfileTimer(COMMANDS);	115	stopProfileTimer(COMMANDS);
116	#endif	116	#endif
117		117
118	#ifdef DO_PROFILE	118	#ifdef DO_PROFILE
119	startProfileTimer(FLIGHT);	119	startProfileTimer(FLIGHT);
120	#endif	120	#endif
121	flight_control();	121	flight_control();
122	#ifdef DO_PROFILE	122	#ifdef DO_PROFILE
123	stopProfileTimer(FLIGHT);	123	stopProfileTimer(FLIGHT);
124	#endif	124	#endif
125		125
126	// Allow Serial Data Transmit if motors must not updated or motors are not running	126	// Allow Serial Data Transmit if motors must not updated or motors are not running
127	if (!runFlightControl \|\| !isFlying) {	127	if (!runFlightControl \|\| !isFlying) {
128	usart0_transmitTxData();	128	usart0_transmitTxData();
129	}	129	}
130		130
131	usart0_processRxData();	131	usart0_processRxData();
132		132
133	static uint8_t aboveWarningLimitVoltageSeen = 0;	133	static uint8_t aboveWarningLimitVoltageSeen = 0;
134		134
135	if (checkDelay(timer)) {	135	if (checkDelay(timer)) {
136	if (UBat >= staticParams.batteryWarningVoltage) {	136	if (UBat >= staticParams.batteryWarningVoltage) {
137	aboveWarningLimitVoltageSeen = 1;	137	aboveWarningLimitVoltageSeen = 1;
138	} else { // If we are above USB voltage, or if we have once been above warning voltage	138	} else { // If we are above USB voltage, or if we have once been above warning voltage
139	if (aboveWarningLimitVoltageSeen \|\| UBat > UBAT_AT_5V) {	139	if (aboveWarningLimitVoltageSeen \|\| UBat > UBAT_AT_5V) {
140	beepBatteryAlarm();	140	beepBatteryAlarm();
141	}	141	}
142	}	142	}
143	calculateFeaturedServoValues();	143	//calculateFeaturedServoValues();
144	timer = setDelay(20); // every 20 ms	144	timer = setDelay(20); // every 20 ms
145	}	145	}
146		146
147	output_update();	147	output_update();
148		148
149	if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error.	149	if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error.
150	debugOut.digital[1] \|= DEBUG_MAINLOOP_TIMER;	150	debugOut.digital[1] \|= DEBUG_MAINLOOP_TIMER;
151	} else {	151	} else {
152	debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;	152	debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;
153	}	153	}
154	}	154	}
155	}	155	}
156	return (1);	156	return (1);
157	}	157	}
158		158

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/main.c - Rev 2137 → 2142