WebSVN - FlightCtrl - Blame - Rev 1762 - /branches/V0.88n_ACC-HH_HR

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	{
1762	holgerb	194	char cosinus, sinus;
1760	holgerb	195	long nick, roll;
		196
		197	cosinus = sintab[EE_Parameter.CamOrientation + 6];
		198	sinus = sintab[EE_Parameter.CamOrientation];
		199
		200	if(CalculateServoSignals == 1)
		201	{
1762	holgerb	202	nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L;
		203	nick = (EE_Parameter.ServoNickComp * nick) / 512L;
1760	holgerb	204	ServoNickOffset += ((int16_t)Parameter_ServoNickControl * (MULTIPLYER*16) - ServoNickOffset) / EE_Parameter.ServoManualControlSpeed;
		205	ServoNickValue = ServoNickOffset / 16; // offset (Range from 0 to 255 * 3 = 765)
		206	if(EE_Parameter.ServoCompInvert & 0x01)
		207	{ // inverting movement of servo
1762	holgerb	208	ServoNickValue += nick;//(int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * nick) / (256L) );
1760	holgerb	209	}
		210	else
		211	{ // non inverting movement of servo
1762	holgerb	212	ServoNickValue -= nick;
1760	holgerb	213	}
		214	// limit servo value to its parameter range definition
		215	if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
		216	{
		217	ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
		218	}
		219	else
		220	if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
		221	{
		222	ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
		223	}
		224	if(PlatinenVersion < 20) CalculateServoSignals = 0; else CalculateServoSignals++;
		225	}
		226	else
		227	{
1762	holgerb	228	roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L;
		229	roll = (EE_Parameter.ServoRollComp * roll) / 512L;
1760	holgerb	230	ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed;
		231	ServoRollValue = ServoRollOffset/16; // offset (Range from 0 to 255 * 3 = 765)
		232	if(EE_Parameter.ServoCompInvert & 0x02)
		233	{ // inverting movement of servo
1762	holgerb	234	ServoRollValue += roll;
1760	holgerb	235	}
		236	else
		237	{ // non inverting movement of servo
1762	holgerb	238	ServoRollValue -= roll;
1760	holgerb	239	}
		240	// limit servo value to its parameter range definition
		241	if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) )
		242	{
		243	ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER;
		244	}
		245	else
		246	if(ServoRollValue > ((int16_t)EE_Parameter.ServoRollMax * MULTIPLYER) )
		247	{
		248	ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER;
		249	}
		250	CalculateServoSignals = 0;
		251	}
		252	}
		253
1156	hbuss	254	ISR(TIMER2_COMPA_vect)
		255	{
		256	// frame len 22.5 ms = 14063 * 1.6 us
		257	// stop pulse: 0.3 ms = 188 * 1.6 us
		258	// min servo pulse: 0.6 ms = 375 * 1.6 us
		259	// max servo pulse: 2.4 ms = 1500 * 1.6 us
		260	// resolution: 1500 - 375 = 1125 steps
		261
		262	#define IRS_RUNTIME 127
		263	#define PPM_STOPPULSE 188
1171	hbuss	264	#define PPM_FRAMELEN (1757 * EE_Parameter.ServoNickRefresh)
1156	hbuss	265	#define MINSERVOPULSE 375
		266	#define MAXSERVOPULSE 1500
		267	#define SERVORANGE (MAXSERVOPULSE - MINSERVOPULSE)
		268
		269	static uint8_t PulseOutput = 0;
		270	static uint16_t ServoFrameTime = 0;
		271	static uint8_t ServoIndex = 0;
		272
		273
		274	if(PlatinenVersion < 20)
		275	{
		276	//---------------------------
		277	// Nick servo state machine
		278	//---------------------------
		279	if(!PulseOutput) // pulse output complete
		280	{
		281	if(TCCR2A & (1<<COM2A0)) // we had a low pulse
		282	{
		283	TCCR2A &= ~(1<<COM2A0);// make a high pulse
		284	RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
		285	RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
		286	// range servo pulse width
		287	if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit
		288	else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit
		289	// accumulate time for correct update rate
		290	ServoFrameTime = RemainingPulse;
		291	}
		292	else // we had a high pulse
		293	{
		294	TCCR2A \|= (1<<COM2A0); // make a low pulse
		295	RemainingPulse = PPM_FRAMELEN - ServoFrameTime;
		296	}
		297	// set pulse output active
		298	PulseOutput = 1;
		299	}
		300	} // EOF Nick servo state machine
		301	else
		302	{
		303	//-----------------------------------------------------
		304	// PPM state machine, onboard demultiplexed by HEF4017
		305	//-----------------------------------------------------
		306	if(!PulseOutput) // pulse output complete
		307	{
		308	if(TCCR2A & (1<<COM2A0)) // we had a low pulse
		309	{
		310	TCCR2A &= ~(1<<COM2A0);// make a high pulse
		311
		312	if(ServoIndex == 0) // if we are at the sync gap
		313	{
		314	RemainingPulse = PPM_FRAMELEN - ServoFrameTime; // generate sync gap by filling time to full frame time
		315	ServoFrameTime = 0; // reset servo frame time
		316	HEF4017R_ON; // enable HEF4017 reset
		317	}
		318	else // servo channels
		319	{
		320	RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
		321	switch(ServoIndex) // map servo channels
		322	{
		323	case 1: // Nick Compensation Servo
		324	RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
		325	break;
1224	hbuss	326	case 2: // Roll Compensation Servo
		327	RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
1232	hbuss	328	break;
1565	killagreg	329	case 3:
		330	RemainingPulse += ((int16_t)Parameter_Servo3 * MULTIPLYER) - (256 / 2) * MULTIPLYER;
1403	hbuss	331	break;
1543	killagreg	332	case 4:
1565	killagreg	333	RemainingPulse += ((int16_t)Parameter_Servo4 * MULTIPLYER) - (256 / 2) * MULTIPLYER;
1403	hbuss	334	break;
1543	killagreg	335	case 5:
1565	killagreg	336	RemainingPulse += ((int16_t)Parameter_Servo5 * MULTIPLYER) - (256 / 2) * MULTIPLYER;
1403	hbuss	337	break;
1156	hbuss	338	default: // other servo channels
		339	RemainingPulse += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2�s
		340	break;
		341	}
		342	// range servo pulse width
		343	if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit
		344	else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit
		345	// substract stop pulse width
		346	RemainingPulse -= PPM_STOPPULSE;
		347	// accumulate time for correct sync gap
		348	ServoFrameTime += RemainingPulse;
		349	}
		350	}
		351	else // we had a high pulse
		352	{
		353	TCCR2A \|= (1<<COM2A0); // make a low pulse
		354	// set pulsewidth to stop pulse width
		355	RemainingPulse = PPM_STOPPULSE;
		356	// accumulate time for correct sync gap
		357	ServoFrameTime += RemainingPulse;
1701	holgerb	358	if((ServoActive && SenderOkay > 180) \|\| ServoActive == 2) HEF4017R_OFF; // disable HEF4017 reset
1232	hbuss	359	else HEF4017R_ON;
1156	hbuss	360	ServoIndex++; // change to next servo channel
1760	holgerb	361	if(ServoIndex > EE_Parameter.ServoNickRefresh)
		362	{
		363	CalculateServoSignals = 1;
		364	ServoIndex = 0; // reset to the sync gap
		365	}
1156	hbuss	366	}
		367	// set pulse output active
		368	PulseOutput = 1;
		369	}
		370	} // EOF PPM state machine
		371
		372	// General pulse output generator
		373	if(RemainingPulse > (255 + IRS_RUNTIME))
		374	{
		375	OCR2A = 255;
		376	RemainingPulse -= 255;
910	hbuss	377	}
1156	hbuss	378	else
		379	{
		380	if(RemainingPulse > 255) // this is the 2nd last part
		381	{
		382	if((RemainingPulse - 255) < IRS_RUNTIME)
		383	{
		384	OCR2A = 255 - IRS_RUNTIME;
		385	RemainingPulse -= 255 - IRS_RUNTIME;
		386
		387	}
		388	else // last part > ISR_RUNTIME
		389	{
		390	OCR2A = 255;
		391	RemainingPulse -= 255;
		392	}
		393	}
		394	else // this is the last part
		395	{
		396	OCR2A = RemainingPulse;
		397	RemainingPulse = 0;
		398	PulseOutput = 0; // trigger to stop pulse
		399	}
		400	} // EOF general pulse output generator
1111	hbuss	401	}

Subversion Repositories FlightCtrl

(root)/branches/V0.88n_ACC-HH_HR_MartinR/timer0.c - Rev 1762