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

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