WebSVN - FlightCtrl - Blame - Rev 1286 - /branches/V0.74d_ACC-HH

Rev	Author	Line No.	Line
1	ingob	1	#include "main.h"
		2
		3	volatile unsigned int CountMilliseconds = 0;
		4	volatile static unsigned int tim_main;
		5	volatile unsigned char UpdateMotor = 0;
		6	volatile unsigned int cntKompass = 0;
		7	volatile unsigned int beeptime = 0;
1171	hbuss	8	volatile unsigned char SendSPI = 0, ServoActive = 0;
723	hbuss	9
173	holgerb	10	unsigned int BeepMuster = 0xffff;
1105	killagreg	11	int ServoValue = 0;
1	ingob	12
1156	hbuss	13	volatile int16_t ServoNickValue = 0;
		14	volatile int16_t ServoRollValue = 0;
		15
		16
1	ingob	17	enum {
		18	STOP = 0,
		19	CK = 1,
		20	CK8 = 2,
		21	CK64 = 3,
		22	CK256 = 4,
		23	CK1024 = 5,
		24	T0_FALLING_EDGE = 6,
		25	T0_RISING_EDGE = 7
		26	};
		27
		28
		29	SIGNAL (SIG_OVERFLOW0) // 8kHz
		30	{
		31	static unsigned char cnt_1ms = 1,cnt = 0;
173	holgerb	32	unsigned char pieper_ein = 0;
1	ingob	33	// TCNT0 -= 250;//TIMER_RELOAD_VALUE;
723	hbuss	34	if(SendSPI) SendSPI--;
1286	MartinR	35
		36	ANALOG_START; // MartinR: um Start AD zu steuern. 16 AD-Wandlungen mit 8kHz= 500 Hz
		37
1	ingob	38	if(!cnt--)
		39	{
1105	killagreg	40	cnt = 9;
1	ingob	41	cnt_1ms++;
		42	cnt_1ms %= 2;
		43	if(!cnt_1ms) UpdateMotor = 1;
		44	CountMilliseconds++;
1105	killagreg	45	}
1	ingob	46
		47	if(beeptime > 1)
		48	{
1105	killagreg	49	beeptime--;
		50	if(beeptime & BeepMuster)
173	holgerb	51	{
		52	pieper_ein = 1;
		53	}
		54	else pieper_ein = 0;
1	ingob	55	}
1105	killagreg	56	else
173	holgerb	57	{
		58	pieper_ein = 0;
		59	BeepMuster = 0xffff;
1105	killagreg	60	}
173	holgerb	61
		62
		63	if(pieper_ein)
		64	{
		65	if(PlatinenVersion == 10) PORTD \|= (1<<2); // Speaker an PORTD.2
		66	else PORTC \|= (1<<7); // Speaker an PORTC.7
		67	}
1105	killagreg	68	else
173	holgerb	69	{
		70	if(PlatinenVersion == 10) PORTD &= ~(1<<2);
		71	else PORTC &= ~(1<<7);
1105	killagreg	72	}
		73
1	ingob	74	if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
		75	{
		76	if(PINC & 0x10)
		77	{
1105	killagreg	78	cntKompass++;
1	ingob	79	}
		80	else
		81	{
1105	killagreg	82	if((cntKompass) && (cntKompass < 362))
		83	{
693	hbuss	84	cntKompass += cntKompass / 41;
		85	if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0;
1105	killagreg	86	}
1	ingob	87	// if(cntKompass < 10) cntKompass = 10;
		88	// KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L;
		89	KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
		90	cntKompass = 0;
1105	killagreg	91	}
1	ingob	92	}
		93	}
		94
		95
		96	// -----------------------------------------------------------------------
		97
		98	unsigned int SetDelay (unsigned int t)
		99	{
		100	// TIMSK0 &= ~_BV(TOIE0);
1105	killagreg	101	return(CountMilliseconds + t + 1);
1	ingob	102	// TIMSK0 \|= _BV(TOIE0);
		103	}
		104
		105	// -----------------------------------------------------------------------
		106	char CheckDelay(unsigned int t)
		107	{
		108	// TIMSK0 &= ~_BV(TOIE0);
		109	return(((t - CountMilliseconds) & 0x8000) >> 9);
		110	// TIMSK0 \|= _BV(TOIE0);
		111	}
		112
		113	// -----------------------------------------------------------------------
		114	void Delay_ms(unsigned int w)
		115	{
		116	unsigned int akt;
		117	akt = SetDelay(w);
		118	while (!CheckDelay(akt));
		119	}
		120
395	hbuss	121	void Delay_ms_Mess(unsigned int w)
		122	{
		123	unsigned int akt;
		124	akt = SetDelay(w);
1166	hbuss	125	while (!CheckDelay(akt)) if(AdReady) {AdReady = 0; ANALOG_ON;}
395	hbuss	126	}
		127
1156	hbuss	128	/*****************************************************/
		129	/* Initialize Timer 2 */
		130	/*****************************************************/
		131	// The timer 2 is used to generate the PWM at PD7 (J7)
		132	// to control a camera servo for nick compensation.
		133	void TIMER2_Init(void)
910	hbuss	134	{
1156	hbuss	135	uint8_t sreg = SREG;
		136
		137	// disable all interrupts before reconfiguration
		138	cli();
		139
		140	// set PD7 as output of the PWM for nick servo
		141	DDRD \|= (1<<DDD7);
		142	PORTD &= ~(1<<PORTD7); // set PD7 to low
		143
		144	DDRC \|= (1<<DDC6); // set PC6 as output (Reset for HEF4017)
1171	hbuss	145	HEF4017R_ON;
1156	hbuss	146	// Timer/Counter 2 Control Register A
		147
		148	// Timer Mode is FastPWM with timer reload at OCR2A (Bits: WGM22 = 1, WGM21 = 1, WGM20 = 1)
		149	// PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0)
		150	// PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0)
		151	TCCR2A &= ~((1<<COM2A1)\|(1<<COM2A0)\|(1<<COM2B1)\|(1<<COM2B0));
		152	TCCR2A \|= (1<<WGM21)\|(1<<WGM20);
		153
		154	// Timer/Counter 2 Control Register B
		155
		156	// Set clock divider for timer 2 to SYSKLOCK/32 = 20MHz / 32 = 625 kHz
		157	// The timer increments from 0x00 to 0xFF with an update rate of 625 kHz or 1.6 us
		158	// hence the timer overflow interrupt frequency is 625 kHz / 256 = 2.44 kHz or 0.4096 ms
		159
		160	// divider 32 (Bits: CS022 = 0, CS21 = 1, CS20 = 1)
		161	TCCR2B &= ~((1<<FOC2A)\|(1<<FOC2B)\|(1<<CS22));
		162	TCCR2B \|= (1<<CS21)\|(1<<CS20)\|(1<<WGM22);
		163
		164	// Initialize the Timer/Counter 2 Register
		165	TCNT2 = 0;
		166
		167	// Initialize the Output Compare Register A used for PWM generation on port PD7.
		168	OCR2A = 255;
		169	TCCR2A \|= (1<<COM2A1); // set or clear at compare match depends on value of COM2A0
		170
		171	// Timer/Counter 2 Interrupt Mask Register
		172	// Enable timer output compare match A Interrupt only
		173	TIMSK2 &= ~((1<<OCIE2B)\|(1<<TOIE2));
		174	TIMSK2 \|= (1<<OCIE2A);
		175
		176	SREG = sreg;
910	hbuss	177	}
		178
1156	hbuss	179	//----------------------------
		180	void Timer_Init(void)
1	ingob	181	{
1156	hbuss	182	tim_main = SetDelay(10);
		183	TCCR0B = CK8;
		184	TCCR0A = (1<<COM0A1)\|(1<<COM0B1)\|3;//fast PWM
		185	OCR0A = 0;
		186	OCR0B = 120;
		187	TCNT0 = (unsigned char)-TIMER_RELOAD_VALUE; // reload
		188	//OCR1 = 0x00;
1105	killagreg	189
1156	hbuss	190	TIMSK0 \|= _BV(TOIE0);
		191	}
		192
		193
		194	/*****************************************************/
		195	/* Control Servo Position */
		196	/*****************************************************/
		197
		198	ISR(TIMER2_COMPA_vect)
		199	{
		200
		201	// frame len 22.5 ms = 14063 * 1.6 us
		202	// stop pulse: 0.3 ms = 188 * 1.6 us
		203	// min servo pulse: 0.6 ms = 375 * 1.6 us
		204	// max servo pulse: 2.4 ms = 1500 * 1.6 us
		205	// resolution: 1500 - 375 = 1125 steps
		206
		207	#define IRS_RUNTIME 127
		208	#define PPM_STOPPULSE 188
		209	// #define PPM_FRAMELEN (14063
1171	hbuss	210	#define PPM_FRAMELEN (1757 * EE_Parameter.ServoNickRefresh)
1156	hbuss	211	#define MINSERVOPULSE 375
		212	#define MAXSERVOPULSE 1500
		213	#define SERVORANGE (MAXSERVOPULSE - MINSERVOPULSE)
		214
		215	static uint8_t PulseOutput = 0;
		216	static uint16_t RemainingPulse = 0;
		217	static uint16_t ServoFrameTime = 0;
		218	static uint8_t ServoIndex = 0;
		219
		220	#define MULTIPLYER 4
		221	static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center positon
1224	hbuss	222	static int16_t ServoRollOffset = (255 / 2) * MULTIPLYER; // initial value near center positon
1156	hbuss	223
		224	if(PlatinenVersion < 20)
		225	{
		226	//---------------------------
		227	// Nick servo state machine
		228	//---------------------------
		229	if(!PulseOutput) // pulse output complete
		230	{
		231	if(TCCR2A & (1<<COM2A0)) // we had a low pulse
		232	{
		233	TCCR2A &= ~(1<<COM2A0);// make a high pulse
		234	RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
		235
		236	ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
		237	ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
		238	if(EE_Parameter.ServoNickCompInvert & 0x01)
		239	{ // inverting movement of servo
		240	ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
		241	}
		242	else
		243	{ // non inverting movement of servo
		244	ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
		245	}
		246	// limit servo value to its parameter range definition
		247	if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
		248	{
		249	ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
		250	}
		251	else
		252	if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
		253	{
		254	ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
		255	}
		256
		257	RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
		258
		259	ServoNickValue /= MULTIPLYER;
		260	DebugOut.Analog[20] = ServoNickValue;
		261
		262	// range servo pulse width
		263	if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit
		264	else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit
		265	// accumulate time for correct update rate
		266	ServoFrameTime = RemainingPulse;
		267	}
		268	else // we had a high pulse
		269	{
		270	TCCR2A \|= (1<<COM2A0); // make a low pulse
		271	RemainingPulse = PPM_FRAMELEN - ServoFrameTime;
		272	}
		273	// set pulse output active
		274	PulseOutput = 1;
		275	}
		276	} // EOF Nick servo state machine
		277	else
		278	{
		279	//-----------------------------------------------------
		280	// PPM state machine, onboard demultiplexed by HEF4017
		281	//-----------------------------------------------------
		282	if(!PulseOutput) // pulse output complete
		283	{
		284	if(TCCR2A & (1<<COM2A0)) // we had a low pulse
		285	{
		286	TCCR2A &= ~(1<<COM2A0);// make a high pulse
		287
		288	if(ServoIndex == 0) // if we are at the sync gap
		289	{
		290	RemainingPulse = PPM_FRAMELEN - ServoFrameTime; // generate sync gap by filling time to full frame time
		291	ServoFrameTime = 0; // reset servo frame time
		292	HEF4017R_ON; // enable HEF4017 reset
		293	}
		294	else // servo channels
		295	{
		296	RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
		297	switch(ServoIndex) // map servo channels
		298	{
		299	case 1: // Nick Compensation Servo
		300	ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
		301	ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
		302	if(EE_Parameter.ServoNickCompInvert & 0x01)
		303	{ // inverting movement of servo
		304	ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
		305	}
		306	else
		307	{ // non inverting movement of servo
		308	ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
		309	}
		310	// limit servo value to its parameter range definition
		311	if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
		312	{
		313	ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
		314	}
		315	else
		316	if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
		317	{
		318	ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
		319	}
		320	RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
		321	ServoNickValue /= MULTIPLYER;
		322	DebugOut.Analog[20] = ServoNickValue;
		323	break;
1224	hbuss	324	case 2: // Roll Compensation Servo
		325	ServoRollOffset = (ServoRollOffset * 3 + (int16_t) 80 * MULTIPLYER) / 4; // lowpass offset
		326	ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765)
		327	//if(EE_Parameter.ServoRollCompInvert & 0x01)
		328	{ // inverting movement of servo
		329	ServoRollValue += (int16_t)( ( (int32_t) 50 * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
		330	}
		331	/* else
		332	{ // non inverting movement of servo
		333	ServoRollValue -= (int16_t)( ( (int32_t) 40 * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
		334	}
		335	*/ // limit servo value to its parameter range definition
		336	if(ServoRollValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
		337	{
		338	ServoRollValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
		339	}
		340	else
		341	if(ServoRollValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
		342	{
		343	ServoRollValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
		344	}
		345	RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
		346	ServoRollValue /= MULTIPLYER;
		347	//DebugOut.Analog[20] = ServoRollValue;
1156	hbuss	348
1224	hbuss	349	/* ServoRollOffset = (ServoRollOffset * 3 + (int16_t)Parameter_ServoRollControl * MULTIPLYER) / 4; // lowpass offset
		350	ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765)
		351	if(EE_Parameter.ServoRollCompInvert & 0x01)
		352	{ // inverting movement of servo
		353	ServoRollValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
		354	}
		355	else
		356	{ // non inverting movement of servo
		357	ServoRollValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
		358	}
		359	// limit servo value to its parameter range definition
		360	if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) )
		361	{
		362	ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER;
		363	}
		364	else
		365	if(ServoRollValue > ((int16_t)EE_Parameter.ServoRollMax * MULTIPLYER) )
		366	{
		367	ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER;
		368	}
		369	RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
		370	ServoRollValue /= MULTIPLYER;
		371	//DebugOut.Analog[20] = ServoRollValue;
		372	*/ break;
		373
1156	hbuss	374	default: // other servo channels
		375	RemainingPulse += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2�s
		376	break;
		377	}
		378	// range servo pulse width
		379	if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit
		380	else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit
		381	// substract stop pulse width
		382	RemainingPulse -= PPM_STOPPULSE;
		383	// accumulate time for correct sync gap
		384	ServoFrameTime += RemainingPulse;
		385	}
		386	}
		387	else // we had a high pulse
		388	{
		389	TCCR2A \|= (1<<COM2A0); // make a low pulse
		390	// set pulsewidth to stop pulse width
		391	RemainingPulse = PPM_STOPPULSE;
		392	// accumulate time for correct sync gap
		393	ServoFrameTime += RemainingPulse;
1171	hbuss	394	if(ServoActive && SenderOkay > 180) HEF4017R_OFF; // disable HEF4017 reset
1156	hbuss	395	ServoIndex++; // change to next servo channel
		396	if(ServoIndex > EE_Parameter.ServoNickRefresh) ServoIndex = 0; // reset to the sync gap
		397	}
		398	// set pulse output active
		399	PulseOutput = 1;
		400	}
		401	} // EOF PPM state machine
		402
		403	// General pulse output generator
		404	if(RemainingPulse > (255 + IRS_RUNTIME))
		405	{
		406	OCR2A = 255;
		407	RemainingPulse -= 255;
910	hbuss	408	}
1156	hbuss	409	else
		410	{
		411	if(RemainingPulse > 255) // this is the 2nd last part
		412	{
		413	if((RemainingPulse - 255) < IRS_RUNTIME)
		414	{
		415	OCR2A = 255 - IRS_RUNTIME;
		416	RemainingPulse -= 255 - IRS_RUNTIME;
		417
		418	}
		419	else // last part > ISR_RUNTIME
		420	{
		421	OCR2A = 255;
		422	RemainingPulse -= 255;
		423	}
		424	}
		425	else // this is the last part
		426	{
		427	OCR2A = RemainingPulse;
		428	RemainingPulse = 0;
		429	PulseOutput = 0; // trigger to stop pulse
		430	}
		431	} // EOF general pulse output generator
1111	hbuss	432	}

Subversion Repositories FlightCtrl

(root)/branches/V0.74d_ACC-HH_MartinR/timer0.c - Rev 1286