WebSVN - FlightCtrl - Diff - Rev 2135 and 2139 - /branches/dongfang_FC_fixedwing/arduino


#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
 
        // Set up LED output. This is here and not in configuration.c because the setBoardRelease function where it used to reside no longer exists.
        DDRB |= ((1<<DDB4)|(1<<DDB5));
        PORTB &= ~((1<<DDB4)|(1<<DDB5));
 
        // disable interrupts global
        cli();
 
        // wdt_enable(WDTO_2000MS);
        wdt_disable();
 
        // initalize modules
        output_init();
        timer0_init();
        timer2_init();
        usart0_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();
 
        // 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 (sensorDataReady != ALL_DATA_READY) {
                                // Analog data should have been ready but is not!!
                                debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER;
                        } else {
                                debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
                        }
                        PD2HIGH;
                        // 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.
                        calculateFlightAttitude();
                        controlMixer_periodicTask();
                        commands_handleCommands();
                        flight_control();
                        PD2LOW;
 
                        // 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 2135	Rev 2139
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	// Set up LED output. This is here and not in configuration.c because the setBoardRelease function where it used to reside no longer exists.	37	// Set up LED output. This is here and not in configuration.c because the setBoardRelease function where it used to reside no longer exists.
38	DDRB \|= ((1<<DDB4)\|(1<<DDB5));	38	DDRB \|= ((1<<DDB4)\|(1<<DDB5));
39	PORTB &= ~((1<<DDB4)\|(1<<DDB5));	39	PORTB &= ~((1<<DDB4)\|(1<<DDB5));
40		40
41	// disable interrupts global	41	// disable interrupts global
42	cli();	42	cli();
43		43
44	// wdt_enable(WDTO_2000MS);	44	// wdt_enable(WDTO_2000MS);
45	wdt_disable();	45	wdt_disable();
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	RC_Init();	52	RC_Init();
53	analog_init();	53	analog_init();
54		54
55	// Parameter Set handling	55	// Parameter Set handling
56	IMUConfig_readOrDefault();	56	IMUConfig_readOrDefault();
57	channelMap_readOrDefault();	57	channelMap_readOrDefault();
58	rcTrim_readOrDefault();	58	rcTrim_readOrDefault();
59	paramSet_readOrDefault();	59	paramSet_readOrDefault();
60		60
61	// enable interrupts global	61	// enable interrupts global
62	sei();	62	sei();
63		63
64	controlMixer_setNeutral();	64	controlMixer_setNeutral();
65		65
66	// Cal. attitude sensors and reset integrals.	66	// Cal. attitude sensors and reset integrals.
67	attitude_setNeutral();	67	attitude_setNeutral();
68		68
69	// This is not a busy-wait operation and should be OK.	69	// This is not a busy-wait operation and should be OK.
70	beep(2000);	70	beep(2000);
71		71
72	while (1) {	72	while (1) {
73	if (runFlightControl) { // control interval	73	if (runFlightControl) { // control interval
74	runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0	74	runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
75		75
76	/*	76	/*
77	if (!resetFlag) {	77	if (!resetFlag) {
78	wdt_reset();	78	wdt_reset();
79	}	79	}
80	*/	80	*/
81		81
82	if (sensorDataReady != ALL_DATA_READY) {	82	if (sensorDataReady != ALL_DATA_READY) {
83	// Analog data should have been ready but is not!!	83	// Analog data should have been ready but is not!!
84	debugOut.digital[0] \|= DEBUG_MAINLOOP_TIMER;	84	debugOut.digital[0] \|= DEBUG_MAINLOOP_TIMER;
85	} else {	85	} else {
86	debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;	86	debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
87	}	87	}
88	PD2HIGH;	88	PD2HIGH;
89	// This is probably the correct order:	89	// This is probably the correct order:
90	// The attitude computation should not depend on anything from control (except maybe the estimation of control activity level)	90	// The attitude computation should not depend on anything from control (except maybe the estimation of control activity level)
91	// The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc.	91	// The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc.
92	// Flight control uses results from both.	92	// Flight control uses results from both.
93	calculateFlightAttitude();	93	calculateFlightAttitude();
94	controlMixer_periodicTask();	94	controlMixer_periodicTask();
95	commands_handleCommands();	95	commands_handleCommands();
96	flight_control();	96	flight_control();
97	PD2LOW;	97	PD2LOW;
98		98
99	// Allow Serial Data Transmit if motors must not updated or motors are not running	99	// Allow Serial Data Transmit if motors must not updated or motors are not running
100	if (!runFlightControl \|\| !isFlying) {	100	if (!runFlightControl \|\| !isFlying) {
101	usart0_transmitTxData();	101	usart0_transmitTxData();
102	}	102	}
103		103
104	usart0_processRxData();	104	usart0_processRxData();
105		105
106	static uint8_t aboveWarningLimitVoltageSeen = 0;	106	static uint8_t aboveWarningLimitVoltageSeen = 0;
107		107
108	if (checkDelay(timer)) {	108	if (checkDelay(timer)) {
109	if (UBat >= staticParams.batteryWarningVoltage) {	109	if (UBat >= staticParams.batteryWarningVoltage) {
110	aboveWarningLimitVoltageSeen = 1;	110	aboveWarningLimitVoltageSeen = 1;
111	} else { // If we are above USB voltage, or if we have once been above warning voltage	111	} else { // If we are above USB voltage, or if we have once been above warning voltage
112	if (aboveWarningLimitVoltageSeen \|\| UBat > UBAT_AT_5V) {	112	if (aboveWarningLimitVoltageSeen \|\| UBat > UBAT_AT_5V) {
113	beepBatteryAlarm();	113	beepBatteryAlarm();
114	}	114	}
115	}	115	}
116	calculateFeaturedServoValues();	116	//calculateFeaturedServoValues();
117	timer = setDelay(20); // every 20 ms	117	timer = setDelay(20); // every 20 ms
118	}	118	}
119		119
120	output_update();	120	output_update();
121		121
122	if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error.	122	if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error.
123	debugOut.digital[1] \|= DEBUG_MAINLOOP_TIMER;	123	debugOut.digital[1] \|= DEBUG_MAINLOOP_TIMER;
124	} else {	124	} else {
125	debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;	125	debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;
126	}	126	}
127	}	127	}
128	}	128	}
129	return (1);	129	return (1);
130	}	130	}
131		131

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/arduino_atmega328/main.c @ 2339 - Rev 2135 → 2139