File content as of revision 8:32445aab4589:

/*
    GPA: frequency point wise gain and phase analyser to measure the frequency
         respone of a dynamical system

        hint:       the measurements should only be perfomed in closed loop
        assumption: the system is at the desired steady state of interest when
                    the measurment starts

                             exc(2)                 C: controller
                               |                    P: plant
                               v
     exc(1) --> o   ->| C |--->o------->| P |----------> out
                ^ -                |             |
                |                   --> inp      |  exc: excitation signal (E)
                |                                |  inp: input plant (U)
                 --------------------------------   out: output plant (Y)

    instantiate option 1: (logarithmic equaly spaced frequency points)
    
    GPA(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
    
        fMin:     minimal desired frequency that should be measured in Hz
        fMax:     maximal desired frequency that should be measured in Hz
        NfexcDes: number of logarithmic equaly spaced frequency points
        NperMin:  minimal number of periods that are used for each frequency point
        NmeasMin: minimal number of samples that are used for each frequency point
        Ts:       sampling time
        Aexc0:    excitation amplitude at fMin
        Aexc1:    excitation amplitude at fMax
           
    instantiate option 2: (for a second, refined frequency grid measurement)
    
    GPA(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
    
        f0:       frequency point for the calculation of Aexc0 in Hz (should be chosen like in the first measurement)
        f1:       frequency point for the calculation of Aexc1 in Hz (should be chosen like in the first measurement)
        *fexcDes: sorted frequency point array in Hz
        NfexcDes: length of fexcDes
        
    instantiate option 3: (for an arbitary but sorted frequency grid measurement)
    
    GPA(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
    
        *fexcDes: sorted frequency point array in Hz
        Aexc0:    excitation amplitude at fexcDes[0]
        Aexc1:    excitation amplitude at fexcDes[NfexcDes-1]
        NfexcDes: length of fexcDes

    hints:    the amplitude drops with 1/fexc, if you're using Axc1 = Aexc0/fMax then d/dt exc = const.,
              this is recommended if your controller does not have a rolloff. if a desired frequency point
              is not measured try to increase Nmeas.

    pseudo code for a closed loop measurement with a controller C:

        excitation input at (1):
       
        - measuring the plant P and the closed loop tf T = PC/(1 + PC):
            desTorque = pi_w(omega_desired - omega + excWobble);
            inpWobble = desTorque;
            outWobble = omega;
            excWobble = Wobble(excWobble, outWobble);
                        
        - measuring the controller C and the closed loop tf SC = C/(1 + PC)
            desTorque = pi_w(omega_desired - omega + excWobble);
            inpWobble = n_soll + excWobble - omega;
            outWobble = desTorque;
            excWobble = Wobble(inpWobble, outWobble);

        excitation input at (2):
        
        - measuring the plant P and the closed loop tf SP = P/(1 + PC):
            desTorque = pi_w(omega_desired - omega) + excWobble;
            inpWobble = desTorque;
            outWobble = omega;
            excWobble = Wobble(excWobble, outWobble);    

    usage:
        exc(k+1) = myGPA(inp(k), out(k)) does update the internal states of the 
        gpa at the timestep k and returns the excitation signal for the timestep
        k+1. the results are plotted to a terminal (putty) over a serial 
        connection and look as follows:
 -----------------------------------------------------------------------------------------
   fexc[Hz]    |Gyu|     ang(Gyu)    |Gye|     ang(Gye)     |E|        |U|        |Y|
 -----------------------------------------------------------------------------------------
   1.000e+00  2.924e+01 -1.572e+00  1.017e+00 -4.983e-02  5.000e+00  1.739e-01  5.084e+00

    in matlab you can use:
        dataNucleo = [... insert measurement data here ...];
        g = frd(dataNucleo(:,2).*exp(1i*dataNucleo(:,3)), dataNucleo(:,1), Ts, 'Units', 'Hz');
        gcl = frd(dataNucleo(:,4).*exp(1i*dataNucleo(:,5)), dataNucleo(:,1), Ts, 'Units', 'Hz');

    if you're evaluating more than one measurement which contain equal frequency points try:
        dataNucleo = [dataNucleo1; dataNucleo2];
        [~, ind] = unique(dataNucleo(:,1),'stable');
        dataNucleo = dataNucleo(ind,:);

        autor: M.E. Peter
*/

#include "GPAf.h"
#include "mbed.h"
#include "math.h"
#define   pi 3.14159f

using namespace std;

GPAf::GPAf(float fMin, float fMax, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
{
    this->NfexcDes = NfexcDes;
    this->NperMin = NperMin;
    this->NmeasMin = NmeasMin;
    this->Ts = (float)Ts;

    // calculate logarithmic spaced frequency points
    fexcDes = (float*)malloc(NfexcDes*sizeof(float));
    fexcDesLogspace((float)fMin, (float)fMax, NfexcDes);

    // calculate coefficients for decreasing amplitude (1/fexc)
    this->aAexcDes = ((float)Aexc1 - (float)Aexc0)/(1.0f/fexcDes[NfexcDes-1] - 1.0f/fexcDes[0]);
    this->bAexcDes = (float)Aexc0 - aAexcDes/fexcDes[0];

    fnyq = 1.0f/2.0f/(float)Ts;
    pi2 = 2.0f*pi;
    pi2Ts = pi2*(float)Ts;
    piDiv2 = pi/2.0f;
    
    sU = (float*)malloc(3*sizeof(float));
    sY = (float*)malloc(3*sizeof(float));
    reset();
}

GPAf::GPAf(float f0, float f1, float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
{
    // convert fexcDes from float to float, it is assumed that it is sorted
    this->NfexcDes = NfexcDes;
    this->fexcDes = (float*)malloc(NfexcDes*sizeof(float));
    for(int i = 0; i < NfexcDes; i++) {
        this->fexcDes[i] = (float)fexcDes[i];
    }
    this->NperMin = NperMin;
    this->NmeasMin = NmeasMin;
    this->Ts = (float)Ts;

    // calculate coefficients for decreasing amplitude (1/fexc)
    this->aAexcDes = ((float)Aexc1 - (float)Aexc0)/(1.0f/(float)f1 - 1.0f/(float)f0);
    this->bAexcDes = (float)Aexc0 - aAexcDes/fexcDes[0];

    fnyq = 1.0f/2.0f/(float)Ts;
    pi2 = 2.0f*pi;
    pi2Ts = pi2*(float)Ts;
    piDiv2 = pi/2.0f;

    sU = (float*)malloc(3*sizeof(float));
    sY = (float*)malloc(3*sizeof(float));
    reset();
}

GPAf::GPAf(float *fexcDes, int NfexcDes, int NperMin, int NmeasMin, float Ts, float Aexc0, float Aexc1)
{
    // convert fexcDes from float to float, it is assumed that it is sorted
    this->NfexcDes = NfexcDes;
    this->fexcDes = (float*)malloc(NfexcDes*sizeof(float));
    for(int i = 0; i < NfexcDes; i++) {
        this->fexcDes[i] = (float)fexcDes[i];
    }
    this->NperMin = NperMin;
    this->NmeasMin = NmeasMin;
    this->Ts = (float)Ts;

    // calculate coefficients for decreasing amplitude (1/fexc)
    this->aAexcDes = ((float)Aexc1 - (float)Aexc0)/(1.0f/this->fexcDes[NfexcDes-1] - 1.0f/this->fexcDes[0]);
    this->bAexcDes = (float)Aexc0 - aAexcDes/fexcDes[0];

    fnyq = 1.0f/2.0f/(float)Ts;
    pi2 = 2.0f*pi;
    pi2Ts = pi2*(float)Ts;
    piDiv2 = pi/2.0f;

    sU = (float*)malloc(3*sizeof(float));
    sY = (float*)malloc(3*sizeof(float));
    reset();
}

GPAf::~GPAf() {}

void GPAf::reset()
{
    Nmeas = 0;
    Nper = 0;
    fexc = 0.0;
    fexcPast = 0.0;
    ii = 1; // iterating through desired frequency points
    jj = 1; // iterating through measurement points w.r.t. reachable frequency
    scaleG = 0.0;
    scaleH = 2.0;
    wk = 0.0;
    cr = 0.0;
    ci = 0.0;
    for(int i = 0; i < 3; i++) {
        sU[i] = 0.0;
        sY[i] = 0.0;
    }
    sinarg = 0.0;
    NmeasTotal = 0;
    Aexc = 0.0;
    pi2Tsfexc = 0.0;
}

float GPAf::update(float inp, float out)
{
    // a new frequency point has been reached
    if(jj == 1) { 
        // get a new unique frequency point
        while(fexc == fexcPast) {
            // measurement finished
            if(ii > NfexcDes) {
                return 0.0f;
            }
            calcGPAmeasPara(fexcDes[ii - 1]);
            // secure fexc is not higher or equal to nyquist frequency
            if(fexc >= fnyq) {
                fexc = fexcPast;
            }
            // no frequency found
            if(fexc == fexcPast) {
                ii += 1;
            } else {
                Aexc = aAexcDes/fexc + bAexcDes;
                pi2Tsfexc = pi2Ts*fexc;
            }
        }
        // secure sinarg starts at 0 (numerically maybe not given)
        sinarg = 0.0;
        // filter scaling
        scaleG = 1.0f/sqrt((float)Nmeas);
        // filter coefficients
        cr = cos(pi2Tsfexc);
        ci = sin(pi2Tsfexc);
        // filter storage
        for(int i = 0; i < 3; i++) {
            sU[i] = 0.0;
            sY[i] = 0.0;
        }
    }
    // update hann window
    calcHann();
    // filter step for signal su
    sU[0] = wk*scaleG*inp + 2.0f*cr*sU[1] - sU[2];
    sU[2] = sU[1];
    sU[1] = sU[0];
    // filter step for signal sy
    sY[0] = wk*scaleG*out + 2.0f*cr*sY[1] - sY[2];
    sY[2] = sY[1];
    sY[1] = sY[0];
    // measurement of frequencypoint is finished
    if(jj == Nmeas) {
        jj = 1;
        ii += 1;
        fexcPast = fexc;
        // calculate the one point dft
        float Ureal = 2.0f*scaleH*scaleG*(cr*sU[1] - sU[2]);
        float Uimag = 2.0f*scaleH*scaleG*ci*sU[1];
        float Yreal = 2.0f*scaleH*scaleG*(cr*sY[1] - sY[2]);
        float Yimag = 2.0f*scaleH*scaleG*ci*sY[1];
        // calculate magnitude and angle
        float Umag = (float)(sqrt(Ureal*Ureal + Uimag*Uimag));
        float Ymag = (float)(sqrt(Yreal*Yreal + Yimag*Yimag));
        float absGyu = (float)(Ymag/Umag);
        float angGyu = (float)(atan2(Yimag, Yreal) - atan2(Uimag, Ureal));
        float absGye = (float)(Ymag/Aexc);
        float angGye = (float)(atan2(Yimag, Yreal) + piDiv2);
        // user info
        if(ii == 2) {
            printLine();
            printf("   fexc[Hz]    |Gyu|     ang(Gyu)    |Gye|     ang(Gye)     |E|        |U|        |Y|\r\n");
            printLine();
        }
        printf("%11.3e %10.3e %10.3e %10.3e %10.3e %10.3e %10.3e %10.3e\r\n", (float)fexc, absGyu, angGyu, absGye, angGye, (float)Aexc, Umag, Ymag);
    } else {
        jj += 1;
    }
    sinarg = fmod(sinarg + pi2Tsfexc, pi2);
    NmeasTotal += 1;
    return (float)(Aexc*sin(sinarg));
}

void GPAf::fexcDesLogspace(float fMin, float fMax, int NfexcDes)
{
    // calculate logarithmic spaced frequency points
    float Gain = log10(fMax/fMin)/((float)NfexcDes - 1.0f);
    float expon = 0.0;
    for(int i = 0; i < NfexcDes; i++) {
        fexcDes[i] = fMin*pow(10.0f, expon);
        expon += Gain;
    }
}

void GPAf::calcGPAmeasPara(float fexcDes_i)
{
    // Nmeas has to be an integer
    Nper = NperMin;
    Nmeas = (int)floor((float)Nper/fexcDes_i/Ts + 0.5f);
    //  secure that the minimal number of measurements is fullfilled
    int Ndelta = NmeasMin - Nmeas;
    if(Ndelta > 0) {
        Nper = (int)ceil((float)NmeasMin*fexcDes_i*Ts);
        Nmeas = (int)floor((float)Nper/fexcDes_i/Ts + 0.5f);
    }
    // evaluating reachable frequency
    fexc = (float)Nper/(float)Nmeas/Ts;
}

void GPAf::calcHann()
{
    wk = 0.5f - 0.5f*cos(2.0f*pi*((float)jj-1.0f)/(float)Nmeas);
}

void GPAf::printLine()
{
    printf("-----------------------------------------------------------------------------------------\r\n");
}

void GPAf::printGPAfexcDes()
{
    printLine();
    for(int i = 0; i < NfexcDes; i++) {
        printf("%9.4f\r\n", (float)fexcDes[i]);
    }
}

void GPAf::printGPAmeasPara()
{
    printLine();
    printf(" fexcDes[Hz]  fexc[Hz]       Aexc   Nmeas   Nper\r\n");
    printLine();
    for(int i = 0; i < NfexcDes; i++) {
        calcGPAmeasPara(fexcDes[i]);
        if(fexc == fexcPast || fexc >= fnyq) {
            fexc = 0.0;
            Nmeas = 0;
            Nper = 0;
            Aexc = 0.0;
        } else {
            Aexc = aAexcDes/fexc + bAexcDes;
            fexcPast = fexc;
        }
        NmeasTotal += Nmeas;
        printf("%12.2e %9.2e %10.2e %7i %6i \r\n", (float)fexcDes[i], (float)fexc, (float)Aexc, Nmeas, Nper);
    }
    printGPAmeasTime();
    reset();
}

void GPAf::printGPAmeasTime()
{
    printLine();
    printf(" number of data points:  %9i\r\n", NmeasTotal);
    printf(" measurment time in sec: %9.2f\r\n", (float)((float)NmeasTotal*Ts));
}

void GPAf::printNfexcDes()
{
    printLine();
    printf(" number of frequancy points:  %2i\r\n", NfexcDes);
}