C Code

/*  This is an integrator with output limiter and bleed off
    Inputs:
    1. Error signal (double)
    2. Excitation input (double)
    3. Request word (bit encoded int)
    -----------------------------------------------------------------------
    Bit     Name            Description
    -----------------------------------------------------------------------
     0      Filter          Request for integrator to run
     1      Input Switch    Request for input switch to be on
     2      Offset Switch   Request for offset to be enabled 
     3      Limit Switch    Request for the limit switch to be engaged
     4      Output Switch   Request for the ouptut switch to be on
     5      Hold Output     Request for the output to hold
     6      Bleed Enable    Request for filter to bleed off
     7      Bias Enable     Request for output bias to be on
    15      Decimation      Request for output decimation (8Hz single pole)
    -----------------------------------------------------------------------
    4. Offset input (double)
    5. Gain input (double)
    6. Unity gain frequency in Hz (double)
    7. Bleed rate in cts/s (double)
    8. Maximum absolute value/limiter (double)
    9. Bias input (double)
   10. Model rate in Hz (double)
   Outputs:
    1. Control signal (double)
    2. IN1 ouptut (double)
    3. IN2 output (double)
    4. OUT ouptut (double) 
    5. Decimated control signal (double) 
    6. Status word (bit encoded int)
    -----------------------------------------------------------------------
    Bit     Name            Description
    -----------------------------------------------------------------------
     0      Coeff Reset     not used.
     1      Master Reset    Momentary; when set, INT will reset all 
                            filter history buffers.
     2      Input On/Off    Enables/disables signal input to INT.
     3      Offset Switch   Enables/disables application of INT input
                            offset value.
     4      Filter Request  Set to one when an INT filter is requested ON,
                            or zero when INT filter requested OFF 
     5      Filter Status   Set to one by INT when an INT filter is ON, 
                            or zero when INT filter is OFF 
     6      Limiter Switch  Enables/disables application of INT output 
                            limit value.
     7      Limiter Status  Set for 1 sec, when the limiter is enabled 
                            and the output value became too large.
     8      Decimation      Enables/Disables application of decimation 
            Switch          filter to INT OUT16 calculation.
     9      Output Switch   Enables/Disables INT output (INT OUT and 
                            OUT16 variables)
    10      Hold Output     If (!bit 26 && bit27), INT OUT will be held 
                            at last value.
    11      Bleed Enable    If set, the accumulated integrator value will
                            be bled off with the specifed bleed rate 
    12      Bias Enable     If set, a bias will be added to the output
    -----------------------------------------------------------------------
*/

#define INTFILTER 0x01
#define INTINPUT 0x02
#define INTOFFSET 0x04
#define INTLIMIT 0x08
#define INTOUTPUT 0x10
#define INTHOLD 0x20
#define INTBLEED 0x40
#define INTBIAS 0x80
#define INTDEC 0x8000

#define PI 3.1415926535897932384626433832795028841971693993751058

#ifdef _WIN32
typedef enum {false, true} bool;
#define isnan(s) 0
#define printk(s) 
#endif

bool myfinite (double s)
{
    // By IEEE 754 rule, 2*Inf equals Inf
    return !isnan(s) && ((s == 0) || (s != 2*s));
}

void Integrator (double* in, int ins, double* out, int outs)
{ 
    /* INPUT and OUTPUT vars for Integrator function block */
    double inval = 0.0;
    double exc = 0.0;
    int req = 0;
    double ofs = 0.0;
    double gain = 0.0;
    double ugf = 0.0;
    double bleed = 0.0;
    double limit = 0.0;
    double bias = 0.0;
    double rate = 16384;

    double outval = 0.0;
    double in1 = 0.0; 
    double in2 = 0.0; 
    double out1 = 0.0; 
    static double out16 = 0.0;
    int stat = 0;

    /* local vars */
    static int error = 0;
    static double oldout = 0.0;
    static double holdval = 0.0;
    static int limcount = 0;
    double val = 0.0;
    double g = 0.0;

    /* argument check */
    if ((ins != 10) || (outs != 6)) {
        if (!(error & 0x1)) {
            printk("Integrator: wrong number of inputs and/or outputs\n");
            error |= 0x1;
        }
        return;
    }

    // Read input values
    inval = myfinite (in[0]) ? in[0] : 0.0; // must be finite
    exc = myfinite (in[1]) ? in[1] : 0.0; // must be finite
    req = (int) (myfinite (in[2]) ? in[2] : 0); // must be finite
    ofs = myfinite (in[3]) ? in[3] : 0.0; // must be finite
    gain = myfinite (in[4]) ? in[4] : 0.0; // must be finite
    ugf = myfinite (in[5]) && (in[5] > 0) ? in[5] : 0.0; // must be finite and non-negative
    bleed = myfinite (in[6]) && (in[6] > 0) ? in[6] : 0.0; // must be finite and non-negative
    limit = myfinite (in[7]) ? (in[7] > 0 ? in[7] : -in[7]) : 0.0; // must be finite, take abs
    bias = myfinite (in[8]) ? in[8] : 0.0; // must be finite
    rate = (in[9] >= 0) ? in[9] : 1.0; // must be finite and positive

    // Add offset and muliply with gain
    val = inval;
    if (req & INTOFFSET) {
        val += ofs;
    }
    val *= gain;
    // input switch and input test points
    in1 = val;
    if (!(req & INTINPUT)) {
        val = 0.0;
    }
    val += exc;
    in2 = val;

    // Calculate filter
    if (req & INTFILTER) { // filter on
        g = 2*ugf/rate;
        if (g > 1) { // ugf above Nyquist!
            g = 1.0;
        }
        oldout = oldout+PI*g*val; 
    }
    else { // filter is off
        oldout = val;
    }
    // enforce limits when on
    if (req & INTLIMIT) {
        if (oldout < -limit) { // lower limit exceeded
            oldout = -limit;
            limcount = (int)rate + 1;
        }
        else if (oldout > limit) { // upper limit exceeded
            oldout = limit;
            limcount = (int)rate + 1;
        }
    }
    if (limcount > 0) {
        --limcount;
    }

    // Bleed off integrator value if selected
    if (req & INTBLEED) {
        g = bleed/rate;
        if (oldout > g) {
            oldout -= g;
        }
        else if (oldout < -g) {
            oldout += g;
        }
        else {
            oldout = 0.0;
        }
    }

    // Output testpoint
    out1 = oldout;

    // Output enabled
    if (req & INTOUTPUT) {
        val = oldout;
    }
    else {
        val = 0.0;
    }

    // Add bias if selected
    if (req & INTBIAS) {
        val += bias;
    }

    // Hold output if selected
    if (req & INTHOLD) {
        outval = holdval;
    }
    else { // no hold
        outval = val;
        holdval = val;
    }

    // Compute decimated output: 8Hz single pole low pass
    if (req & INTDEC) {
        g = 2*8.0/rate;
        if (g > 1) {
            g = 1.0;
        }
        g *= PI;
        out16 = (1-g)*out16+g*outval;
    }
    else { // no decimation
        out16 = outval;
    }

    // Set status
    stat = 0;
    stat |= (req & INTINPUT) ? 0x0004 : 0;
    stat |= (req & INTOFFSET) ? 0x0008 : 0;
    stat |= (req & INTFILTER) ? 0x0010 : 0;
    stat |= (req & INTFILTER) ? 0x0020 : 0;
    stat |= (req & INTLIMIT) ? 0x0040 : 0;
    stat |= limcount ? 0x0080 : 0;
    stat |= (req & INTDEC) ? 0x0100 : 0;
    stat |= (req & INTOUTPUT) ? 0x0200 : 0;
    stat |= (req & INTHOLD) ? 0x0400 : 0;
    stat |= (req & INTBLEED) ? 0x0800 : 0;
    stat |= (req & INTBIAS) ? 0x1000 : 0;

    // Write output values
    out[0] = outval;
    out[1] = in1;
    out[2] = in2;
    out[3] = out1;
    out[4] = out16;
    out[5] = stat;
}