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

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/main.c @ 1630 - Rev 2112 → 2158