Changeset 13060 for trunk/FACT++


Ignore:
Timestamp:
03/11/12 13:10:02 (13 years ago)
Author:
tbretz
Message:
Implemented the possibility of a current feedback which corrects the applied voltage according to the measured currents.
File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/FACT++/src/feedback.cc

    r12994 r13060  
    5454        kStateConnectedFAD,
    5555        kStateConnected,
    56         kStateTempCtrlIdle,
    57         kStateFeedbackCtrlIdle,
    58         kStateTempCtrlRunning,
    59         kStateFeedbackCtrlRunning,
    60         kStateCalibrating,
     56        kStateTempCtrlIdle,      // 7
     57        kStateFeedbackCtrlIdle,  // 8
     58        kStateCurrentCtrlIdle,  // 9
     59        kStateTempCtrlRunning,  // 9->10
     60        kStateFeedbackCtrlRunning, // 10->11
     61        kStateCurrentCtrlRunning,  // 12
     62        kStateCalibrating,         // 11->13
    6163    };
    6264
     
    6668        kTemp,
    6769        kFeedback,
    68         kFeedbackGlobal
     70        kFeedbackGlobal,
     71        kCurrents,
    6972    };
    7073
     
    100103    vector<vector<float>> fData;
    101104
    102     uint64_t fCursor;
     105    uint64_t fCursorCur;
     106    uint64_t fCursorAmpl;
    103107
    104108    Time fBiasLast;
     
    117121
    118122    double fBiasOffset;
     123
     124    uint16_t fCurrentRequestInterval;
    119125
    120126    bool fOutputEnabled;
     
    160166            vec[i+416] = diff;
    161167
    162         if (fControlType!=kTemp)
    163             return;
     168        if (fControlType==kCurrents)
     169        {
     170            if (fCursorCur==0)
     171            {
     172                DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
     173                return;
     174            }
     175
     176            // Convert from DAC counts to uA
     177            const double conv = 5000e-6/4096;
     178            for (int i=0; i<BIAS::kNumChannels; i++)
     179            {
     180                // 3900 Ohm/n  +  1000 Ohm  +  1200 Ohm
     181                const double R = fMap.hv(i).group()==0 ? 3175 : 2980;
     182                const double I = double(fCurrentsAvg[i])/fCursorCur - fCalibration[i];
     183                vec[i+416] += R * I*conv;
     184            }
     185
     186            fCurrentsAvg.assign(416, 0);
     187            fCursorCur = 0;
     188        }
    164189
    165190        fDimDeviation.setQuality(fControlType);
     
    175200    }
    176201
     202    int AverageCurrents()
     203    {
     204        if (fBiasA.getSize()!=416*sizeof(int16_t))
     205            return -1;
     206
     207        if (fStatusBias.second==BIAS::kRamping)
     208            return false;
     209
     210        const int16_t *ptr = static_cast<int16_t*>(fBiasA.getData());
     211
     212        for (int i=0; i<416; i++)
     213        {
     214            fCurrentsAvg[i] += ptr[i];
     215            fCurrentsRms[i] += ptr[i]*ptr[i];
     216        }
     217
     218        fCursorCur++;
     219
     220        return true;
     221    }
     222
     223
    177224    void HandleCalibration()
    178225    {
    179         if (fBiasA.getSize()!=416*sizeof(int16_t))
    180             return;
    181 
    182         if (fStatusBias.second==BIAS::kRamping)
    183             return;
    184 
    185         const int16_t *ptr = static_cast<int16_t*>(fBiasA.getData());
    186 
    187         for (int i=0; i<416; i++)
    188             if (ptr[i]>0)
    189             {
    190                 fCurrentsAvg[i] += ptr[i];
    191                 fCurrentsRms[i] += ptr[i]*ptr[i];
    192             }
    193 
    194         if (++fCursor<100)
     226        const int rc = AverageCurrents();
     227        if (rc<0)
     228            return;
     229
     230        if (fCursorCur<100)
    195231        {
    196232            DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
    197233            return;
    198234        }
     235
     236        if (rc==0)
     237            return;
    199238
    200239        fCalibration.resize(416*2);
    201240        for (int i=0; i<416; i++)
    202241        {
    203             fCalibration[i]     = double(fCurrentsAvg[i])/fCursor;
    204             fCalibration[i+416] = sqrt(double(fCurrentsRms[i])/fCursor-fCalibration[i]*fCalibration[i]);
     242            fCalibration[i]     = double(fCurrentsAvg[i])/fCursorCur;
     243            fCalibration[i+416] = sqrt(double(fCurrentsRms[i])/fCursorCur-fCalibration[i]*fCalibration[i]);
    205244        }
    206245
     
    235274        // -------- Store new event --------
    236275
    237         fData[fCursor%fData.size()].assign(reinterpret_cast<float*>(fBiasData.getData()),
    238                                            reinterpret_cast<float*>(fBiasData.getData())+1440);
    239 
    240         if (++fCursor<fData.size())
     276        fData[fCursorAmpl%fData.size()].assign(reinterpret_cast<float*>(fBiasData.getData()),
     277                                               reinterpret_cast<float*>(fBiasData.getData())+1440);
     278
     279        if (++fCursorAmpl<fData.size())
    241280            return;
    242281
     
    308347        // CO(k)-CO(k-1) = - Kp[ PV(k) - PV(k-1) ] + Ki * T * (SP(k)-PV(k)) - Kd/T [ PV(k) - 2PV(k-1) + PV(k-2) ]
    309348
    310         if (fCursor%fData.size()>0)
     349        if (fCursorAmpl%fData.size()>0)
    311350            return;
    312351
     
    322361
    323362        ostringstream out;
    324         out << "New " << fData.size() << " event received: " << fCursor << " / " << setprecision(3) << T21 << "s";
     363        out << "New " << fData.size() << " event received: " << fCursorAmpl << " / " << setprecision(3) << T21 << "s";
    325364        Info(out);
    326365
     
    441480        const float med = arr[arr.size()/2];
    442481
    443         fData[fCursor%fData.size()].resize(1); //assign(&med, &med);
    444         fData[fCursor%fData.size()][0] = med;  //assign(&med, &med);
    445 
    446         if (++fCursor<fData.size())
     482        fData[fCursorAmpl%fData.size()].resize(1); //assign(&med, &med);
     483        fData[fCursorAmpl%fData.size()][0] = med;  //assign(&med, &med);
     484
     485        if (++fCursorAmpl<fData.size())
    447486            return;
    448487
     
    464503        // -------- Calculate correction --------
    465504
    466         if (fCursor%fData.size()!=0)
     505        if (fCursorAmpl%fData.size()!=0)
    467506            return;
    468507
     
    504543        fPV[2] = valarray<double>(&avg, 1);
    505544
    506         // => Kp = 0.01 * gain     = 0.00005
    507         // => Ki = 0.8  * gain/20s = 0.00025
    508         // => Kd = 0.1  * gain/20s = 0.00003
    509 
    510         /*
    511         fKp = 0;
    512         fKd = 0;
    513         fKi = 0.00003*20;
    514         */
     545        // ----- Calculate average currents -----
     546
     547        vector<float> A(416);
     548        for (int i=0; i<416; i++)
     549            A[i] = double(fCurrentsAvg[i]) / fCursorCur;
     550
     551        fCurrentsAvg.assign(416, 0);
     552        fCursorCur = 0;
     553
     554        // -------- Calculate correction --------
    515555
    516556        // correction = (fSP[0]-fPV[2])*fKi
     
    531571        const valarray<double> correction = 1./fGain/1000*
    532572            (
    533              fKi*(pow(fSP[0], 1./1.6)-pow(fPV[2], 1./1.6))
     573             //fKi*(pow(fSP[0], 1./1.6)-pow(fPV[2], 1./1.6))
     574             fKi*(fSP[0]-fPV[2])
    534575            );
    535576
     
    588629        }
    589630
    590         if (curr==&fCameraTemp)
     631        if (curr==&fCameraTemp && (fControlType==kTemp || fControlType==kCurrents))
    591632            HandleCameraTemp();
    592633
    593634        if (curr==&fBiasA && fControlType==kTemp && GetCurrentState()==kStateCalibrating)
    594635            HandleCalibration();
     636
     637        if (curr==&fBiasA && (fControlType==kFeedbackGlobal || fControlType==kCurrents))
     638            AverageCurrents();
    595639
    596640        if (curr==&fBiasData && fControlType==kFeedback)
     
    692736        fData.assign(n>0 ? n : fData.size(), vector<float>(0));
    693737
    694         fCursor = 0;
     738        fCursorAmpl = 0;
     739        fCursorCur  = 0;
     740
    695741        fStartTime = Time();
    696742
     
    705751        fPV[2].resize(0);
    706752
     753        fCurrentsAvg.assign(416, 0);
     754        fCurrentsRms.assign(416, 0);
     755
    707756        if (fKp==0 && fKi==0 && fKd==0)
    708757            Warn("Control loop parameters are all set to zero.");
     
    743792        ostringstream out;
    744793        out << "Starting temperature feedback with an offset of " << fBiasOffset << "V";
     794        Message(out);
     795
     796        DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
     797
     798        return GetCurrentState();
     799    }
     800
     801    int StartCurrentCtrl(const EventImp &evt)
     802    {
     803        if (!CheckEventSize(evt.GetSize(), "StartCurrentCtrl", 4))
     804            return kSM_FatalError;
     805
     806        if (fCalibration.size()==0)
     807        {
     808            ostringstream out;
     809            out << "Current control needs a bias crate calibration first... command ignored.";
     810            Error(out);
     811            return GetCurrentState();
     812        }
     813
     814        ResetData(0);
     815
     816        fBiasOffset = evt.GetFloat();
     817        fControlType = kCurrents;
     818
     819        ostringstream out;
     820        out << "Starting current/temp feedback with an offset of " << fBiasOffset << "V";
    745821        Message(out);
    746822
     
    815891        fBiasOffset = -2;
    816892        fControlType = kTemp;
    817         fCursor = 0;
     893        fCursorCur = 0;
    818894        fCurrentsAvg.assign(416, 0);
    819895        fCurrentsRms.assign(416, 0);
     
    823899
    824900        return kStateCalibrating;
     901    }
     902
     903    int SetCurrentRequestInterval(const EventImp &evt)
     904    {
     905        if (!CheckEventSize(evt.GetSize(), "SetCurrentRequestInterval", 2))
     906            return kSM_FatalError;
     907
     908        fCurrentRequestInterval = evt.GetUShort();
     909
     910        Out() << "New current request interval: " << fCurrentRequestInterval << "ms" << endl;
     911
     912        return GetCurrentState();
    825913    }
    826914
     
    901989            if (fControlType==kTemp)
    902990            {
    903                 if (GetCurrentState()==kStateCalibrating && fCursor<100)
     991                if (GetCurrentState()==kStateCalibrating && fCursorCur<100)
    904992                    return GetCurrentState();
    905993
    906994                return fOutputEnabled ? kStateTempCtrlRunning : kStateTempCtrlIdle;
     995            }
     996            if (fControlType==kCurrents)
     997            {
     998                /*
     999                static Time past;
     1000                if (fCurrentRequestInterval>0 && Time()-past>boost::posix_time::milliseconds(fCurrentRequestInterval))
     1001                {
     1002                    DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
     1003                    past = Time();
     1004                }*/
     1005
     1006                return fOutputEnabled ? kStateCurrentCtrlRunning : kStateCurrentCtrlIdle;
    9071007            }
    9081008        }
     
    9381038        fDimCalibration("FEEDBACK/CALIBRATION", "F:416;F:416",
    9391039                        "Current offsets"
    940                         "|Avg[nA]:Average offset"
    941                         "|Rms[nA]:Rms of offset"),
     1040                        "|Avg[dac]:Average offset (5000uA/4096dac)"
     1041                        "|Rms[dac]:Rms of offset (5000uA/4096dac)"),
    9421042        fSP(416),
    943         fKp(0), fKi(0), fKd(0), fT(-1), fOutputEnabled(false)
     1043        fKp(0), fKi(0), fKd(0), fT(-1),
     1044        fCurrentRequestInterval(0),
     1045        fOutputEnabled(false)
    9441046    {
    9451047        // ba::io_service::work is a kind of keep_alive for the loop.
     
    9711073        AddStateName(kStateFeedbackCtrlIdle, "FeedbackIdle",
    9721074                     "Feedback control activated, but voltage output disabled.");
    973 
    974         AddStateName(kStateTempCtrlIdle, "FeedbackIdle",
     1075        AddStateName(kStateTempCtrlIdle, "TempCtrlIdle",
    9751076                     "Temperature control activated, but voltage output disabled.");
     1077        AddStateName(kStateCurrentCtrlIdle, "CurrentCtrlIdle",
     1078                     "Current control activated, but voltage output disabled.");
    9761079
    9771080        AddStateName(kStateFeedbackCtrlRunning, "FeedbackControl",
    9781081                     "Feedback control activated and voltage output enabled.");
    979 
    9801082        AddStateName(kStateTempCtrlRunning, "TempControl",
    9811083                     "Temperature control activated and voltage output enabled.");
    982 
     1084        AddStateName(kStateCurrentCtrlRunning, "CurrentControl",
     1085                     "Current/Temp control activated and voltage output enabled.");
    9831086        AddStateName(kStateCalibrating, "Calibrating",
    9841087                     "Calibrating current offsets.");
     
    9991102             "|offset[V]:Offset from the nominal temperature corrected value in Volts");
    10001103
     1104        AddEvent("START_CURRENT_CONTROL", "F:1", kStateConnectedFSC, kStateConnected)
     1105            (bind(&StateMachineFeedback::StartCurrentCtrl, this, placeholders::_1))
     1106            ("Start the current/temperature control loop"
     1107             "|offset[V]:Offset from the nominal current/temperature corrected value in Volts");
     1108
    10011109        // kStateTempCtrlIdle, kStateFeedbackCtrlIdle, kStateTempCtrlRunning, kStateFeedbackCtrlRunning
    10021110        AddEvent("STOP")
     
    10351143            (bind(&StateMachineFeedback::CalibrateCurrents, this))
    10361144            ("");
     1145
     1146        AddEvent("SET_CURRENT_REQUEST_INTERVAL", kStateConnectedFSC, kStateConnected)//, kStateIdle)
     1147            (bind(&StateMachineFeedback::SetCurrentRequestInterval, this, placeholders::_1))
     1148            ("|interval[ms]:Interval between two current requests in modes which need that.");
    10371149
    10381150        // Verbosity commands
     
    10871199        Message(msg);
    10881200
     1201        fCurrentRequestInterval = conf.Get<uint16_t>("current-request-interval");
     1202
    10891203        return -1;
    10901204    }
     
    11061220    control.add_options()
    11071221        ("pixel-map-file",  var<string>("FACTmapV5a.txt"), "Pixel mapping file. Used here to get the default reference voltage.")
     1222        ("current-request-interval",  var<uint16_t>(1000), "Interval between two current requests.")
    11081223        ;
    11091224
Note: See TracChangeset for help on using the changeset viewer.