WebSVN - FlightCtrl - Diff - Rev 2110 and 2119 - /branches/dongfang_FC


#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 "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"
 
int16_t main(void) {
  uint16_t timer;
 
  // disable interrupts global
  cli();
 
  // analyze hardware environment
  setCPUType();
  setBoardRelease();
 
  // disable watchdog
  MCUSR &= ~(1 << WDRF);
  WDTCSR |= (1 << WDCE) | (1 << WDE);
  WDTCSR = 0;
 
// This is strange: It should NOT be necessarty to do. But the call of the same,
// in channelMap_readOrDefault (if eeprom read fails) just sets all to 0,0,0,....
  channelMap_default();
 
  // 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();
  paramSet_readOrDefault();
 
  // enable interrupts global
  sei();
 
  printf("\n\r===================================");
  printf("\n\rFlightControl");
  printf("\n\rHardware: Custom");
  printf("\n\r     CPU: Atmega644");
  if (CPUType == ATMEGA644P)
    printf("p");
  printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a');
  printf("\n\r===================================");
 
  // 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);
  outputSet(0,1);
  GRN_OFF;
  RED_ON;
  while (!checkDelay(timer))
    ;
 
  timer = setDelay(200);
  outputSet(0, 0);
  outputSet(1, 1);
  RED_OFF;
  GRN_ON;
  while (!checkDelay(timer))
    ;
 
  timer = setDelay(200);
  while (!checkDelay(timer))
    ;
  outputSet(1,0);
  GRN_OFF;
 
    printf("\n\r===================================");
 
#ifdef USE_NAVICTRL
  printf("\n\rSupport for NaviCtrl");
#endif
 
#ifdef USE_DIRECT_GPS
  printf("\n\rDirect (no NaviCtrl) navigation");
#endif
 
  controlMixer_setNeutral();
 
  // Cal. attitude sensors and reset integrals.
  attitude_setNeutral();
 
  // Init flight parameters
  // flight_setNeutral();
 
  beep(2000);
 
  printf("\n\n\r");
 
      while (1) {
    if (runFlightControl) { // control interval
      runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
      if (!analogDataReady) {
        // Analog data should have been ready but is not!!
        debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER;
      } else {
        debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
          }
        J4HIGH;
        // 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();
        J4LOW;
       
        // Allow Serial Data Transmit if motors must not updated or motors are not running
        if (!runFlightControl || !isFlying) {
          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.batteryWarningVoltage) {
            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
          calculateFeaturedServoValues();
 
          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 2110	Rev 2119
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 <util/delay.h>	4	#include <util/delay.h>
5		5
6	#include "timer0.h"	6	#include "timer0.h"
7	#include "timer2.h"	7	#include "timer2.h"
8	#include "uart0.h"	8	#include "uart0.h"
9	#include "output.h"	9	#include "output.h"
10	#include "attitude.h"	10	#include "attitude.h"
11	#include "commands.h"	11	#include "commands.h"
12	#include "flight.h"	12	#include "flight.h"
13	#include "rc.h"	13	#include "rc.h"
14	#include "analog.h"	14	#include "analog.h"
15	#include "configuration.h"	15	#include "configuration.h"
16	#include "controlMixer.h"	16	#include "controlMixer.h"
17	#include "eeprom.h"	17	#include "eeprom.h"
18	#include "printf_P.h"	18	#include "printf_P.h"
19		19
20	int16_t main(void) {	20	int16_t main(void) {
21	uint16_t timer;	21	uint16_t timer;
22		22
23	// disable interrupts global	23	// disable interrupts global
24	cli();	24	cli();
25		25
26	// analyze hardware environment	26	// analyze hardware environment
27	setCPUType();	27	setCPUType();
28	setBoardRelease();	28	setBoardRelease();
29		29
30	// disable watchdog	30	// disable watchdog
31	MCUSR &= ~(1 << WDRF);	31	MCUSR &= ~(1 << WDRF);
32	WDTCSR \|= (1 << WDCE) \| (1 << WDE);	32	WDTCSR \|= (1 << WDCE) \| (1 << WDE);
33	WDTCSR = 0;	33	WDTCSR = 0;
34		34
35	// This is strange: It should NOT be necessarty to do. But the call of the same,	35	// This is strange: It should NOT be necessarty to do. But the call of the same,
36	// in channelMap_readOrDefault (if eeprom read fails) just sets all to 0,0,0,....	36	// in channelMap_readOrDefault (if eeprom read fails) just sets all to 0,0,0,....
37	channelMap_default();	37	channelMap_default();
38		38
39	// initalize modules	39	// initalize modules
40	output_init();	40	output_init();
41	timer0_init();	41	timer0_init();
42	timer2_init();	42	timer2_init();
43	usart0_init();	43	usart0_init();
44	//if (CPUType == ATMEGA644P);// usart1_Init();	44	//if (CPUType == ATMEGA644P);// usart1_Init();
45	RC_Init();	45	RC_Init();
46	analog_init();	46	analog_init();
47		47
48	// Parameter Set handling	48	// Parameter Set handling
49	IMUConfig_readOrDefault();	49	IMUConfig_readOrDefault();
50	channelMap_readOrDefault();	50	channelMap_readOrDefault();
51	paramSet_readOrDefault();	51	paramSet_readOrDefault();
52		52
53	// enable interrupts global	53	// enable interrupts global
54	sei();	54	sei();
55		55
56	printf("\n\r===================================");	56	printf("\n\r===================================");
57	printf("\n\rFlightControl");	57	printf("\n\rFlightControl");
58	printf("\n\rHardware: Custom");	58	printf("\n\rHardware: Custom");
59	printf("\n\r CPU: Atmega644");	59	printf("\n\r CPU: Atmega644");
60	if (CPUType == ATMEGA644P)	60	if (CPUType == ATMEGA644P)
61	printf("p");	61	printf("p");
62	printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a');	62	printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a');
63	printf("\n\r===================================");	63	printf("\n\r===================================");
64		64
65	// Wait for a short time (otherwise the RC channel check won't work below)	65	// Wait for a short time (otherwise the RC channel check won't work below)
66	// timer = SetDelay(500);	66	// timer = SetDelay(500);
67	// while(!CheckDelay(timer));	67	// while(!CheckDelay(timer));
68		68
69	// Instead, while away the time by flashing the 2 outputs:	69	// Instead, while away the time by flashing the 2 outputs:
70	// First J16, then J17. Makes it easier to see which is which.	70	// First J16, then J17. Makes it easier to see which is which.
71	timer = setDelay(200);	71	timer = setDelay(200);
72	outputSet(0,1);	72	outputSet(0,1);
73	GRN_OFF;	73	GRN_OFF;
74	RED_ON;	74	RED_ON;
75	while (!checkDelay(timer))	75	while (!checkDelay(timer))
76	;	76	;
77		77
78	timer = setDelay(200);	78	timer = setDelay(200);
79	outputSet(0, 0);	79	outputSet(0, 0);
80	outputSet(1, 1);	80	outputSet(1, 1);
81	RED_OFF;	81	RED_OFF;
82	GRN_ON;	82	GRN_ON;
83	while (!checkDelay(timer))	83	while (!checkDelay(timer))
84	;	84	;
85		85
86	timer = setDelay(200);	86	timer = setDelay(200);
87	while (!checkDelay(timer))	87	while (!checkDelay(timer))
88	;	88	;
89	outputSet(1,0);	89	outputSet(1,0);
90	GRN_OFF;	90	GRN_OFF;
91		91
92	printf("\n\r===================================");	92	printf("\n\r===================================");
93		93
94	#ifdef USE_NAVICTRL	94	#ifdef USE_NAVICTRL
95	printf("\n\rSupport for NaviCtrl");	95	printf("\n\rSupport for NaviCtrl");
96	#endif	96	#endif
97		97
98	#ifdef USE_DIRECT_GPS	98	#ifdef USE_DIRECT_GPS
99	printf("\n\rDirect (no NaviCtrl) navigation");	99	printf("\n\rDirect (no NaviCtrl) navigation");
100	#endif	100	#endif
101		101
102	controlMixer_setNeutral();	102	controlMixer_setNeutral();
103		103
104	// Cal. attitude sensors and reset integrals.	104	// Cal. attitude sensors and reset integrals.
105	attitude_setNeutral();	105	attitude_setNeutral();
106		106
107	// Init flight parameters	107	// Init flight parameters
108	// flight_setNeutral();	108	// flight_setNeutral();
109		109
110	beep(2000);	110	beep(2000);
111		111
112	printf("\n\n\r");	112	printf("\n\n\r");
113		113
114	while (1) {	114	while (1) {
115	if (runFlightControl) { // control interval	115	if (runFlightControl) { // control interval
116	runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0	116	runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
117	if (!analogDataReady) {	117	if (!analogDataReady) {
118	// Analog data should have been ready but is not!!	118	// Analog data should have been ready but is not!!
119	debugOut.digital[0] \|= DEBUG_MAINLOOP_TIMER;	119	debugOut.digital[0] \|= DEBUG_MAINLOOP_TIMER;
120	} else {	120	} else {
121	debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;	121	debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
122	}	122	}
123	J4HIGH;	123	J4HIGH;
124	// This is probably the correct order:	124	// This is probably the correct order:
125	// The attitude computation should not depend on anything from control (except maybe the estimation of control activity level)	125	// The attitude computation should not depend on anything from control (except maybe the estimation of control activity level)
126	// The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc.	126	// The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc.
127	// Flight control uses results from both.	127	// Flight control uses results from both.
128	calculateFlightAttitude();	128	calculateFlightAttitude();
129	controlMixer_periodicTask();	129	controlMixer_periodicTask();
130	commands_handleCommands();	130	commands_handleCommands();
131	flight_control();	131	flight_control();
132	J4LOW;	132	J4LOW;
133		133
134	// Allow Serial Data Transmit if motors must not updated or motors are not running	134	// Allow Serial Data Transmit if motors must not updated or motors are not running
135	if (!runFlightControl \|\| !isFlying) {	135	if (!runFlightControl \|\| !isFlying) {
136	usart0_transmitTxData();	136	usart0_transmitTxData();
137	}	137	}
138		138
139	usart0_processRxData();	139	usart0_processRxData();
140		140
141	if (checkDelay(timer)) {	141	if (checkDelay(timer)) {
142	if (UBat <= UBAT_AT_5V) {	142	if (UBat <= UBAT_AT_5V) {
143	// Do nothing. The voltage on the input side of the regulator is <5V;	143	// Do nothing. The voltage on the input side of the regulator is <5V;
144	// we must be running off USB power. Keep it quiet.	144	// we must be running off USB power. Keep it quiet.
145	} else if (UBat < staticParams.batteryVoltageWarning) {	145	} else if (UBat < staticParams.batteryWarningVoltage) {
146	beepBatteryAlarm();	146	beepBatteryAlarm();
147	}	147	}
148		148
149	#ifdef USE_NAVICTRL	149	#ifdef USE_NAVICTRL
150	SPI_StartTransmitPacket();	150	SPI_StartTransmitPacket();
151	SendSPI = 4;	151	SendSPI = 4;
152	#endif	152	#endif
153	timer = setDelay(20); // every 20 ms	153	timer = setDelay(20); // every 20 ms
154	}	154	}
155	output_update();	155	output_update();
156	}	156	}
157		157
158	#ifdef USE_NAVICTRL	158	#ifdef USE_NAVICTRL
159	if(!SendSPI) {	159	if(!SendSPI) {
160	// SendSPI is decremented in timer0.c with a rate of 9.765 kHz.	160	// SendSPI is decremented in timer0.c with a rate of 9.765 kHz.
161	// within the SPI_TransmitByte() routine the value is set to 4.	161	// within the SPI_TransmitByte() routine the value is set to 4.
162	// I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz,	162	// I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz,
163	// and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms.	163	// and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms.
164	SPI_TransmitByte();	164	SPI_TransmitByte();
165	}	165	}
166	#endif	166	#endif
167	calculateFeaturedServoValues();	167	calculateFeaturedServoValues();
168		168
169	if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error.	169	if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error.
170	debugOut.digital[1] \|= DEBUG_MAINLOOP_TIMER;	170	debugOut.digital[1] \|= DEBUG_MAINLOOP_TIMER;
171	} else {	171	} else {
172	debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;	172	debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;
173	}	173	}
174	}	174	}
175	return (1);	175	return (1);
176	}	176	}
177		177

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/main.c - Rev 2110 → 2119