WebSVN - FlightCtrl - Blame - Rev 1754 - /branches/V0.80d-Arthur-P/timer0.c

Rev	Author	Line No.	Line
1754	-	1	// Arthur P. Modified to use several parameters for servo control:
		2	// User_Parameter4:
		3	// User_Parameter5:
		4	// User_Parameter6:
		5	// User_Parameter7:
		6	// User_Parameter8: Use external HEF4017 if bit 8 is set (>127). The remaining 7 bits are used
		7	// for the shutter cycle counter: the value is multiplied by 5 programmatically,
		8	// resulting in steps of approx. 0.1sec. Minimum value to start using the
		9	// interval timer is 10 (approx. 1 sec, or countervalue of 50). Note that this
		10	// was originally done through user para 6.
		11
		12
		13	#include "main.h"
		14
		15
		16	volatile unsigned int CountMilliseconds = 0;
		17	volatile static unsigned int tim_main;
		18	volatile unsigned char UpdateMotor = 0;
		19	volatile unsigned int cntKompass = 0;
		20	volatile unsigned int beeptime = 0;
		21	volatile unsigned char SendSPI = 0, ServoActive = 0;
		22
		23	unsigned int BeepMuster = 0xffff;
		24
		25	volatile int16_t ServoNickValue = 0;
		26	volatile int16_t ServoRollValue = 0;
		27
		28	// Arthur P: Added two variables for control of the shutter servo cycle.
		29	// 091114 Inserted same changes into v.0.76g code.
		30	// 091114 Inactivated the following two lines as the shutter interval funtion is not
		31	// used at the moment.
		32	// 20100804 Reactivated to be able to choose slower shutter rate than normal for
		33	// Panasonic FX150 in continuous mode.
		34	volatile static unsigned int CameraShutterCycleCounter = 0;
		35	volatile static unsigned int CameraShutterCycle = 0;
		36	volatile static unsigned int CameraShutterCycleOnCount = 20; // Leave the shutter on for at least
		37	// 20 cycles or approx. 0.2 seconds.
		38
		39	enum {
		40	STOP = 0,
		41	CK = 1,
		42	CK8 = 2,
		43	CK64 = 3,
		44	CK256 = 4,
		45	CK1024 = 5,
		46	T0_FALLING_EDGE = 6,
		47	T0_RISING_EDGE = 7
		48	};
		49
		50
		51	ISR(TIMER0_OVF_vect) // 9,7kHz
		52	{
		53	static unsigned char cnt_1ms = 1,cnt = 0, compass_active = 0;
		54	unsigned char pieper_ein = 0;
		55	if(SendSPI) SendSPI--;
		56	if(SpektrumTimer) SpektrumTimer--;
		57	if(!cnt--)
		58	{
		59	cnt = 9;
		60	CountMilliseconds++;
		61	cnt_1ms++;
		62	cnt_1ms %= 2;
		63
		64	if(!cnt_1ms) UpdateMotor = 1;
		65	if(!PINC & 0x10) compass_active = 1;
		66
		67	if(beeptime)
		68	{
		69	if(beeptime > 10) beeptime -= 10; else beeptime = 0;
		70	if(beeptime & BeepMuster)
		71	{
		72	pieper_ein = 1;
		73	}
		74	else pieper_ein = 0;
		75	}
		76	else
		77	{
		78	pieper_ein = 0;
		79	BeepMuster = 0xffff;
		80	}
		81	if(pieper_ein)
		82	{
		83	if(PlatinenVersion == 10) PORTD \|= (1<<2); // Speaker an PORTD.2
		84	else PORTC \|= (1<<7); // Speaker an PORTC.7
		85	}
		86	else
		87	{
		88	if(PlatinenVersion == 10) PORTD &= ~(1<<2);
		89	else PORTC &= ~(1<<7);
		90	}
		91	}
		92	if(compass_active && !NaviDataOkay && EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
		93	{
		94	if(PINC & 0x10)
		95	{
		96	if(++cntKompass > 1000) compass_active = 0;
		97	}
		98	else
		99	{
		100	if((cntKompass) && (cntKompass < 362))
		101	{
		102	cntKompass += cntKompass / 41;
		103	if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0;
		104	KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
		105	}
		106	cntKompass = 0;
		107	}
		108	}
		109	}
		110
		111
		112	// -----------------------------------------------------------------------
		113	unsigned int SetDelay (unsigned int t)
		114	{
		115	// TIMSK0 &= ~_BV(TOIE0);
		116	return(CountMilliseconds + t + 1);
		117	// TIMSK0 \|= _BV(TOIE0);
		118	}
		119
		120	// -----------------------------------------------------------------------
		121	char CheckDelay(unsigned int t)
		122	{
		123	// TIMSK0 &= ~_BV(TOIE0);
		124	return(((t - CountMilliseconds) & 0x8000) >> 9);
		125	// TIMSK0 \|= _BV(TOIE0);
		126	}
		127
		128	// -----------------------------------------------------------------------
		129	void Delay_ms(unsigned int w)
		130	{
		131	unsigned int akt;
		132	akt = SetDelay(w);
		133	while (!CheckDelay(akt));
		134	}
		135
		136	void Delay_ms_Mess(unsigned int w)
		137	{
		138	unsigned int akt;
		139	akt = SetDelay(w);
		140	while (!CheckDelay(akt)) if(AdReady) {AdReady = 0; ANALOG_ON;}
		141	}
		142
		143	/*****************************************************/
		144	/* Initialize Timer 2 */
		145	/*****************************************************/
		146	// The timer 2 is used to generate the PWM at PD7 (J7)
		147	// to control a camera servo for nick compensation.
		148	void TIMER2_Init(void)
		149	{
		150	uint8_t sreg = SREG;
		151
		152
		153	// Arthur P: Added initialization of the CameraShutterCycle value here as this routine is only
		154	// called once. This retains all code changes in timer0.c. If parameter8 lower 6 > 0 then the user
		155	// has set a value for the cycle. CameraShuytterCycle == 5x Para6 to get approx 0.1sec increments.
		156	// 090807: Arthur P.: Removed the shutter cycle parts as they may be impacting timing loops.
		157	// 20100804 Arthur P.: Reactivate shutter cycle counters. Modified to use the lower 7 bits of
		158	// user parameter 7 (bit 8 is used for enabling the external HEF4017).
		159	// CameraShutterCycle = Parameter_UserParam6;
		160	CameraShutterCycle = 5 * (Parameter_UserParam8 & 127);
		161
		162	// disable all interrupts before reconfiguration
		163	cli();
		164
		165	PORTD &= ~(1<<PORTD7); // set PD7 to low
		166
		167	DDRC \|= (1<<DDC6); // set PC6 as output (Reset for HEF4017)
		168	HEF4017R_ON;
		169	// Timer/Counter 2 Control Register A
		170
		171	// Timer Mode is FastPWM with timer reload at OCR2A (Bits: WGM22 = 1, WGM21 = 1, WGM20 = 1)
		172	// PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0)
		173	// PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0)
		174	TCCR2A &= ~((1<<COM2A1)\|(1<<COM2A0)\|(1<<COM2B1)\|(1<<COM2B0));
		175	TCCR2A \|= (1<<WGM21)\|(1<<WGM20);
		176
		177	// Timer/Counter 2 Control Register B
		178
		179	// Set clock divider for timer 2 to SYSKLOCK/32 = 20MHz / 32 = 625 kHz
		180	// The timer increments from 0x00 to 0xFF with an update rate of 625 kHz or 1.6 us
		181	// hence the timer overflow interrupt frequency is 625 kHz / 256 = 2.44 kHz or 0.4096 ms
		182
		183	// divider 32 (Bits: CS022 = 0, CS21 = 1, CS20 = 1)
		184	TCCR2B &= ~((1<<FOC2A)\|(1<<FOC2B)\|(1<<CS22));
		185	TCCR2B \|= (1<<CS21)\|(1<<CS20)\|(1<<WGM22);
		186
		187	// Initialize the Timer/Counter 2 Register
		188	TCNT2 = 0;
		189
		190	// Initialize the Output Compare Register A used for PWM generation on port PD7.
		191	OCR2A = 255;
		192	TCCR2A \|= (1<<COM2A1); // set or clear at compare match depends on value of COM2A0
		193
		194	// Timer/Counter 2 Interrupt Mask Register
		195	// Enable timer output compare match A Interrupt only
		196	TIMSK2 &= ~((1<<OCIE2B)\|(1<<TOIE2));
		197	TIMSK2 \|= (1<<OCIE2A);
		198
		199	SREG = sreg;
		200	}
		201
		202	//----------------------------
		203	void Timer_Init(void)
		204	{
		205	tim_main = SetDelay(10);
		206	TCCR0B = CK8;
		207	TCCR0A = (1<<COM0A1)\|(1<<COM0B1)\|3;//fast PWM
		208	OCR0A = 0;
		209	OCR0B = 180;
		210	TCNT0 = (unsigned char)-TIMER_RELOAD_VALUE; // reload
		211	//OCR1 = 0x00;
		212	TIMSK0 \|= _BV(TOIE0);
		213	}
		214
		215
		216	/*****************************************************/
		217	/* Control Servo Position */
		218	/*****************************************************/
		219
		220	ISR(TIMER2_COMPA_vect)
		221	{
		222	// frame len 22.5 ms = 14063 * 1.6 us
		223	// stop pulse: 0.3 ms = 188 * 1.6 us
		224	// min servo pulse: 0.6 ms = 375 * 1.6 us
		225	// max servo pulse: 2.4 ms = 1500 * 1.6 us
		226	// resolution: 1500 - 375 = 1125 steps
		227
		228	#define IRS_RUNTIME 127
		229	#define PPM_STOPPULSE 188
		230	// #define PPM_FRAMELEN (14063
		231	#define PPM_FRAMELEN (1757 * EE_Parameter.ServoNickRefresh)
		232	#define MINSERVOPULSE 375
		233	#define MAXSERVOPULSE 1500
		234	#define SERVORANGE (MAXSERVOPULSE - MINSERVOPULSE)
		235
		236	static uint8_t PulseOutput = 0;
		237	static uint16_t RemainingPulse = 0;
		238	static uint16_t ServoFrameTime = 0;
		239	static uint8_t ServoIndex = 0;
		240
		241	#define MULTIPLYER 4
		242	static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center positon
		243	static int16_t ServoRollOffset = (255 / 2) * MULTIPLYER; // initial value near center positon
		244
		245
		246	// Arthur P: Modified the code to scheck the value of parameter 8. If 128 or higher then a HEF4017 is
		247	// expected and will be used. Else J7 and J9 are seen as separate normal outputs.
		248	// if((PlatinenVersion < 20)
		249	// 091114. Inserted same changes into v.0.76g code.
		250	// 20100802 Inserted same changes into v.0.80d code.
		251
		252	if((PlatinenVersion < 20) && (Parameter_UserParam8 < 128 ))
		253	{
		254	//---------------------------
		255	// Nick servo state machine
		256	//---------------------------
		257	if(!PulseOutput) // pulse output complete
		258	{
		259	if(TCCR2A & (1<<COM2A0)) // we had a low pulse
		260	{
		261	TCCR2A &= ~(1<<COM2A0);// make a high pulse
		262	RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
		263
		264	ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
		265	ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
		266	if(EE_Parameter.ServoCompInvert & 0x01)
		267	{ // inverting movement of servo
		268	ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
		269	}
		270	else
		271	{ // non inverting movement of servo
		272	ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
		273	}
		274	// limit servo value to its parameter range definition
		275	if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
		276	{
		277	ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
		278	}
		279	else
		280	if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
		281	{
		282	ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
		283	}
		284
		285	RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
		286
		287	ServoNickValue /= MULTIPLYER;
		288
		289	// range servo pulse width
		290	if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit
		291	else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit
		292	// accumulate time for correct update rate
		293	ServoFrameTime = RemainingPulse;
		294	}
		295	else // we had a high pulse
		296	{
		297	TCCR2A \|= (1<<COM2A0); // make a low pulse
		298	RemainingPulse = PPM_FRAMELEN - ServoFrameTime;
		299	}
		300	// set pulse output active
		301	PulseOutput = 1;
		302	}
		303	} // EOF Nick servo state machine
		304	else
		305	{
		306	//-----------------------------------------------------
		307	// PPM state machine, onboard demultiplexed by HEF4017
		308	//-----------------------------------------------------
		309	if(!PulseOutput) // pulse output complete
		310	{
		311	if(TCCR2A & (1<<COM2A0)) // we had a low pulse
		312	{
		313	TCCR2A &= ~(1<<COM2A0);// make a high pulse
		314
		315	if(ServoIndex == 0) // if we are at the sync gap
		316	{
		317	RemainingPulse = PPM_FRAMELEN - ServoFrameTime; // generate sync gap by filling time to full frame time
		318	ServoFrameTime = 0; // reset servo frame time
		319	HEF4017R_ON; // enable HEF4017 reset
		320	}
		321	else // servo channels
		322	{
		323	RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms
		324	switch(ServoIndex) // map servo channels
		325	{
		326	case 1: // Nick Compensation Servo
		327	ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset
		328	ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765)
		329	if(EE_Parameter.ServoCompInvert & 0x01)
		330	{ // inverting movement of servo
		331	ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
		332	}
		333	else
		334	{ // non inverting movement of servo
		335	ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) );
		336	}
		337	// limit servo value to its parameter range definition
		338	if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
		339	{
		340	ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
		341	}
		342	else
		343	if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
		344	{
		345	ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
		346	}
		347	RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
		348	ServoNickValue /= MULTIPLYER;
		349	break;
		350	case 2: // Roll Compensation Servo
		351	ServoRollOffset = (ServoRollOffset * 3 + (int16_t) Parameter_ServoRollControl * MULTIPLYER) / 4; // lowpass offset
		352	ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765)
		353	if(EE_Parameter.ServoCompInvert & 0x02)
		354	{ // inverting movement of servo
		355	ServoRollValue += (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
		356	}
		357	else
		358	{ // non inverting movement of servo
		359	ServoRollValue -= (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) );
		360	}
		361	// limit servo value to its parameter range definition
		362	if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) )
		363	{
		364	ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER;
		365	}
		366	else
		367	if(ServoRollValue > ((int16_t)EE_Parameter.ServoRollMax * MULTIPLYER) )
		368	{
		369	ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER;
		370	}
		371	RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position
		372	ServoRollValue /= MULTIPLYER;
		373	//DebugOut.Analog[20] = ServoRollValue;
		374	break;
		375
		376	case 3: // Arthur P: Shutter Servo including interval control over parameter 8 lower 7 bits.
		377	// 091114 Inserted same modification into v.0.76g code, removing previously REM-ed out modified parts.
		378	// 20100802 Inserted same modification into v.0.76g code, removing previously REM-ed out modified parts.
		379	// Modified to use lower 7 bits of user parameter 8.
		380	// RemainingPulse += ((int16_t)Parameter_Servo3 * MULTIPLYER) - (256 / 2) * MULTIPLYER;
		381	// break;
		382
		383
		384	if(PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] < -32)
		385	{
		386	// Set servo to null position, turning camera off.
		387	RemainingPulse = MINSERVOPULSE;
		388	}
		389	else
		390	{
		391	// 090807: Arthur P.: Removed the shutter cycle parts as they may be impacting timing loops.
		392	// 20100804 Reactived shutter interval timer capability.
		393	if(PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] > 32)
		394	// Top position on a 3 position switch which runs from -127 to +127
		395	{
		396
		397	// Changed so that on full up position full servo range out is given, while
		398	// on half position, have range is given. This will work better with electronic
		399	// switches.
		400	RemainingPulse = MINSERVOPULSE + SERVORANGE;
		401	}
		402	else
		403	{
		404	// Cycle shutter servo between on and off depending upon CameraShutterCycleCounter
		405	// If CameraShutterCylce < 50 then default to continuous / manual shoot.
		406	if(CameraShutterCycle < 50 )
		407	{
		408	RemainingPulse = MINSERVOPULSE + SERVORANGE/2;
		409	}
		410	else
		411	{
		412	if(CameraShutterCycleCounter == CameraShutterCycle)
		413	{
		414	// Shutter on
		415	CameraShutterCycleCounter = 0;
		416	RemainingPulse = MINSERVOPULSE + SERVORANGE/2;
		417	}
		418	else
		419	{
		420	// Leave on for at least 20 cycles or 0.20 seconds to allow
		421	// the camera to properly trigger, turn off if past 0.2 sec.
		422	CameraShutterCycleCounter++;
		423	if(CameraShutterCycleCounter == CameraShutterCycleOnCount)
		424	{
		425	// Shutter off
		426	RemainingPulse = MINSERVOPULSE;
		427	}
		428	}
		429	}
		430	}
		431	}
		432	break;
		433	case 4:
		434	RemainingPulse += ((int16_t)Parameter_Servo4 * MULTIPLYER) - (256 / 2) * MULTIPLYER;
		435	break;
		436	case 5:
		437	RemainingPulse += ((int16_t)Parameter_Servo5 * MULTIPLYER) - (256 / 2) * MULTIPLYER;
		438	break;
		439	default: // other servo channels
		440	RemainingPulse += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2�s
		441	break;
		442	}
		443	// range servo pulse width
		444	if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit
		445	else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit
		446	// substract stop pulse width
		447	RemainingPulse -= PPM_STOPPULSE;
		448	// accumulate time for correct sync gap
		449	ServoFrameTime += RemainingPulse;
		450	}
		451	}
		452	else // we had a high pulse
		453	{
		454	TCCR2A \|= (1<<COM2A0); // make a low pulse
		455	// set pulsewidth to stop pulse width
		456	RemainingPulse = PPM_STOPPULSE;
		457	// accumulate time for correct sync gap
		458	ServoFrameTime += RemainingPulse;
		459	if((ServoActive && SenderOkay > 180) \|\| ServoActive == 2) HEF4017R_OFF; // disable HEF4017 reset
		460	else HEF4017R_ON;
		461	ServoIndex++; // change to next servo channel
		462	if(ServoIndex > EE_Parameter.ServoNickRefresh) ServoIndex = 0; // reset to the sync gap
		463	}
		464	// set pulse output active
		465	PulseOutput = 1;
		466	}
		467	} // EOF PPM state machine
		468
		469	// General pulse output generator
		470	if(RemainingPulse > (255 + IRS_RUNTIME))
		471	{
		472	OCR2A = 255;
		473	RemainingPulse -= 255;
		474	}
		475	else
		476	{
		477	if(RemainingPulse > 255) // this is the 2nd last part
		478	{
		479	if((RemainingPulse - 255) < IRS_RUNTIME)
		480	{
		481	OCR2A = 255 - IRS_RUNTIME;
		482	RemainingPulse -= 255 - IRS_RUNTIME;
		483
		484	}
		485	else // last part > ISR_RUNTIME
		486	{
		487	OCR2A = 255;
		488	RemainingPulse -= 255;
		489	}
		490	}
		491	else // this is the last part
		492	{
		493	OCR2A = RemainingPulse;
		494	RemainingPulse = 0;
		495	PulseOutput = 0; // trigger to stop pulse
		496	}
		497	} // EOF general pulse output generator
		498	}

Subversion Repositories FlightCtrl

(root)/branches/V0.80d-Arthur-P/timer0.c @ 1821 - Rev 1754