WebSVN - FlightCtrl - Blame - /branches/V0.76g-Arthur P/timer0.c

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

Subversion Repositories FlightCtrl

(root)/branches/V0.76g-Arthur P/timer0.c @ 1388 - Rev