Astronomical Adaptive Optics 
by Thomas G. Schneider

Listing One

// LGSReconstructorThread
// This routine is called on receiving an interrupt from the wavefront sensor
// PCI interface board.
// Returns: 0 if OK
//          -EFAULT if external resources not initialized 
// NOTE : Runs in real time - NO SYSTEM CALLS ALLOWED!!!

int LGSReconstructorThread(void)
{
    int i = 0, Row =0;
    float A, B, C, D, G, T, V, RowCounter, Accumulator, Average;
    float * InVecP = 0, * P = 0, * GPtr = 0, * GOVPtr = 0; 
    hrtime_t StartTime;
    // Bail out if pointers, buffers, etc. not initialized  
    if (LGSInitialized == 0)
        return -E;
    StartTime = gethrtime();
    // Acquire, rasterize & offset intensity data
    for (i = 0; i < LGSPixelCount; i++)
    {
        LGSIntensityBuffer[i] = LGSCameraBuffer[LGSCameraMap[i]]
                                - LGSIntensityOffsetVector[i];
    }
    LGSStatus.LoopTiming.FrameAcquired = 
                                 (unsigned long)(gethrtime() - StartTime);
    // Calculate gradients - For future performance increase use pre-built
    // offset tables for indexing in buffers
    // Orientation of ABCD values imposed on intensity buffer:
    //   A | B
    //   - + -
    //   D | C
    // The gradient vector is organized in XY pairs in raster order, as
    // follows:
    //   0 - Row  0, Col  0 X
    //   1 - Row  0, Col  0 Y
    //   2 - Row  0, Col  1 X
    // ...
    //  12 - Row  0, Col 12 Y
    //  13 - Row  1, Col  0 X
    // ...
    // 168 - Row 12, Col 12 X
    // 168 - Row 12, Col 12 Y
     
    GPtr = LGSGradientBuffer;
    GOVPtr = LGSGradientOffsetVector;
    for (i = 0; i < LGS_XYGRAD_COUNT / 2; i++)
    {
        Row = (i / LGS_GCOL_COUNT) * LGS_GCOL_COUNT; 
        A = LGSIntensityBuffer[(Row + i) * 2];
        B = LGSIntensityBuffer[(Row + i) * 2 + 1];
        C = LGSIntensityBuffer[(Row + LGS_GCOL_COUNT + i) * 2 + 1];
        D = LGSIntensityBuffer[(Row + LGS_GCOL_COUNT + i) * 2];            
        *(GPtr++) = (C + B) * *(GOVPtr++) - (D + A);
        *(GPtr++) = (D + C) * *(GOVPtr++) - (A + B);
    }
    LGSStatus.LoopTiming.GradientsDone = 
                              (unsigned long)(gethrtime() - StartTime);
    // Multiply control matrix by gradient vector
    for (i = 0; i < LGSActiveActuatorCount; i++)
    {
        T = 0;
        P = LGSControlMatrix + i * LGS_XYGRAD_COUNT;
        InVecP = LGSGradientBuffer;
        RowCounter = LGS_XYGRAD_COUNT;
        while (RowCounter--)
        {
            T += *InVecP++ * *P++;
        }
        LGSOutputBuffer[i] = T;
    }
    LGSStatus.LoopTiming.MatMulDone = 
                          (unsigned long)(gethrtime() - StartTime);
    // Type 1 integrating servo
    G = LGSLoopParams.DMGain;
    D = LGSLoopParams.DMDamping;
    A = LGSLoopParams.DMUpperLimit;
    B = LGSLoopParams.DMLowerLimit;
    Accumulator = 0;
    for (i = 0; i < LGSActiveActuatorCount; i++)
    {
        V = G * LGSOutputBuffer[i]
            + (1 - D) * LGSTMinus1Buffer[i]
            + D * LGSActuatorOffsetVector[i];
        V = LGSClamp(V, A, B); 
        Accumulator += V;
        LGSOutputBuffer[i] = V;
    }
    LGSStatus.LoopTiming.ServoDone = (unsigned long)(gethrtime() - StartTime);
    // Normalize, clamp and set TMinus1, move to final buffer
    // as 16-bit signed integers
    Average = Accumulator / (float)LGSActiveActuatorCount;
    for (i = 0; i < LGSActiveActuatorCount; i++)
    {
        LGSOutputBuffer[i] -= Average;
        LGSOutputBuffer[i] = LGSClamp(LGSOutputBuffer[i], A, B);
        LGSTMinus1Buffer[i] = LGSOutputBuffer[i];
        // OR into mapped Xinetics buffer, since control bits are there
        DMBufferStart[ToXiMap[i]] |= (int16_t)LGSOutputBuffer[i];
    }
    LGSStatus.LoopTiming.PostServoDone = 
                               (unsigned long)(gethrtime() - StartTime);
    // Output
    DMDriverStartDMA();
    LGSStatus.LoopTiming.DMOutputDone = 
                               (unsigned long)(gethrtime() - StartTime);
    // Keep track of loop passes
    LGSStatus.LoopCount = ++LoopCount;
    // Process runtime requests if a request processor is registered
    if (RequestProcessor)
        RequestProcessor();
    return 0;
}


Listing Two

// ProcessRequests - Handle requests made during loop runtime
// Returns: 0 - No request pending
//          Received command value if request received
// NOTE : Runs in real-time - NO SYSTEM CALLS ALLOWED!!!

int ProcessRequests(void)
{
    uint32_t Command, PayloadCount;
    uint32_t CommandBuffer;
    // Check command FIFO for complete requests and don't continue until
    // one has arrived
    if (ReadFIFOStream(CMD_FIFO,
                       (char *)&CommandBuffer,
                       sizeof(CommandBuffer)) == 0)
        return 0;
    // Extract request command and data count
    Command = CommandBuffer & COMMAND_MASK;
    PayloadCount = (CommandBuffer & COUNT_MASK) >> 16;
    // Process requests
    switch (Command)
    {
    case CMD_GET_ALL_DATA:
        rtf_flush(CMD_RESPONSE_FIFO);
        rtf_put(CMD_RESPONSE_FIFO, (char *)pStatus[ModuleIndex],
                sizeof(LOOP_STATUS));
        rtf_put(CMD_RESPONSE_FIFO, (char *)IntensityBuffer[ModuleIndex],
                IntensityBufferByteCount[ModuleIndex]);
        rtf_put(CMD_RESPONSE_FIFO, (char *)GradientBuffer[ModuleIndex],
                GradientBufferByteCount[ModuleIndex]);
        rtf_put(CMD_RESPONSE_FIFO, (char *)OutputBuffer[ModuleIndex],
                OutputBufferByteCount[ModuleIndex]);
        break;
    case CMD_GET_IOV_FRAME_COUNTER:
        rtf_flush(CMD_RESPONSE_FIFO);
        rtf_put(CMD_RESPONSE_FIFO, (char *)&IOVFrameCounter,
                sizeof(IOVFrameCounter));
        break;
    case CMD_GET_GOV_FRAME_COUNTER:
        rtf_flush(CMD_RESPONSE_FIFO);
        rtf_put(CMD_RESPONSE_FIFO, (char *)&GOVFrameCounter,
                sizeof(GOVFrameCounter));
        break;
    case CMD_SET_LOOP_PARAMS:
        if (rtf_get(CMD_DATA_FIFO, (char *)LoopParams[ModuleIndex],
                    sizeof(LOOP_PARAMS)) == 0)

           // Errors sent back to user space through ERROR_FIFO channel
            SendError(-EINVAL, "No loop parameters provided");
        break;
    case CMD_STOP_LOOP:
        break;
    default:
        break;
    }
    return Command;
}






3


