WebSVN - FlightCtrl - Diff - Rev 2125 and 2132 - /branches/dongfang_FC


#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/wdt.h>
#include "eeprom.h"
#include "analog.h"
#include "controlMixer.h"
 
#include "timer0.h"
#include "output.h"
 
#ifdef DO_PROFILE
volatile uint32_t global10kHzClock = 0;
volatile int32_t profileTimers[NUM_PROFILE_TIMERS];
volatile int32_t runningProfileTimers[NUM_PROFILE_TIMERS];
#endif
 
volatile uint32_t globalMillisClock = 0;
volatile uint8_t  runFlightControl = 0;
volatile uint16_t beepTime = 0;
volatile uint16_t beepModulation = BEEP_MODULATION_NONE;
 
/*****************************************************
 * Initialize Timer 0                  
 *****************************************************/
// timer 0 is used for the PWM generation to control the offset voltage at the air pressure sensor
// Its overflow interrupt routine is used to generate the beep signal and the flight control motor update rate
void timer0_init(void) {
  uint8_t sreg = SREG;
 
  // disable all interrupts before reconfiguration
  cli();
 
  // Configure speaker port as output.
  if (boardRelease == 10) { // Speaker at PD2
    DDRD |= (1 << DDD2);
    PORTD &= ~(1 << PORTD2);
  } else { // Speaker at PC7
    DDRC |= (1 << DDC7);
    PORTC &= ~(1 << PORTC7);
  }
 
  // set PB3 and PB4 as output for the PWM used as offset for the pressure sensor
  DDRB |= (1 << DDB4) | (1 << DDB3);
  PORTB &= ~((1 << PORTB4) | (1 << PORTB3));
 
  // Timer/Counter 0 Control Register A
 
  // Waveform Generation Mode is Fast PWM (Bits WGM02 = 0, WGM01 = 1, WGM00 = 1)
  // Clear OC0A on Compare Match, set OC0A at BOTTOM, noninverting PWM (Bits COM0A1 = 1, COM0A0 = 0)
  // Clear OC0B on Compare Match, set OC0B at BOTTOM, (Bits COM0B1 = 1, COM0B0 = 0)
  TCCR0A &= ~((1 << COM0A0) | (1 << COM0B0));
  TCCR0A |= (1 << COM0A1) | (1 << COM0B1) | (1 << WGM01) | (1 << WGM00);
 
  // Timer/Counter 0 Control Register B
  // set clock divider for timer 0 to SYSCLOCK/8 = 20MHz/8 = 2.5MHz
  // i.e. the timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz
  // hence the timer overflow interrupt frequency is 2.5 MHz/256 = 9.765 kHz
 
  // divider 8 (Bits CS02 = 0, CS01 = 1, CS00 = 0)
  TCCR0B &= ~((1 << FOC0A) | (1 << FOC0B) | (1 << WGM02));
  TCCR0B = (TCCR0B & 0xF8) | (0 << CS02) | (1 << CS01) | (0 << CS00);
 
  // initialize the Output Compare Register A & B used for PWM generation on port PB3 & PB4
  OCR0A = 0; // for PB3
  OCR0B = 120; // for PB4
 
  // init Timer/Counter 0 Register
  TCNT0 = 0;
 
  // Timer/Counter 0 Interrupt Mask Register
  // enable timer overflow interrupt only
  TIMSK0 &= ~((1 << OCIE0B) | (1 << OCIE0A));
  TIMSK0 |= (1 << TOIE0);
 
#ifdef DO_PROFILE
  for (uint8_t i=0; i<NUM_PROFILE_TIMERS; i++) {
          profileTimers[i] = 0;
  }
#endif
 
  SREG = sreg;
}
 
/*****************************************************/
/*          Interrupt Routine of Timer 0             */
/*****************************************************/
ISR(TIMER0_OVF_vect) { // 9765.625 Hz
  static uint8_t cnt_1ms = 1, cnt = 0;
  uint8_t beeperOn = 0;
 
#ifdef DO_PROFILE
    global10kHzClock++;
#endif
 
if (!cnt--) { // every 10th run (9.765625kHz/10 = 976.5625Hz)
    cnt = 9;
    cnt_1ms ^= 1;
    if (!cnt_1ms) {
      runFlightControl = 1; // every 2nd run (976.5625 Hz/2 = 488.28125 Hz)
    }
    globalMillisClock++; // increment millisecond counter
  }
 
  // beeper on if duration is not over
  if (beepTime) {
    beepTime--; // decrement BeepTime
    if (beepTime & beepModulation)
      beeperOn = 1;
    else
      beeperOn = 0;
  } else { // beeper off if duration is over
    beeperOn = 0;
    beepModulation = BEEP_MODULATION_NONE;
  }
 
  if (beeperOn) {
    // set speaker port to high.
    if (boardRelease == 10)
      PORTD |= (1 << PORTD2); // Speaker at PD2
    else
      PORTC |= (1 << PORTC7); // Speaker at PC7
  } else { // beeper is off
    // set speaker port to low
    if (boardRelease == 10)
      PORTD &= ~(1 << PORTD2);// Speaker at PD2
    else
      PORTC &= ~(1 << PORTC7);// Speaker at PC7
  }
 
#ifdef USE_MK3MAG
  // update compass value if this option is enabled in the settings
  if (staticParams.bitConfig & CFG_COMPASS_ENABLED) {
    MK3MAG_periodicTask(); // read out mk3mag pwm
  }
#endif
}
 
// -----------------------------------------------------------------------
uint16_t setDelay(uint16_t t) {
  return (globalMillisClock + t - 1);
}
 
// -----------------------------------------------------------------------
int8_t checkDelay(uint16_t t) {
  return (((t - globalMillisClock) & 0x8000) >> 8); // check sign bit
}
 
// -----------------------------------------------------------------------
void delay_ms(uint16_t w) {
  uint16_t t_stop = setDelay(w);
  while (!checkDelay(t_stop))
        wdt_reset();
}
 
// -----------------------------------------------------------------------
void delay_ms_with_adc_measurement(uint16_t w, uint8_t stop) {
  uint16_t t_stop;
  t_stop = setDelay(w);
  while (!checkDelay(t_stop)) {
        wdt_reset();
        if (analogDataReady) {
          analog_update();
          startAnalogConversionCycle();
        }
  }
  if (stop) {
  // Wait for new samples to get prepared but do not restart AD conversion after that!
  // Caller MUST to that.
         if (!analogDataReady) wdt_reset();
  }
}
 
#ifdef DO_PROFILE
void startProfileTimer(uint8_t timer) {
  runningProfileTimers[timer] = global10kHzClock++;
}
 
void stopProfileTimer(uint8_t timer) {
  int32_t t = global10kHzClock++ - runningProfileTimers[timer];
  profileTimers[timer] += t;
}
 
void debugProfileTimers(uint8_t index) {
  for (uint8_t i=0; i<NUM_PROFILE_TIMERS; i++) {
        uint16_t tenths = profileTimers[i] / 1000L;
        debugOut.analog[i+index] = tenths;
  }
  uint16_t tenths = global10kHzClock / 1000L;
  debugOut.analog[index + NUM_PROFILE_TIMERS] = tenths;
}
#endif;
 

Rev 2125	Rev 2132
1	#include <inttypes.h>	1	#include <inttypes.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 "eeprom.h"	5	#include "eeprom.h"
5	#include "analog.h"	6	#include "analog.h"
6	#include "controlMixer.h"	7	#include "controlMixer.h"
7		8
8	#include "timer0.h"	9	#include "timer0.h"
9	#include "output.h"	10	#include "output.h"
10		11
11	#ifdef DO_PROFILE	12	#ifdef DO_PROFILE
12	volatile uint32_t global10kHzClock = 0;	13	volatile uint32_t global10kHzClock = 0;
13	volatile int32_t profileTimers[NUM_PROFILE_TIMERS];	14	volatile int32_t profileTimers[NUM_PROFILE_TIMERS];
14	volatile int32_t runningProfileTimers[NUM_PROFILE_TIMERS];	15	volatile int32_t runningProfileTimers[NUM_PROFILE_TIMERS];
15	#endif	16	#endif
16		17
17	volatile uint32_t globalMillisClock = 0;	18	volatile uint32_t globalMillisClock = 0;
18	volatile uint8_t runFlightControl = 0;	19	volatile uint8_t runFlightControl = 0;
19	volatile uint16_t beepTime = 0;	20	volatile uint16_t beepTime = 0;
20	volatile uint16_t beepModulation = BEEP_MODULATION_NONE;	21	volatile uint16_t beepModulation = BEEP_MODULATION_NONE;
21		22
22	/*****************************************************	23	/*****************************************************
23	* Initialize Timer 0	24	* Initialize Timer 0
24	*****************************************************/	25	*****************************************************/
25	// timer 0 is used for the PWM generation to control the offset voltage at the air pressure sensor	26	// timer 0 is used for the PWM generation to control the offset voltage at the air pressure sensor
26	// Its overflow interrupt routine is used to generate the beep signal and the flight control motor update rate	27	// Its overflow interrupt routine is used to generate the beep signal and the flight control motor update rate
27	void timer0_init(void) {	28	void timer0_init(void) {
28	uint8_t sreg = SREG;	29	uint8_t sreg = SREG;
29		30
30	// disable all interrupts before reconfiguration	31	// disable all interrupts before reconfiguration
31	cli();	32	cli();
32		33
33	// Configure speaker port as output.	34	// Configure speaker port as output.
34	if (boardRelease == 10) { // Speaker at PD2	35	if (boardRelease == 10) { // Speaker at PD2
35	DDRD \|= (1 << DDD2);	36	DDRD \|= (1 << DDD2);
36	PORTD &= ~(1 << PORTD2);	37	PORTD &= ~(1 << PORTD2);
37	} else { // Speaker at PC7	38	} else { // Speaker at PC7
38	DDRC \|= (1 << DDC7);	39	DDRC \|= (1 << DDC7);
39	PORTC &= ~(1 << PORTC7);	40	PORTC &= ~(1 << PORTC7);
40	}	41	}
41		42
42	// set PB3 and PB4 as output for the PWM used as offset for the pressure sensor	43	// set PB3 and PB4 as output for the PWM used as offset for the pressure sensor
43	DDRB \|= (1 << DDB4) \| (1 << DDB3);	44	DDRB \|= (1 << DDB4) \| (1 << DDB3);
44	PORTB &= ~((1 << PORTB4) \| (1 << PORTB3));	45	PORTB &= ~((1 << PORTB4) \| (1 << PORTB3));
45		46
46	// Timer/Counter 0 Control Register A	47	// Timer/Counter 0 Control Register A
47		48
48	// Waveform Generation Mode is Fast PWM (Bits WGM02 = 0, WGM01 = 1, WGM00 = 1)	49	// Waveform Generation Mode is Fast PWM (Bits WGM02 = 0, WGM01 = 1, WGM00 = 1)
49	// Clear OC0A on Compare Match, set OC0A at BOTTOM, noninverting PWM (Bits COM0A1 = 1, COM0A0 = 0)	50	// Clear OC0A on Compare Match, set OC0A at BOTTOM, noninverting PWM (Bits COM0A1 = 1, COM0A0 = 0)
50	// Clear OC0B on Compare Match, set OC0B at BOTTOM, (Bits COM0B1 = 1, COM0B0 = 0)	51	// Clear OC0B on Compare Match, set OC0B at BOTTOM, (Bits COM0B1 = 1, COM0B0 = 0)
51	TCCR0A &= ~((1 << COM0A0) \| (1 << COM0B0));	52	TCCR0A &= ~((1 << COM0A0) \| (1 << COM0B0));
52	TCCR0A \|= (1 << COM0A1) \| (1 << COM0B1) \| (1 << WGM01) \| (1 << WGM00);	53	TCCR0A \|= (1 << COM0A1) \| (1 << COM0B1) \| (1 << WGM01) \| (1 << WGM00);
53		54
54	// Timer/Counter 0 Control Register B	55	// Timer/Counter 0 Control Register B
55	// set clock divider for timer 0 to SYSCLOCK/8 = 20MHz/8 = 2.5MHz	56	// set clock divider for timer 0 to SYSCLOCK/8 = 20MHz/8 = 2.5MHz
56	// i.e. the timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz	57	// i.e. the timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz
57	// hence the timer overflow interrupt frequency is 2.5 MHz/256 = 9.765 kHz	58	// hence the timer overflow interrupt frequency is 2.5 MHz/256 = 9.765 kHz
58		59
59	// divider 8 (Bits CS02 = 0, CS01 = 1, CS00 = 0)	60	// divider 8 (Bits CS02 = 0, CS01 = 1, CS00 = 0)
60	TCCR0B &= ~((1 << FOC0A) \| (1 << FOC0B) \| (1 << WGM02));	61	TCCR0B &= ~((1 << FOC0A) \| (1 << FOC0B) \| (1 << WGM02));
61	TCCR0B = (TCCR0B & 0xF8) \| (0 << CS02) \| (1 << CS01) \| (0 << CS00);	62	TCCR0B = (TCCR0B & 0xF8) \| (0 << CS02) \| (1 << CS01) \| (0 << CS00);
62		63
63	// initialize the Output Compare Register A & B used for PWM generation on port PB3 & PB4	64	// initialize the Output Compare Register A & B used for PWM generation on port PB3 & PB4
64	OCR0A = 0; // for PB3	65	OCR0A = 0; // for PB3
65	OCR0B = 120; // for PB4	66	OCR0B = 120; // for PB4
66		67
67	// init Timer/Counter 0 Register	68	// init Timer/Counter 0 Register
68	TCNT0 = 0;	69	TCNT0 = 0;
69		70
70	// Timer/Counter 0 Interrupt Mask Register	71	// Timer/Counter 0 Interrupt Mask Register
71	// enable timer overflow interrupt only	72	// enable timer overflow interrupt only
72	TIMSK0 &= ~((1 << OCIE0B) \| (1 << OCIE0A));	73	TIMSK0 &= ~((1 << OCIE0B) \| (1 << OCIE0A));
73	TIMSK0 \|= (1 << TOIE0);	74	TIMSK0 \|= (1 << TOIE0);
74		75
75	#ifdef DO_PROFILE	76	#ifdef DO_PROFILE
76	for (uint8_t i=0; i<NUM_PROFILE_TIMERS; i++) {	77	for (uint8_t i=0; i<NUM_PROFILE_TIMERS; i++) {
77	profileTimers[i] = 0;	78	profileTimers[i] = 0;
78	}	79	}
79	#endif	80	#endif
80		81
81	SREG = sreg;	82	SREG = sreg;
82	}	83	}
83		84
84	/*****************************************************/	85	/*****************************************************/
85	/* Interrupt Routine of Timer 0 */	86	/* Interrupt Routine of Timer 0 */
86	/*****************************************************/	87	/*****************************************************/
87	ISR(TIMER0_OVF_vect) { // 9765.625 Hz	88	ISR(TIMER0_OVF_vect) { // 9765.625 Hz
88	static uint8_t cnt_1ms = 1, cnt = 0;	89	static uint8_t cnt_1ms = 1, cnt = 0;
89	uint8_t beeperOn = 0;	90	uint8_t beeperOn = 0;
90		91
91	#ifdef DO_PROFILE	92	#ifdef DO_PROFILE
92	global10kHzClock++;	93	global10kHzClock++;
93	#endif	94	#endif
94		95
95	if (!cnt--) { // every 10th run (9.765625kHz/10 = 976.5625Hz)	96	if (!cnt--) { // every 10th run (9.765625kHz/10 = 976.5625Hz)
96	cnt = 9;	97	cnt = 9;
97	cnt_1ms ^= 1;	98	cnt_1ms ^= 1;
98	if (!cnt_1ms) {	99	if (!cnt_1ms) {
99	runFlightControl = 1; // every 2nd run (976.5625 Hz/2 = 488.28125 Hz)	100	runFlightControl = 1; // every 2nd run (976.5625 Hz/2 = 488.28125 Hz)
100	}	101	}
101	globalMillisClock++; // increment millisecond counter	102	globalMillisClock++; // increment millisecond counter
102	}	103	}
103		104
104	// beeper on if duration is not over	105	// beeper on if duration is not over
105	if (beepTime) {	106	if (beepTime) {
106	beepTime--; // decrement BeepTime	107	beepTime--; // decrement BeepTime
107	if (beepTime & beepModulation)	108	if (beepTime & beepModulation)
108	beeperOn = 1;	109	beeperOn = 1;
109	else	110	else
110	beeperOn = 0;	111	beeperOn = 0;
111	} else { // beeper off if duration is over	112	} else { // beeper off if duration is over
112	beeperOn = 0;	113	beeperOn = 0;
113	beepModulation = BEEP_MODULATION_NONE;	114	beepModulation = BEEP_MODULATION_NONE;
114	}	115	}
115		116
116	if (beeperOn) {	117	if (beeperOn) {
117	// set speaker port to high.	118	// set speaker port to high.
118	if (boardRelease == 10)	119	if (boardRelease == 10)
119	PORTD \|= (1 << PORTD2); // Speaker at PD2	120	PORTD \|= (1 << PORTD2); // Speaker at PD2
120	else	121	else
121	PORTC \|= (1 << PORTC7); // Speaker at PC7	122	PORTC \|= (1 << PORTC7); // Speaker at PC7
122	} else { // beeper is off	123	} else { // beeper is off
123	// set speaker port to low	124	// set speaker port to low
124	if (boardRelease == 10)	125	if (boardRelease == 10)
125	PORTD &= ~(1 << PORTD2);// Speaker at PD2	126	PORTD &= ~(1 << PORTD2);// Speaker at PD2
126	else	127	else
127	PORTC &= ~(1 << PORTC7);// Speaker at PC7	128	PORTC &= ~(1 << PORTC7);// Speaker at PC7
128	}	129	}
129		130
130	#ifdef USE_MK3MAG	131	#ifdef USE_MK3MAG
131	// update compass value if this option is enabled in the settings	132	// update compass value if this option is enabled in the settings
132	if (staticParams.bitConfig & CFG_COMPASS_ENABLED) {	133	if (staticParams.bitConfig & CFG_COMPASS_ENABLED) {
133	MK3MAG_periodicTask(); // read out mk3mag pwm	134	MK3MAG_periodicTask(); // read out mk3mag pwm
134	}	135	}
135	#endif	136	#endif
136	}	137	}
137		138
138	// -----------------------------------------------------------------------	139	// -----------------------------------------------------------------------
139	uint16_t setDelay(uint16_t t) {	140	uint16_t setDelay(uint16_t t) {
140	return (globalMillisClock + t - 1);	141	return (globalMillisClock + t - 1);
141	}	142	}
142		143
143	// -----------------------------------------------------------------------	144	// -----------------------------------------------------------------------
144	int8_t checkDelay(uint16_t t) {	145	int8_t checkDelay(uint16_t t) {
145	return (((t - globalMillisClock) & 0x8000) >> 8); // check sign bit	146	return (((t - globalMillisClock) & 0x8000) >> 8); // check sign bit
146	}	147	}
147		148
148	// -----------------------------------------------------------------------	149	// -----------------------------------------------------------------------
149	void delay_ms(uint16_t w) {	150	void delay_ms(uint16_t w) {
150	uint16_t t_stop = setDelay(w);	151	uint16_t t_stop = setDelay(w);
151	while (!checkDelay(t_stop))	152	while (!checkDelay(t_stop))
152	;	153	wdt_reset();
153	}	154	}
154		155
155	// -----------------------------------------------------------------------	156	// -----------------------------------------------------------------------
156	void delay_ms_with_adc_measurement(uint16_t w, uint8_t stop) {	157	void delay_ms_with_adc_measurement(uint16_t w, uint8_t stop) {
157	uint16_t t_stop;	158	uint16_t t_stop;
158	t_stop = setDelay(w);	159	t_stop = setDelay(w);
159	while (!checkDelay(t_stop)) {	160	while (!checkDelay(t_stop)) {
-		161	wdt_reset();
160	if (analogDataReady) {	162	if (analogDataReady) {
161	analog_update();	163	analog_update();
162	startAnalogConversionCycle();	164	startAnalogConversionCycle();
163	}	165	}
164	}	166	}
165	if (stop) {	167	if (stop) {
166	// Wait for new samples to get prepared but do not restart AD conversion after that!	168	// Wait for new samples to get prepared but do not restart AD conversion after that!
167	// Caller MUST to that.	169	// Caller MUST to that.
168	while (!analogDataReady);	170	if (!analogDataReady) wdt_reset();
169	}	171	}
170	}	172	}
171		173
172	#ifdef DO_PROFILE	174	#ifdef DO_PROFILE
173	void startProfileTimer(uint8_t timer) {	175	void startProfileTimer(uint8_t timer) {
174	runningProfileTimers[timer] = global10kHzClock++;	176	runningProfileTimers[timer] = global10kHzClock++;
175	}	177	}
176		178
177	void stopProfileTimer(uint8_t timer) {	179	void stopProfileTimer(uint8_t timer) {
178	int32_t t = global10kHzClock++ - runningProfileTimers[timer];	180	int32_t t = global10kHzClock++ - runningProfileTimers[timer];
179	profileTimers[timer] += t;	181	profileTimers[timer] += t;
180	}	182	}
181		183
182	void debugProfileTimers(uint8_t index) {	184	void debugProfileTimers(uint8_t index) {
183	for (uint8_t i=0; i<NUM_PROFILE_TIMERS; i++) {	185	for (uint8_t i=0; i<NUM_PROFILE_TIMERS; i++) {
184	uint16_t tenths = profileTimers[i] / 1000L;	186	uint16_t tenths = profileTimers[i] / 1000L;
185	debugOut.analog[i+index] = tenths;	187	debugOut.analog[i+index] = tenths;
186	}	188	}
187	uint16_t tenths = global10kHzClock / 1000L;	189	uint16_t tenths = global10kHzClock / 1000L;
188	debugOut.analog[index + NUM_PROFILE_TIMERS] = tenths;	190	debugOut.analog[index + NUM_PROFILE_TIMERS] = tenths;
189	}	191	}
190	#endif;	192	#endif;
191		193

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/timer0.c @ 2098 - Rev 2125 → 2132