void SetAlpha(ALPHA Alpha)
{
InitArrays();
SetGapChar('.');
SetGapChar('-');
switch (Alpha)
{
case ALPHA_Amino:
SetAlphaAmino();
break;
case ALPHA_DNA:
SetAlphaDNA();
case ALPHA_RNA:
SetAlphaRNA();
break;
default:
Quit("Invalid Alpha=%d", Alpha);
}
g_AlphaSize = GetAlphaSize(Alpha);
g_Alpha = Alpha;
if (g_bVerbose)
Log("Alphabet %s\n", ALPHAToStr(g_Alpha));
}
LsysGa::LsysGa(int nPopSize, int nMutants, int nOffspring)
{
m_pDrawWidget = NULL;
SetPopParams(nPopSize, nMutants, nOffspring);
InitArrays();
m_pImgCompare = NULL;
m_pTmpImage = NULL;
}
LsysGa::LsysGa(void)
{
m_pDrawWidget = NULL;
SetPopParams(50, 25, 150);
InitArrays();
m_pImgCompare = NULL;
m_pTmpImage = NULL;
}
// main program for transmitter controlled setup
void DoProgMode(void)
{
uns8 i;
int size1 *p;
ALL_LEDS_OFF;
InitArrays();
LedBlue_ON; // signal prog mode set #1
if( IK5 > _Neutral )
LedRed_ON; // set #2
Delaysec(50); // time to put throttle back to neutral
p = &FirstProgReg;
for(i = 1; p<=&LastProgReg; i++)
{
ShowMode(i, p);
p++;
}
ALL_LEDS_OFF;
// signal prog mode: 5 seconds blue flashing
for(i=0; i<50; i++)
{
Delaysec(1); // time 1/10 sec
LedBlue_TOG; // toggle LED
if( IK5 > _Neutral )
LedRed_TOG; // set #2
}
if( IGas > _ThresStart )
{
ALL_LEDS_ON; // = white color
Delaysec(10);
// prog values into data flash
EEPGD = 0; // access data eeprom
WREN = 1; // enable eeprom writes
EEADR = _EESet1; // start with address 0
if( IK5 > _Neutral )
EEADR = _EESet2; // set #2
for(p = &FirstProgReg; p <= &LastProgReg; p++)
{
EEDATA = *p; // the data byte
GIE = 0;
EECON2 = 0x55; // fix prog sequence (see 16F628A datasheet)
EECON2 = 0xAA;
WR = 1; // start write cycle
GIE = 1;
while( WR == 1 ); // wait to complete
EEADR++;
}
WREN = 0; // disable eeprom writes
}
ALL_LEDS_OFF;
}
void MemoryHandler::Allocate(unsigned int _vertexsize, unsigned int _indexsize)
{
vertexArray = new GLfloat[_vertexsize];
vertexsize = _vertexsize;
Debug::Dvalue("", vertexsize);
indexArray = new GLuint[_indexsize];
indexsize = _indexsize;
Debug::Dvalue("", indexsize);
InitArrays();
}
void PRTHierarchyMapping::GetMapping(
Mesh* renderMesh, Mesh* approxMesh,
int* mappingIndices, float* mappingWeights)
{
mTree = new K3Tree(renderMesh, approxMesh);
mTree->FillTreeWithData();
bool debug = false;
InitArrays();
for(int i = 0; i < renderMesh->GetNumVertices(); ++i) {
Vertex* vertices = renderMesh->GetVertices();
Vertex vertex = vertices[i];
if(i == 12314 || i == 12272 || i == 12266) {
PD(L"mapping info for vertex ", i);
PD(L"Blue, Point[{", vertex.pos.x);
PD(L",", vertex.pos.y);
PD(L",", vertex.pos.z);
PD(L"}]");
mTree->SetDebug(true);
debug = true;
}
else{
mTree->SetDebug(false);
debug = false;
}
mTree->GetMapping(vertex, indices, weights);
float sumOfWeights = 0;
for(int k = 0; k < 3; ++k) {
if(debug) {
PD(L"nn: ", k);
PD(L"index :", indices[k]);
PD(L"weights :", weights[k]);
}
mappingIndices[(i*3) + k] = indices[k];
mappingWeights[(i*3) + k] = weights[k];
sumOfWeights += weights[k];
}
if(sumOfWeights > 1.001f || sumOfWeights < 0.999f) {
PD(L"sum of weights not 1");
}
}
FreeArrays();
}
static void FillSlab()
{
SlabAlloc1(uint64, AttN, 64);
SlabAlloc1(uint64, AttK, 64);
SlabAlloc1(uint64, AttPw, 64);
SlabAlloc1(uint64, AttPb, 64);
SlabAlloc1(typeMM, RookMM, 64);
SlabAlloc1(typeMM, BishopMM, 64);
SlabAlloc2(uint64, MMOrtho, 102400);
SlabAlloc2(uint64, MMDiag, 5248);
SlabAlloc1(uint64, SqSet, 64);
SlabAlloc1(uint64, SqClear, 64);
SlabAlloc1(uint64, NonDiag, 64);
SlabAlloc1(uint64, NonOrtho, 64);
SlabAlloc1(uint64, Ortho, 64);
SlabAlloc1(uint64, Diag, 64);
SlabAlloc1(uint64, OrthoDiag, 64);
SlabAlloc1(uint64, OpenFileW, 64);
SlabAlloc1(uint64, OpenFileB, 64);
SlabAlloc1(uint64, PassedPawnW, 64);
SlabAlloc1(uint64, PassedPawnB, 64);
SlabAlloc1(uint64, ProtectedPawnW, 64);
SlabAlloc1(uint64, ProtectedPawnB, 64);
SlabAlloc1(uint64, IsolatedPawnW, 64);
SlabAlloc1(uint64, IsolatedPawnB, 64);
SlabAlloc1(uint64, ConnectedPawns, 64);
SlabAlloc1(uint64, InFrontW, 8);
SlabAlloc1(uint64, NotInFrontW, 8);
SlabAlloc1(uint64, InFrontB, 8);
SlabAlloc1(uint64, NotInFrontB, 8);
SlabAlloc1(uint64, IsolatedFiles, 8);
SlabAlloc1(uint64, FilesLeft, 8);
SlabAlloc1(uint64, FilesRight, 8);
SlabAlloc1(uint64, Doubled, 64);
SlabAlloc1(uint64, Left2, 64);
SlabAlloc1(uint64, Right2, 64);
SlabAlloc1(uint64, Left1, 64);
SlabAlloc1(uint64, Right1, 64);
SlabAlloc1(uint64, Adjacent, 64);
SlabAlloc1(uint64, LongDiag, 64);
SlabAlloc1(uint64, Northwest, 64);
SlabAlloc1(uint64, Southwest, 64);
SlabAlloc1(uint64, Northeast, 64);
SlabAlloc1(uint64, Souteast, 64);
SlabAlloc1(uint64, QuadrantWKwtm, 64);
SlabAlloc1(uint64, QuadrantBKwtm, 64);
SlabAlloc1(uint64, QuadrantWKbtm, 64);
SlabAlloc1(uint64, QuadrantBKbtm, 64);
SlabAlloc1(uint64, ShepherdWK, 64);
SlabAlloc1(uint64, ShepherdBK, 64);
SlabAlloc3(uint64, Interpose, 0100 * 0100);
SlabAlloc3(uint64, Evade, 0100 * 0100);
SlabAlloc3(uint64, Zobrist, 0x10 * 0100);
SlabAlloc3(sint8, Line, 0100 * 0100);
SlabAlloc1(uint64, HashCastling, 16);
SlabAlloc1(uint64, HashEP, 8);
SlabAlloc1(uint64, HashRev, 16);
#ifdef MultiplePosGain
SlabAlloc2(sint16, MaxPositionalGain, MaxCPUs * 0x10 * 010000);
#else
SlabAlloc2(sint16, MaxPositionalGain, 0x10 * 010000);
#endif
#ifdef MultipleHistory
SlabAlloc2(uint16, History, MaxCPUs * 0x10 * 0100);
#else
SlabAlloc2(uint16, History, 0x10 * 0100);
#endif
SlabAlloc2(sint32, PieceSquareValue, 0x10 * 0100);
SlabAlloc2(typeMaterial, Material, 419904);
ResetHistory();
ResetPositionalGain();
InitArrays();
InitMaterialValue();
InitStatic();
}
void FillSlab ()
{
SLAB_ALLOC1 (uint64, AttN, 64);
SLAB_ALLOC1 (uint64, AttK, 64);
SLAB_ALLOC1 (uint64, AttPw, 64);
SLAB_ALLOC1 (uint64, AttPb, 64);
#ifndef MAGIC_BITBOARDS
SLAB_ALLOC3 (uint64, LineMask, 4 * 0100 * 0100);
SLAB_ALLOC3 (int, LineShift, 4 * 0100);
SLAB_ALLOC1 (uint64, ClearL90, 64);
SLAB_ALLOC1 (uint64, ClearL45, 64);
SLAB_ALLOC1 (uint64, ClearR45, 64);
SLAB_ALLOC1 (uint64, SetL90, 64);
SLAB_ALLOC1 (uint64, SetL45, 64);
SLAB_ALLOC1 (uint64, SetR45, 64);
#else
SLAB_ALLOC1 (type_MM, ROOK_MM, 64);
SLAB_ALLOC1 (type_MM, BISHOP_MM, 64);
SLAB_ALLOC2 (uint64, MM_ORTHO, 102400); /* SLAB 800k */
SLAB_ALLOC2 (uint64, MM_DIAG, 5248);
#endif
SLAB_ALLOC1 (uint64, SqSet, 64);
SLAB_ALLOC1 (uint64, SqClear, 64);
SLAB_ALLOC1 (uint64, NON_DIAG, 64);
SLAB_ALLOC1 (uint64, NON_ORTHO, 64);
SLAB_ALLOC1 (uint64, ORTHO, 64);
SLAB_ALLOC1 (uint64, DIAG, 64);
SLAB_ALLOC1 (uint64, ORTHO_DIAG, 64);
SLAB_ALLOC1 (uint64, OpenFileW, 64);
SLAB_ALLOC1 (uint64, OpenFileB, 64);
SLAB_ALLOC1 (uint64, PassedPawnW, 64);
SLAB_ALLOC1 (uint64, PassedPawnB, 64);
SLAB_ALLOC1 (uint64, ProtectedPawnW, 64);
SLAB_ALLOC1 (uint64, ProtectedPawnB, 64);
SLAB_ALLOC1 (uint64, IsolatedPawnW, 64);
SLAB_ALLOC1 (uint64, IsolatedPawnB, 64);
SLAB_ALLOC1 (uint64, ConnectedPawns, 64);
SLAB_ALLOC1 (uint64, InFrontW, 8);
SLAB_ALLOC1 (uint64, NotInFrontW, 8);
SLAB_ALLOC1 (uint64, InFrontB, 8);
SLAB_ALLOC1 (uint64, NotInFrontB, 8);
SLAB_ALLOC1 (uint64, IsolatedFiles, 8);
SLAB_ALLOC1 (uint64, FilesLeft, 8);
SLAB_ALLOC1 (uint64, FilesRight, 8);
SLAB_ALLOC1 (uint64, DOUBLED, 64);
SLAB_ALLOC1 (uint64, LEFT2, 64);
SLAB_ALLOC1 (uint64, RIGHT2, 64);
SLAB_ALLOC1 (uint64, LEFT1, 64);
SLAB_ALLOC1 (uint64, RIGHT1, 64);
SLAB_ALLOC1 (uint64, ADJACENT, 64);
SLAB_ALLOC1 (uint64, LONG_DIAG, 64);
SLAB_ALLOC1 (uint64, NORTHWEST, 64);
SLAB_ALLOC1 (uint64, SOUTHWEST, 64);
SLAB_ALLOC1 (uint64, NORTHEAST, 64);
SLAB_ALLOC1 (uint64, SOUTHEAST, 64);
SLAB_ALLOC1 (uint64, QuadrantWKwtm, 64);
SLAB_ALLOC1 (uint64, QuadrantBKwtm, 64);
SLAB_ALLOC1 (uint64, QuadrantWKbtm, 64);
SLAB_ALLOC1 (uint64, QuadrantBKbtm, 64);
SLAB_ALLOC1 (uint64, ShepherdWK, 64);
SLAB_ALLOC1 (uint64, ShepherdBK, 64);
SLAB_ALLOC3 (uint64, INTERPOSE, 0100 * 0100);
SLAB_ALLOC3 (uint64, EVADE, 0100 * 0100);
SLAB_ALLOC3 (uint64, ZOBRIST, 0x10 * 0100);
SLAB_ALLOC3 (sint8, LINE, 0100 * 0100);
SLAB_ALLOC1 (uint64, ZobristCastling, 16);
SLAB_ALLOC1 (uint64, ZobristEP, 8);
SLAB_ALLOC1 (uint64, ZobristRev, 16);
#ifdef MULTIPLE_POS_GAIN
SLAB_ALLOC2 (sint16, MAX_POSITIONAL_GAIN, MAX_CPUS * 0x10 * 010000);
#else
SLAB_ALLOC2 (sint16, MAX_POSITIONAL_GAIN, 0x10 * 010000); /* SLAB 1mb */
#endif
#ifdef MULTIPLE_HISTORY
SLAB_ALLOC2 (uint16, HISTORY, MAX_CPUS * 0x10 * 0100); /* SLAB 64k */
#else
SLAB_ALLOC2 (uint16, HISTORY, 0x10 * 0100);
#endif
SLAB_ALLOC2 (sint32, PieceSquareValue, 0x10 * 0100); /* SMP read SLAB 16k */
SLAB_ALLOC2 (typeMATERIAL, MATERIAL, 419904); /* SMP read SLAB 1.68mb */
ResetHistory ();
ResetPositionalGain ();
InitArrays ();
InitMaterialValue ();
InitStatic ();
}
//===========================================================================
// DG_Init
// 'mode' is either DGL_MODE_WINDOW or DGL_MODE_FULLSCREEN. If 'bpp' is
// zero, the current display color depth is used.
//===========================================================================
int DG_Init(int width, int height, int bpp, int mode)
{
boolean fullscreen = (mode == DGL_MODE_FULLSCREEN);
char *token, *extbuf;
const SDL_VideoInfo *info = NULL;
Con_Message("DG_Init: OpenGL.\n");
// Are we in range here?
/* if(!fullscreen)
{
if(width > GetSystemMetrics(SM_CXSCREEN))
width = GetSystemMetrics(SM_CXSCREEN);
if(height > GetSystemMetrics(SM_CYSCREEN))
height = GetSystemMetrics(SM_CYSCREEN);
}
*/
info = SDL_GetVideoInfo();
screenBits = info->vfmt->BitsPerPixel;
screenWidth = width;
screenHeight = height;
windowed = !fullscreen;
allowCompression = true;
verbose = ArgExists("-verbose");
// Set GL attributes. We want at least 5 bits per color and a 16
// bit depth buffer. Plus double buffering, of course.
SDL_GL_SetAttribute(SDL_GL_RED_SIZE, 5);
SDL_GL_SetAttribute(SDL_GL_GREEN_SIZE, 5);
SDL_GL_SetAttribute(SDL_GL_BLUE_SIZE, 5);
SDL_GL_SetAttribute(SDL_GL_DEPTH_SIZE, 16);
SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER, 1);
/*
if(fullscreen)
{
if(!fullscreenMode(screenWidth, screenHeight, bpp))
{
Con_Error("drOpenGL.Init: Resolution change failed (%d x %d).\n",
screenWidth, screenHeight);
}
}
else
{
windowedMode(screenWidth, screenHeight);
}
*/
if(!initOpenGL())
{
Con_Error("drOpenGL.Init: OpenGL init failed.\n");
}
// Clear the buffers.
DG_Clear(DGL_COLOR_BUFFER_BIT | DGL_DEPTH_BUFFER_BIT);
token = (char *) glGetString(GL_EXTENSIONS);
extbuf = malloc(strlen(token) + 1);
strcpy(extbuf, token);
// Check the maximum texture size.
glGetIntegerv(GL_MAX_TEXTURE_SIZE, &maxTexSize);
initExtensions();
if(firstTimeInit)
{
firstTimeInit = DGL_FALSE;
// Print some OpenGL information (console must be initialized by now).
Con_Message("OpenGL information:\n");
Con_Message(" Vendor: %s\n", glGetString(GL_VENDOR));
Con_Message(" Renderer: %s\n", glGetString(GL_RENDERER));
Con_Message(" Version: %s\n", glGetString(GL_VERSION));
Con_Message(" Extensions:\n");
// Show the list of GL extensions.
token = strtok(extbuf, " ");
while(token)
{
Con_Message(" "); // Indent.
if(verbose)
{
// Show full names.
Con_Message("%s\n", token);
}
else
{
// Two on one line, clamp to 30 characters.
Con_Message("%-30.30s", token);
token = strtok(NULL, " ");
if(token)
Con_Message(" %-30.30s", token);
Con_Message("\n");
}
token = strtok(NULL, " ");
}
Con_Message(" GLU Version: %s\n", gluGetString(GLU_VERSION));
glGetIntegerv(GL_MAX_TEXTURE_UNITS, &maxTexUnits);
#ifndef USE_MULTITEXTURE
maxTexUnits = 1;
#endif
// But sir, we are simple people; two units is enough.
if(maxTexUnits > 2)
maxTexUnits = 2;
Con_Message(" Texture units: %i\n", maxTexUnits);
Con_Message(" Maximum texture size: %i\n", maxTexSize);
if(extAniso)
{
glGetFloatv(GL_MAX_TEXTURE_MAX_ANISOTROPY_EXT, &maxAniso);
Con_Message(" Maximum anisotropy: %g\n", maxAniso);
}
//if(!noArrays) Con_Message(" Using vertex arrays.\n");
}
free(extbuf);
// Decide whether vertex arrays should be done manually or with real
// OpenGL calls.
InitArrays();
if(ArgCheck("-dumptextures"))
{
dumpTextures = DGL_TRUE;
Con_Message(" Dumping textures (mipmap level zero).\n");
}
if(extAniso && ArgExists("-anifilter"))
{
useAnisotropic = DGL_TRUE;
Con_Message(" Using anisotropic texture filtering.\n");
}
return DGL_OK;
}
void main(void)
{
uns8 DropoutCount;
int nii@NegFact;
uns8 LowGasCount;
OPTION_REG = 0b.00000.011; // TMR0 w/ int clock, 1:16 presc (see _PreScale0),
// weak pullups on
// general ports setup
TRISA = 0b.00111111; // all inputs
ADCON1 = 0b.0.000.0010; // uses 5V as Vref
#ifdef BOARD_3_0
PORTB = 0b.1000.0000; // all outputs to low, except RB7 (LISL-CS)!
TRISB = 0b.0000.0000; // all servo and LED outputs
PORTC = 0b.0100.0000; // all outputs to low, except TxD
TRISC = 0b.10000100; // RC7, RC2 are inputs
#endif
#ifdef BOARD_3_1
PORTB = 0b.1100.0000; // all outputs to low, except RB6 & 7 (I2C)!
TRISB = 0b.0100.0000; // all servo and LED outputs
PORTC = 0b.0110.0000; // all outputs to low, except TxD and CS
TRISC = 0b.10000100; // RC7, RC2 are inputs
RBPU_ = 1; // enable weak pullups
#endif
#ifdef BOARD_3_1
LedShadow = 0;
#endif
ALL_LEDS_OFF;
// timer setup
T1CON = 0b.00.11.0001; // enable TMR1, 1:8 prescaler, run with 2us clock
CCP1CON = 0b.00.00.0100; // capture mode every falling edge
TMR2 = 0;
T2CON = 0b.0.1111.1.11; // enable TMR2, 1:16 prescaler, 1:16 postscaler (see _Pre/PostScale2)
PR2 = TMR2_5MS; // set compare reg to 9ms
// setup flags register
Flags = 0;
_NoSignal = 1; // assume no signal present
InitArrays();
LedRed_ON; // red LED on
Delaysec(1); // wait 1/10 sec until LISL is ready to talk
#ifdef USE_ACCSENS
IsLISLactive();
#ifdef ICD2_DEBUG
_UseLISL = 1; // because debugger uses RB7 (=LISL-CS) :-(
#endif
NeutralLR = 0;
NeutralFB = 0;
NeutralUD = 0;
if( _UseLISL )
GetEvenValues(); // into Rp, Np, Tp
#endif
// setup serial port for 8N1
TXSTA = 0b.0010.0100; // async mode, BRGH = 1
RCSTA = 0b.1001.0000; // receive mode
SPBRG = _B38400;
#ifdef BOARD_3_0
_SerEnabled = 1; // serial link is enabled
#endif
W = RCREG; // be sure to empty FIFO
// enable the interrupts
CCP1IE = 1;
TMR2IE = 1; // T1-Capture and T2 interrupt enable
PEIE = 1; // enable peripheral interrupts
// send hello text to serial COM
Delaysec(1); // just to see the output after powerup
ShowSetup(1);
IK6 = IK7 = _Neutral;
#ifdef BOARD_3_1
InitDirection(); // init compass sensor
#endif
Restart:
IGas =IK5 = _Minimum; // assume parameter set #1
// DON'T MOVE THE UFO!
// ES KANN LOSGEHEN!
while(1)
{
T0IE = 0; // disable TMR0 interrupt
ALL_LEDS_OFF;
LedRed_ON; // red LED on
if(_UseLISL)
LedYellow_ON; // to signal LISL sensor is active
InitArrays();
GIE = 1; // enable all interrupts
// Wait until a valid RX signal is received
DropoutCount = MODELLOSTTIMER;
do
{
Delaysec(2); // wait 2/10 sec until signal is there
ProcessComCommand();
if( _NoSignal )
{
if( Switch )
{
if( --DropoutCount == 0 )
{
Beeper_TOG; // toggle beeper "model lost"
DropoutCount = MODELLOSTTIMERINT;
}
}
else
Beeper_OFF;
}
}
while( _NoSignal || !Switch); // no signal or switch is off
// RX Signal is OK now
// Wait 2 sec to allow enter of prog mode
LedRed_OFF;
LedYellow_ON;
Delaysec(20);
LedYellow_OFF;
// die Variablen einlesen
ReadEEdata();
// check for prog mode (max. throttle)
if( IGas > _ProgMode )
{
DoProgMode();
goto Restart;
}
// Just for safety: don't let motors start if throttle is open!
// check if Gas is below _ThresStop
DropoutCount = 1;
while( IGas >= _ThresStop )
{
if( _NoSignal )
goto Restart;
if( _NewValues )
{
OutSignals();
_NewValues = 0;
if( --DropoutCount <= 0 )
{
LedRed_TOG; // toggle red Led
DropoutCount = 10; // to signal: THROTTLE OPEN
}
}
ProcessComCommand();
}
// ###############
// ## MAIN LOOP ##
// ###############
// loop length is controlled by a programmable variable "TimeSlot"
// which sets wait time in ms
// standard ESCs will need at least 9 or 10 as TimeSlot.
DropoutCount = 0;
BlinkCount = 0;
IntegralCount = 32; // do 32 cycles to find integral zero point
while(Switch == 1) // as long as power switch is ON
{
// wait pulse pause delay time (TMR0 has 1024us for one loop)
TMR0 = 0;
T0IF = 0;
T0IE = 1; // enable TMR0
RollSamples =
NickSamples = 0; // zero gyros sum-up memory
// sample gyro data and add everything up while waiting for timing delay
GetGyroValues();
#ifdef BOARD_3_1
if( (BlinkCount & 0x03) == 0 ) // enter every 4th scan
GetDirection(); // read compass sensor
#endif
while( TimeSlot > 0 )
{
// Here is the place to insert own routines
// It should consume as less time as possible!
// ATTENTION:
// Your routine must return BEFORE TimeSlot reaches 0
// or non-optimal flight behavior might occur!!!
}
T0IE = 0; // disable timer
GetGyroValues();
nisampcnt = 2; // 2 samples done
/* this section is no longer needed
if( nisampcnt & 0x01 ) // odd value?
{
nisampcnt++; // one more sample to get even numbers
GetGyroValues();
}
*/
ReadEEdata(); // re-sets TimeSlot
// Setup Blink counter
if( BlinkCount == 0 )
BlinkCount = BLINK_LIMIT;
BlinkCount--;
// get the gyro values and Batt status
CalcGyroValues();
GetVbattValue();
// check for signal dropout while in flight
if( _NoSignal && _Flying )
{
DropoutCount++;
if( DropoutCount < MAXDROPOUT )
{ // use last Gas
ALL_LEDS_OFF;
IRoll = 0;
INick = 0;
ITurn = 0;
goto DoPID;
}
break; // timeout, stop everything
}
// allow motors to run on low throttle
// even if stick is at minimum for a short time
if( _Flying && (IGas <= _ThresStop) )
{
if( --LowGasCount > 0 )
goto DoPID;
}
if( _NoSignal ||
( (_Flying && (IGas <= _ThresStop)) ||
(!_Flying && (IGas <= _ThresStart)) ) )
{ // UFO is landed, stop all motors
TimeSlot += 2; // to compensate PID() calc time!
IntegralCount = 32; // do 32 cycles to find integral zero point
InitArrays(); // resets _Flying flag!
MidRoll = 0; // gyro neutral points
MidNick = 0;
MidTurn = 0;
YawNeutral =
RollNeutral = // stick neutral points
NickNeutral = 0x7F;
if( _NoSignal && Switch ) // _NoSignal set, but Switch is on?
{
break; // then RX signal was lost
}
ALL_LEDS_OFF;
LedGreen_ON;
#ifdef BOARD_3_0
if( !_SerEnabled )
{
SPEN = 1; // enable RS232
CREN = 1;
_SerEnabled = 1;
}
#endif
ProcessComCommand();
}
else
{ // UFO is flying!
if( !_Flying ) // about to start
{ // set current gyro values as midpoints
if( RollNeutral == 0x7F )
RollNeutral = IRoll;
if( NickNeutral == 0x7F )
NickNeutral = INick;
if( YawNeutral == 0x7F )
YawNeutral = ITurn;
#ifdef BOARD_3_1
AbsDirection = COMPASS_INVAL;
#endif
#ifdef BOARD_3_0
// check if LISL Sensor is available
if( _UseLISL && _SerEnabled )
{
SPEN = 0; // disable RS232 to allow LISL sensor to work
CREN = 0;
TRISC.7 = 1; // RC7 is input (data)
TRISC.6 = 0; // RC6 is output (clock)
_SerEnabled = 0;
}
// if no LISL available, do COM commands also (important if you set ConfigParam wrong!)
if( _SerEnabled )
#endif
ProcessComCommand();
}
_Flying = 1;
DropoutCount = 0;
LowGasCount = 100;
#ifdef NADA // test
ALL_LEDS_OFF;
nii = IRoll - RollNeutral;
if( (nii > 5) || (nii < -5) )
LedRed_ON;
nii = INick - NickNeutral;
if( (nii > 5) || (nii < -5) )
LedYellow_ON;
nii = ITurn - YawNeutral;
if( (nii > 5) || (nii < -5) )
LedBlue_ON;
#endif
// LED game
#ifndef NOLEDGAME
// do the LED game :-)
ALL_LEDS_OFF;
#define LED_SHOW_LIM 2
if( !_LowBatt )
{
nii = IRoll-RollNeutral;
if( nii > LED_SHOW_LIM )
{
LedRed_ON;
}
if( nii < -LED_SHOW_LIM )
{
LedGreen_ON;
}
x = INick-NickNeutral;
if( nii > LED_SHOW_LIM )
{
LedBlue_ON;
}
if( nii < -LED_SHOW_LIM )
{
LedYellow_ON;
}
if( ARE_ALL_LEDS_OFF )
ALL_LEDS_ON;
}
#endif
DoPID:
// do the calculations
Rp = 0;
Np = 0;
if( IntegralCount > 0 )
IntegralCount--;
else
{
PID();
MixAndLimit();
}
// remember old gyro values
REp = RE;
NEp = NE;
TEp = TE;
}
if( _LowBatt )
{
if( BlinkCount < BLINK_LIMIT/2 )
{
Beeper_OFF;
LedRed_OFF;
}
else
{
Beeper_ON;
LedRed_ON;
}
}
// Output the results to the speed controllers
MixAndLimitCam();
OutSignals();
} // END NORMAL OPERATION WHILE LOOP
// CPU kommt hierher wenn der Schalter ausgeschaltet wird
Beeper_OFF;
#ifdef BOARD_3_0
SPEN = 1; // enable RS232
CREN = 1;
_SerEnabled = 1;
#endif
}
ALL_OUTPUTS_OFF;
}
void main(void)
{
int8 i;
DisableInterrupts;
InitPorts();
InitADC();
OpenUSART(USART_TX_INT_OFF&USART_RX_INT_OFF&USART_ASYNCH_MODE&
USART_EIGHT_BIT&USART_CONT_RX&USART_BRGH_HIGH, _B38400);
OpenTimer0(TIMER_INT_OFF&T0_8BIT&T0_SOURCE_INT&T0_PS_1_16);
OpenTimer1(T1_8BIT_RW&TIMER_INT_OFF&T1_PS_1_8&T1_SYNC_EXT_ON&T1_SOURCE_CCP&T1_SOURCE_INT);
OpenCapture1(CAPTURE_INT_ON & C1_EVERY_FALL_EDGE); // capture mode every falling edge
CCP1CONbits.CCP1M0 = NegativePPM;
OpenTimer2(TIMER_INT_ON&T2_PS_1_16&T2_POST_1_16);
PR2 = TMR2_5MS; // set compare reg to 9ms
INTCONbits.TMR0IE = false;
// setup flags register
for ( i = 0; i<16; i++ )
Flags[i] = false;
_NoSignal = true;
InitArrays();
ReadParametersEE();
ConfigParam = 0;
ALL_LEDS_OFF;
Beeper_OFF;
LedBlue_ON;
INTCONbits.PEIE = true; // enable peripheral interrupts
EnableInterrupts;
LedRed_ON; // red LED on
Delay1mS(100);
IsLISLactive();
#ifdef ICD2_DEBUG
_UseLISL = 1; // because debugger uses RB7 (=LISL-CS) :-(
#endif
NewK1 = NewK2 = NewK3 = NewK4 =NewK5 = NewK6 = NewK7 = 0xFFFF;
PauseTime = 0;
InitBarometer();
InitDirection();
Delay1mS(COMPASS_TIME);
// send hello text to serial COM
Delay1mS(100);
ShowSetup(true);
Delay1mS(BARO_PRESS_TIME);
while(1)
{
// turn red LED on of signal missing or invalid, green if OK
// Yellow led to indicate linear sensor functioning.
if( _NoSignal || !Switch )
{
LedRed_ON;
LedGreen_OFF;
if ( _UseLISL )
LedYellow_ON;
}
else
{
LedGreen_ON;
LedRed_OFF;
LedYellow_OFF;
}
ProcessComCommand();
}
} // main
/****************************************************************************
[NAME]
SLOW\_ART
[SYNOPSIS]
Image *SLOW_ART(Vector *xvector,
Vector *bvector)
[DESCRIPTION]
This function will iterate towards a solution for the sparse system of
equations \mb{b}=\mb{A x}, where \mb{b} is the sinogram (Radon domain)
and \mb{x} is the reconstructed image to be found. The function uses a
slow version of ART (Algebraric Reconstruction Techniques) where the
transformation matrix is calculated on the fly.
[USAGE]
{\tt Image=ART(TestMatrix, TestSinogram);}
Reconstructs the sinogram {\tt TestSinogram}, returns it as an image.
[REVISION]
Jan. 95, JJJ and PT
July 06, J.Schulz, Modification of ReadRefImage and the condition to
SaveIterions
****************************************************************************/
Image *SLOW_ART(Vector *xvector, Vector *bvector)
{
int ARows,ACols,currentrow,currentiteration,TotalIterations,AntPrint,UseRefImage;
float denom,lambda,brk;
//refxdev=0.0;
float *tempXv,*tempBv;
char DiffFileName[200];
FILE *DiffFile=NULL;
Vector *refxvector=NULL,*AVector;
Image *Recon,*RefImage=NULL;
ARows=bvector->N;
ACols=xvector->N;
Print(_DNormal,"Using ART (slow) to solve %i equations with %i unknowns\n",
ARows,ACols);
UseRefImage=(strlen(itINI.RefFileName)!=0);
if (UseRefImage!=0) {
RefImage=ReadRefImage(itINI.RefFileName);
refxvector=ImageToVector(RefImage);
FreeImage(RefImage);
//refxdev=DeviationVector(refxvector);
strcpy(DiffFileName,itINI.RefFileName);
strcat(DiffFileName,".dif");
DiffFile=fopen(DiffFileName,"wt");
Print(_DNormal,"Logging differences in `%s' \n", DiffFileName);
}
tempXv=xvector->value;
tempBv=bvector->value;
//srand((int)clock());
lambda=itINI.Alpha/itINI.Beta;
TotalIterations=itINI.Iterations*ARows;
AntPrint=(int)(TotalIterations/93);
InitArrays();
for (currentiteration=0;currentiteration<TotalIterations;currentiteration++)
{
if (currentiteration%ARows==0) lambda*=itINI.Beta;
if (currentiteration%AntPrint==0)
Print(_DNormal,"Iterating %6.2f %% done\r",
(currentiteration+1)*100.0/TotalIterations);
if (itINI.IterationType==1)
currentrow=currentiteration%ARows;
else {
//currentrow=(int)(ARows*(float)rand()/(RAND_MAX+1.0));
GetRNGstate();
currentrow = (int)(ARows*runif(0, 1));
//currentrow = (int)(ARows*runif(0, RAND_MAX)/(RAND_MAX+1.0));
PutRNGstate();
}
AVector=GenerateAMatrixRow(currentrow);
denom=MultVectorVector(AVector,AVector);
if (fabs(denom)>1e-9)
{
brk=lambda*(tempBv[currentrow]-MultVectorVector(AVector,xvector))/denom;
ARTUpdateAddVector2(xvector, AVector, brk);
}
FreeVector(AVector);
if ( (itINI.SaveIterations) && (currentiteration != 0) && (!(currentiteration%(ARows*(itINI.SaveIterations)))) )
SaveIteration(xvector,(int)(currentiteration/ARows),itINI.SaveIterationsName);
if (UseRefImage==1)
if (currentiteration%AntPrint==0)
fprintf(DiffFile,"%f %f\n",(double)currentiteration/ARows,
(double)L2NormVector(refxvector,xvector));
/*fprintf(DiffFile,"%f %f\n",(double)currentiteration/ARows,
(double)L2NormVector(refxvector,xvector,refxdev));*/
}
Print(_DNormal," \r");
Recon=VectorToImage(xvector,itINI.XSamples,itINI.YSamples);
if (UseRefImage==1){
Print(_DNormal,"L2 = %9.6f \n",L2NormVector(refxvector,xvector));
//Print(_DNormal,"L2 = %9.6f \n",L2NormVector(refxvector,xvector,refxdev));
FreeVector(refxvector);
fclose(DiffFile);
}
RenameImage(Recon,"ReconstructedImage");
Recon->DeltaX=itINI.DeltaX;
Recon->DeltaY=itINI.DeltaY;
Recon->Xmin=itINI.Xmin;
Recon->Ymin=itINI.Ymin;
return Recon;
}
