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);
}
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
}
}
// 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
}
}
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
}
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
{