WebSVN - FlightCtrl - Blame - Rev 1760 - /branches/V2.02a_FirstTrial

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

Subversion Repositories FlightCtrl

(root)/branches/V2.02a_FirstTrial_Peter/timer0.c - Rev 1760