Ejemplo n.º 1
0
Archivo: lisl.c Proyecto: gke/UAVP
void LinearTest(void)
{

	nii = ReadLISL(LISL_STATUS + LISL_READ);
	SendComChar('S');
	SendComChar(':');
	SendComChar('0');
	SendComChar('x');
	SendComValH(nii);
	SendComCRLF();

	nilgval.high8 = (int)ReadLISL(LISL_OUTX_H + LISL_READ);
	nilgval.low8  = (int)ReadLISL(LISL_OUTX_L + LISL_READ);
	SendComChar('X');
	SendComChar(':');
	SendComValUL(NKS3+LEN5+VZ);
	SendComText(_SerLinG);

	nilgval.high8 = (int)ReadLISL(LISL_OUTZ_H + LISL_READ);
	nilgval.low8  = (int)ReadLISL(LISL_OUTZ_L + LISL_READ);
	SendComChar('Y');
	SendComChar(':');
	SendComValUL(NKS3+LEN5+VZ);
	SendComText(_SerLinG);

	nilgval.high8 = (int)ReadLISL(LISL_OUTY_H + LISL_READ);
	nilgval.low8  = (int)ReadLISL(LISL_OUTY_L + LISL_READ);
	SendComChar('Z');
	SendComChar(':');
	SendComValUL(NKS3+LEN5+VZ);
	SendComText(_SerLinG);
	
}
Ejemplo n.º 2
0
void ComputeBaroComp(void)
{
#ifdef NOT_PORTED_FOR18F  // use gregs code when ready

	if( ReadValueFromBaro() )	// returns niltemp as value
	{	// successful
		if( !_BaroTempRun )
		{	// current measurement was "pressure"
			if( ThrDownCount )	// while moving throttle stick
			{
				BasePressure = niltemp;	// current read value is the new level
				BaroCompSum = 0;
			}
			else
			{	// while holding altitude
				niltemp -= BasePressure;
//SendComChar('B');
// the uncorrected relative height
//				SendComValH(niltemp.high8);
//				SendComValH(niltemp.low8);
//				SendComValH(TempCorr.high8);
//				SendComValH(TempCorr.low8);
// niltemp1 has -400..+400 approx
				niltemp1 = TempCorr * BaroTempCoeff;
				niltemp1 += 16;
				niltemp1 /= 32;
				niltemp += niltemp1;	// compensating temp
// the corrected relative height, the higher alti, the lesser value

				// New Baro = (3*BaroSum + New_Baro)/4
				niltemp1 = niltemp;	// because of bank bits
				niltemp = BaroCompSum;	// remember old value for delta
				BaroCompSum *= 3;
				BaroCompSum += niltemp1;
				BaroCompSum += 2;	// rounding
				BaroCompSum >>= 2;	// div by 4
				niltemp1 = BaroCompSum - niltemp;	// subtract new height to get delta
					
#ifdef INTTEST
		SendComChar('a');
		SendComValH(BaroCompSum.high8);
		SendComValH(BaroCompSum.low8);	// current height
		SendComChar(';');
		SendComValH(TempCorr.high8);
		SendComValH(TempCorr.low8);	// current temp
		SendComChar(';');
		SendComValH(niltemp1.low8);		// delta height
		SendComChar(';');
		
#endif
// was: +10 and -5
				if( BaroCompSum > 8 ) // zu tief: ordentlich Gas geben
					BaroCompSum = 8;
				if( BaroCompSum < -3 ) // zu hoch: nur leicht nachlassen
					BaroCompSum = -3;
			// weiche Regelung (proportional)
			// nitemp kann nicht überlaufen (-3..+8 * PropFact)
				nitemp = (int)BaroCompSum.low8 * BaroThrottleProp;
				if( VBaroComp > nitemp )
					VBaroComp--;
				else
				if( VBaroComp < nitemp )
					VBaroComp++;

				if( VBaroComp > nitemp )
					VBaroComp--;
				else
				if( VBaroComp < nitemp )
					VBaroComp++;
			// Differentialanteil
				if( niltemp1 > 8 )
					niltemp1.low8 = 8;
				else
				if( niltemp1 < -8 )
					niltemp1.low8 = -8;
					
				nitemp = (int)niltemp1.low8 * BaroThrottleDiff;
				VBaroComp += nitemp;
				if( VBaroComp > 15 )
					VBaroComp = 15;
				if( VBaroComp < -5 )
					VBaroComp = -5;
					
#ifdef INTTEST
		SendComValH(VBaroComp);
		SendComChar(0x0d);
		SendComChar(0x0a);
#endif

			}
			StartBaroADC(0xee);	// next is temp
		}
		else
		{
			if( ThrDownCount )
				BaseTemp = niltemp; // current read value
			else // TempCorr: The warmer, the higher
			{
				TempCorr = niltemp - BaseTemp;
//				TempCorr += 4;	// compensate rounding error later /8
			}
			StartBaroADC(0xf4);	// next is pressure
		}
	}
Ejemplo n.º 3
0
// Calc the gyro values from added RollSamples 
// and NickSamples (global variable "nisampcnt")
void CalcGyroValues(void)
{
// RollSamples & Nicksamples hold the sum of 2 consecutive conversions
	RollSamples ++;	// for a correct round-up
	NickSamples ++;

#ifdef OPT_ADXRS150
	(long)RollSamples >>= 2;	// recreate the 10 bit resolution
	(long)NickSamples >>= 2;
#else
	(long)RollSamples >>= 1;	// recreate the 10 bit resolution
	(long)NickSamples >>= 1;
#endif

	if( IntegralCount > 0 )
	{
// pre-flight auto-zero mode
		RollSum += RollSamples;
		NickSum += NickSamples;

		if( IntegralCount == 1 )
		{
			RollSum += 8;
			NickSum += 8;
			if( !_UseLISL )
			{
				niltemp = RollSum + MiddleLR;
				RollSum = niltemp;
				niltemp = NickSum + MiddleFB;
				NickSum = niltemp;
			}
			MidRoll = (uns16)RollSum / (uns16)16;	
			MidNick = (uns16)NickSum / (uns16)16;
			RollSum = 0;
			NickSum = 0;
			LRIntKorr = 0;
			FBIntKorr = 0;
		}
	}
	else
	{
// standard flight mode
		RollSamples -= MidRoll;
		NickSamples -= MidNick;

// calc Cross flying mode
		if( FlyCrossMode )
		{
// Real Roll = 0.707 * (N + R)
//      Nick = 0.707 * (N - R)
// the constant factor 0.667 is used instead
			niltemp = RollSamples + NickSamples;	// 12 valid bits!
			NickSamples = NickSamples - RollSamples;	// 12 valid bits!
			RollSamples = niltemp * 2;
			(long)RollSamples /= 3;
			(long)NickSamples *= 2;
			(long)NickSamples /= 3;
		
		}
#ifdef DEBUG_SENSORS
		SendComValH(RollSamples.high8);
		SendComValH(RollSamples.low8);
		SendComChar(';');
		SendComValH(NickSamples.high8);
		SendComValH(NickSamples.low8);
		SendComChar(';');
#endif
	
// Roll
		niltemp = RollSamples;

#ifdef OPT_ADXRS
		RollSamples += 2;
		(long)RollSamples >>= 2;
#endif
#ifdef OPT_IDG
		RollSamples += 1;
		(long)RollSamples >>= 1;
#endif
		RE = RollSamples.low8;	// use 8 bit res. for PD controller

#ifdef OPT_ADXRS
		RollSamples = niltemp + 1;
		(long)RollSamples >>= 1;	// use 9 bit res. for I controller
#endif
#ifdef OPT_IDG
		RollSamples = niltemp;
#endif
		LimitRollSum();		// for roll integration

// Nick
		niltemp = NickSamples;

#ifdef OPT_ADXRS
		NickSamples += 2;
		(long)NickSamples >>= 2;
#endif
#ifdef OPT_IDG
		NickSamples += 1;
		(long)NickSamples >>= 1;
#endif
		NE = NickSamples.low8;

#ifdef OPT_ADXRS
		NickSamples = niltemp + 1;
		(long)NickSamples >>= 1;
#endif
#ifdef OPT_IDG
		NickSamples = niltemp;
#endif
		LimitNickSum();		// for nick integration

// Yaw is sampled only once every frame, 8 bit A/D resolution
		ADFM = 0;
		ADCON0 = 0b.10.100.0.0.1;	// select CH4(RA5) Yaw
		AcqTime();
		TE = ADRESH;
		if( MidTurn == 0 )
			MidTurn = TE;
		TE -= MidTurn;

		LimitYawSum();
#ifdef DEBUG_SENSORS
		SendComValH(TE);
		SendComChar(';');
		SendComValH(RollSum.high8);
		SendComValH(RollSum.low8);
		SendComChar(';');
		SendComValH(NickSum.high8);
		SendComValH(NickSum.low8);
		SendComChar(';');
		SendComValH(YawSum.high8);
		SendComValH(YawSum.low8);
		SendComChar(';');
#endif
	}
}
Ejemplo n.º 4
0
void OutSignals(void)
{
	bank0 uns8 MV, MH, ML, MR;	// must reside on bank0
	uns8 MT@MV;	// cam tilt servo
	uns8 ME@MH; // cam tilt servo


#ifdef NADA
SendComValH(MCamRoll);
SendComValH(MCamNick);
SendComChar(0x0d);
SendComChar(0x0a);
#endif

#ifndef DEBUG_SENSORS

#ifdef DEBUG_MOTORS
	if( _Flying && CamNickFactor.4 )
	{
		SendComValU(IGas);
		SendComChar(';');
		SendComValS(IRoll);
		SendComChar(';');
		SendComValS(INick);
		SendComChar(';');
		SendComValS(ITurn);
		SendComChar(';');
		SendComValU(MVorne);
		SendComChar(';');
		SendComValU(MHinten);
		SendComChar(';');
		SendComValU(MLinks);
		SendComChar(';');
		SendComValU(MRechts);
		SendComChar(0x0d);
		SendComChar(0x0a);
	}
#endif

	TMR0 = 0;
	T0IF = 0;

#ifdef ESC_PPM
	ALL_PULSE_ON;	// turn on all motor outputs
#endif

	MV = MVorne;
	MH = MHinten;
	ML = MLinks;
	MR = MRechts;

#ifdef DEBUG_MOTORS
// if DEBUG_MOTORS is active, CamIntFactor is a bitmap:
// bit 0 = no front motor
// bit 1 = no rear motor
// bit 2 = no left motor
// bit 3 = no right motor
// bit 4 = turns on the serial output

	if( CamNickFactor.0 )
		MV = _Minimum;
	if( CamNickFactor.1 )
		MH = _Minimum;
	if( CamNickFactor.2 )
		ML = _Minimum;
	if( CamNickFactor.3 )
		MR = _Minimum;
#else
#ifdef INTTEST
	MV = _Minimum;
	MH = _Minimum;
	ML = _Minimum;
	MR = _Minimum;
#endif
#endif

#ifdef ESC_PPM

// simply wait for nearly 1 ms
// irq service time is max 256 cycles = 64us = 16 TMR0 ticks
	while( TMR0 < 0x100-3-16 ) ;

	// now stop CCP1 interrupt
	// capture can survive 1ms without service!

	// Strictly only if the masked interrupt region below is
	// less than the minimum valid Rx pulse/gap width which
	// is 1027uS less capture time overheads

	GIE = 0;	// BLOCK ALL INTERRUPTS for NO MORE than 1mS
	while( T0IF == 0 ) ;	// wait for first overflow
	T0IF=0;		// quit TMR0 interrupt

#if !defined DEBUG && !defined DEBUG_MOTORS
	if( _OutToggle )	// driver cam servos only every 2nd pulse
	{
		CAM_PULSE_ON;	// now turn camera servo pulses on too
	}
	_OutToggle ^= 1;
#endif

// This loop is exactly 16 cycles long
// under no circumstances should the loop cycle time be changed
#asm
	BCF	RP0		// clear all bank bits
//	BCF	RP1
OS005
	MOVF	PORTB,W
	ANDLW	0x0F		// output ports 0 to 3
	BTFSC	Zero_
	GOTO	OS006		// stop if all 4 outputs are done

	DECFSZ	MV,f		// front motor
	GOTO	OS007

	BCF	PulseVorne		// stop pulse
OS007
	DECFSZ	ML,f		// left motor
	GOTO	OS008

	BCF	PulseLinks		// stop pulse
OS008
	DECFSZ	MR,f		// right motor
	GOTO	OS009

	BCF	PulseRechts		// stop pulse
OS009
	DECFSZ	MH,f		// rear motor
	GOTO	OS005
	
	BCF	PulseHinten		// stop pulse
	GOTO	OS005
OS006
#endasm
// This will be the corresponding C code:
//	while( ALL_OUTPUTS != 0 )
//	{	// remain in loop as long as any output is still high
//		if( TMR2 = MVorne  ) PulseVorne  = 0;
//		if( TMR2 = MHinten ) PulseHinten = 0;
//		if( TMR2 = MLinks  ) PulseLinks  = 0;
//		if( TMR2 = MRechts ) PulseRechts = 0;
//	}

	GIE = 1;	// Re-enable interrupt

#endif	// ESC_PPM

#if defined ESC_X3D || defined ESC_HOLGER || defined ESC_YGEI2C

#if !defined DEBUG && !defined DEBUG_MOTORS
	if( _OutToggle )	// driver cam servos only every 2nd pulse
	{
		CAM_PULSE_ON;	// now turn camera servo pulses on too
	}
	_OutToggle ^= 1;
#endif

// in X3D- and Holger-Mode, K2 (left motor) is SDA, K3 (right) is SCL
#ifdef ESC_X3D
	EscI2CStart();
	SendEscI2CByte(0x10);	// one command, 4 data bytes
	SendEscI2CByte(MV); // for all motors
	SendEscI2CByte(MH);
	SendEscI2CByte(ML);
	SendEscI2CByte(MR);
	EscI2CStop();
#endif	// ESC_X3D

#ifdef ESC_HOLGER
	EscI2CStart();
	SendEscI2CByte(0x52);	// one cmd, one data byte per motor
	SendEscI2CByte(MV); // for all motors
	EscI2CStop();

	EscI2CStart();
	SendEscI2CByte(0x54);
	SendEscI2CByte(MH);
	EscI2CStop();

	EscI2CStart();
	SendEscI2CByte(0x58);
	SendEscI2CByte(ML);
	EscI2CStop();

	EscI2CStart();
	SendEscI2CByte(0x56);
	SendEscI2CByte(MR);
	EscI2CStop();
#endif	// ESC_HOLGER

#ifdef ESC_YGEI2C
	EscI2CStart();
	SendEscI2CByte(0x62);	// one cmd, one data byte per motor
	SendEscI2CByte(MV>>1); // for all motors
	EscI2CStop();

	EscI2CStart();
	SendEscI2CByte(0x64);
	SendEscI2CByte(MH>>1);
	EscI2CStop();

	EscI2CStart();
	SendEscI2CByte(0x68);
	SendEscI2CByte(ML>>1);
	EscI2CStop();

	EscI2CStart();
	SendEscI2CByte(0x66);
	SendEscI2CByte(MR>>1);
	EscI2CStop();
#endif	// ESC_YGEI2C

#endif	// ESC_X3D or ESC_HOLGER or ESC_YGEI2C

#ifndef DEBUG_MOTORS
	while( TMR0 < 0x100-3-16 ) ; // wait for 2nd TMR0 near overflow

	GIE = 0;					// Int wieder sperren, wegen Jitter

	while( T0IF == 0 ) ;	// wait for 2nd overflow (2 ms)

	// avoid servo overrun when MCamxx == 0
	ME = MCamRoll+1;
	MT = MCamNick+1;

#if !defined DEBUG && !defined DEBUG_SENSORS
// This loop is exactly 16 cycles long
// under no circumstances should the loop cycle time be changed
#asm
	BCF	RP0		// clear all bank bits
	BCF	RP1
OS001
	MOVF	PORTB,W
	ANDLW	0x30		// output ports 4 and 5
	BTFSC	Zero_
	GOTO	OS002		// stop if all 2 outputs are 0

	DECFSZ	MT,f
	GOTO	OS003

	BCF	PulseCamRoll
OS003
	DECFSZ	ME,f
	GOTO	OS004

	BCF	PulseCamNick
OS004
#endasm
	nop2();
	nop2();
#asm
	GOTO	OS001
OS002
#endasm
#endif	// DEBUG
	GIE = 1;	// re-enable interrupt

	while( T0IF == 0 ) ;	// wait for 3rd TMR2 overflow
#endif	// DEBUG_MOTORS

#endif  // !DEBUG_SENSORS
}
Ejemplo n.º 5
0
Archivo: accel.c Proyecto: gke/UAVP
void CheckLISL(void)
{
	int16 nila1@nilarg1;

	if( _UseLISL )
	{
		ReadLISL(LISL_STATUS + LISL_READ);
		// the LISL registers are in order here!!
		Rp.low8  = (int8)ReadLISL(LISL_OUTX_L + LISL_INCR_ADDR + LISL_READ);
		Rp.high8 = (int8)ReadLISLNext();
		Yp.low8  = (int8)ReadLISLNext();
		Yp.high8 = (int8)ReadLISLNext();
		Pp.low8  = (int8)ReadLISLNext();
		Pp.high8 = (int8)ReadLISLNext();
		LISL_CS = 1;	// end transmission
			
		// NeutralLR ,NeutralFB, NeutralUD pass through UAVPSet 
		// and come back as MiddleLR etc.
	
		// 1 unit is 1/4096 of 2g = 1/2048g
		Rp -= MiddleLR;
		Pp -= MiddleFB;
		Yp -= MiddleUD;
		
		Yp -= 1024;	// subtract 1g

		#ifdef DEBUG_SENSORS
		if( IntegralCount == 0 )
		{
			SendComValH(Rp.high8);
			SendComValH(Rp.low8);
			SendComChar(';');
			SendComValH(Pp.high8);
			SendComValH(Pp.low8);
			SendComChar(';');
			SendComValH(Yp.high8);
			SendComValH(Yp.low8);
			SendComChar(';');
		}
		#endif
	
		// DO NOT USE - Requires roll & pitch angle compensation
		#ifdef NADA
		// UDSum rises if ufo climbs
		UDSum += Yp - RollPitchComp;
	
		Yp = UDSum;
		Yp += 8;
		Yp >>= 4;
		Yp *= LinUDIntFactor;
		Yp += 128;
	
		if( (BlinkCount & 0x03) == 0 )	
		{
			if( (int8)Yp.high8 > Vud ) 
				Vud++;
			else
				if( (int8)Yp.high8 < Vud )
					Vud--;
	
			if( Vud >  10 ) // was 20
				Vud =  10; 
			else
				if( Vud < -10 ) 
					Vud = -10;
		}
		if( UDSum >  10 ) 
			UDSum -= 10;
		else
			if( UDSum < -10 ) 
				UDSum += 10;
		#endif // ACCEL_VUD
	
		// =====================================
		// Roll-Axis
		// =====================================
		// Static compensation due to Gravity
		Yl = RollSum * GRAV_COMP;	
		Yl += 16;
		Yl >>= 5;
		Rp -= Yl;
	
		// dynamic correction of moved mass
		Rp += (int16)RollSamples;
		Rp += (int16)RollSamples;
	
		// correct DC level of the integral
		LRIntKorr = 0;
		if ( Rp > 10 )
			LRIntKorr++;
		else
			if ( Rp < -10 )
				LRIntKorr--;
	
		// =====================================
		// Pitch-Axis
		// =====================================
		// Static compensation due to Gravity
		Yl = PitchSum * GRAV_COMP;
		Yl += 16;
		Yl >>= 5;
		Pp -= Yl;

		// dynamic correction of moved mass
		Pp += (int16)PitchSamples;
		Pp += (int16)PitchSamples;

		// correct DC level of the integral
		FBIntKorr = 0;
		if ( Pp > 10 )
			FBIntKorr++;
		else
			if ( Pp < -10 )
				FBIntKorr--;

	}
	else
	{