Changeset 12879 for trunk


Ignore:
Timestamp:
02/08/12 23:15:23 (13 years ago)
Author:
tbretz
Message:
Implemeted a command for gobal feedback; splitted the infoHandler into single functions; updated the control loop parameters
File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/FACT++/src/feedback.cc

    r12847 r12879  
    6363        kIdle,
    6464        kTemp,
    65         kFeedback
     65        kFeedback,
     66        kFeedbackGlobal
    6667    };
    6768
    … …  
    129130    }
    130131
    131     void ResetData()
    132     {
    133         fData.clear();
    134         fData.resize(500);
    135         fCursor = 0;
    136         fStartTime = Time();
    137 
    138         fSP = valarray<double>(0., 416);
     132    void HandleCameraTemp()
     133    {
     134        if (fCameraTemp.getSize()!=60*sizeof(float))
     135            return;
     136
     137        const float *ptr = static_cast<float*>(fCameraTemp.getData());
     138
     139        double avg = 0;
     140        int    num = 0;
     141        for (int i=1; i<32; i++)
     142            if (ptr[i]!=0)
     143            {
     144                avg += ptr[i];
     145                num++;
     146            }
     147
     148        if (num==0)
     149            return;
     150
     151        avg /= num;
     152
     153
     154        const float diff = (avg-25)*4./70 + fBiasOffset;
    139155
    140156        vector<float> vec(2*BIAS::kNumChannels);
     157        for (int i=0; i<BIAS::kNumChannels; i++)
     158            vec[i+416] = diff;
     159
     160        if (fControlType!=kTemp)
     161            return;
     162
    141163        fDimDeviation.Update(vec);
    142164
    143         fPV[0].resize(0);
    144         fPV[1].resize(0);
    145         fPV[2].resize(0);
    146 
    147         if (fKp==0 && fKi==0 && fKd==0)
    148             Warn("Control loop parameters are all set to zero.");
    149     }
    150 
    151     void infoHandler()
    152     {
    153         DimInfo *curr = getInfo(); // get current DimInfo address
    154         if (!curr)
    155             return;
    156 
    157         if (curr==&fBias)
    158         {
    159             fStatusBias = GetNewState(fBias);
    160             return;
    161         }
    162 
    163         if (curr==&fFAD)
    164         {
    165             fStatusFAD = GetNewState(fFAD);
    166             return;
    167         }
    168 
    169         if (curr==&fFSC)
    170         {
    171             fStatusFSC = GetNewState(fFSC);
    172             return;
    173         }
    174 
    175         if (curr==&fDim)
    176         {
    177             fStatusDim = GetNewState(fDim);
    178             fStatusDim.second = curr->getSize()==4 ? curr->getInt() : 0;
    179             return;
    180         }
    181 
    182         if (curr==&fCameraTemp)
    183         {
    184             if (curr->getSize()!=60*sizeof(float))
    185                 return;
    186 
    187             const float *ptr = static_cast<float*>(curr->getData());
    188 
    189             double avg = 0;
    190             int    num = 0;
    191             for (int i=1; i<32; i++)
    192                 if (ptr[i]!=0)
    193                 {
    194                     avg += ptr[i];
    195                     num++;
    196                 }
    197 
    198             if (num==0)
    199                 return;
    200 
    201             avg /= num;
    202 
    203 
    204             const float diff = (avg-25)*4./70 + fBiasOffset;
    205 
    206             vector<float> vec(2*BIAS::kNumChannels);
    207             for (int i=0; i<BIAS::kNumChannels; i++)
    208                 vec[i+416] = diff;
    209 
    210             if (fControlType!=kTemp)
    211                 return;
    212 
    213             fDimDeviation.Update(vec);
    214 
    215             if (fOutputEnabled && fStatusBias.second==BIAS::kVoltageOn)
     165        if (!fOutputEnabled || fStatusBias.second!=BIAS::kVoltageOn)
     166            return;
     167
     168        Info("Sending correction to feedback.");
     169
     170        DimClient::sendCommandNB((char*)"BIAS_CONTROL/SET_GAPD_REFERENCE_OFFSET",
     171                                 (void*)&diff, sizeof(float));
     172    }
     173
     174    void HandleCalibration()
     175    {
     176        if (fBiasA.getSize()!=416*sizeof(int16_t))
     177            return;
     178
     179        if (fStatusBias.second==BIAS::kRamping)
     180            return;
     181
     182        const int16_t *ptr = static_cast<int16_t*>(fBiasA.getData());
     183
     184        for (int i=0; i<416; i++)
     185            if (ptr[i]>0)
    216186            {
    217                 Info("Sending correction to feedback.");
    218 
    219                 DimClient::sendCommandNB((char*)"BIAS_CONTROL/SET_GAPD_REFERENCE_OFFSET",
    220                                          (void*)&diff, sizeof(float));
     187                fCurrentsAvg[i] += ptr[i];
     188                fCurrentsRms[i] += ptr[i]*ptr[i];
    221189            }
    222         }
    223 
    224         if (curr==&fBiasA && fControlType==kTemp && GetCurrentState()==kStateCalibrating)
    225         {
    226             if (curr->getSize()!=416*sizeof(int16_t))
    227                 return;
    228 
    229             if (fStatusBias.second==BIAS::kRamping)
    230                 return;
    231 
    232             const int16_t *ptr = static_cast<int16_t*>(curr->getData());
    233 
    234             for (int i=0; i<416; i++)
    235                 if (ptr[i]>0)
    236                 {
    237                     fCurrentsAvg[i] += ptr[i];
    238                     fCurrentsRms[i] += ptr[i]*ptr[i];
    239                 }
    240 
    241             if (++fCursor<100)
    242             {
    243                 DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
    244                 return;
    245             }
    246 
    247             fCalibration.resize(416*2);
    248             for (int i=0; i<416; i++)
    249             {
    250                 fCalibration[i]     = double(fCurrentsAvg[i])/fCursor;
    251                 fCalibration[i+416] = sqrt(double(fCurrentsRms[i])/fCursor-fCalibration[i]*fCalibration[i]);
    252             }
    253 
    254             fDimCalibration.Update(fCalibration);
    255 
    256             fOutputEnabled = false;
    257             fControlType = kIdle;
    258 
     190
     191        if (++fCursor<100)
     192        {
    259193            DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
    260         }
    261 
    262         if (curr==&fBiasData && fControlType==kFeedback)
    263         {
    264             if (curr->getSize()!=1440*sizeof(float))
    265                 return;
    266 
    267             // -------- Check age of last stored event --------
    268 
    269             // Must be called in this order
    270             const int tsec = curr->getTimestamp();
    271             const int tms  = curr->getTimestampMillisecs();
    272 
    273             const Time tm(tsec, tms*1000);
    274 
    275             if (Time()-fBiasLast>boost::posix_time::seconds(30))
    276             {
    277                 Warn("Last received event data older than 30s... resetting average calculation.");
    278                 ResetData();
    279             }
    280             fBiasLast = tm;
    281 
    282             // -------- Store new event --------
    283 
    284             fData[fCursor%fData.size()].assign(reinterpret_cast<float*>(curr->getData()),
    285                                                reinterpret_cast<float*>(curr->getData())+1440);
    286 
    287 
    288             fCursor++;
    289 
    290             if (fCursor<fData.size())
    291                 return;
    292 
    293             // -------- Calculate statistics --------
    294 
    295             valarray<double> med(1440);
    296 
    297             for (int ch=0; ch<1440; ch++)
    298             {
    299                 vector<float> arr(fData.size());
    300                 for (size_t i=0; i<fData.size(); i++)
    301                     arr[i] = fData[i][ch];
    302 
    303                 sort(arr.begin(), arr.end());
    304 
    305                 med[ch] = arr[arr.size()/2];
    306             }
    307 
    308             /*
     194            return;
     195        }
     196
     197        fCalibration.resize(416*2);
     198        for (int i=0; i<416; i++)
     199        {
     200            fCalibration[i]     = double(fCurrentsAvg[i])/fCursor;
     201            fCalibration[i+416] = sqrt(double(fCurrentsRms[i])/fCursor-fCalibration[i]*fCalibration[i]);
     202        }
     203
     204        fDimCalibration.Update(fCalibration);
     205
     206        fOutputEnabled = false;
     207        fControlType = kIdle;
     208
     209        DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
     210    }
     211
     212    void HandleFeedback()
     213    {
     214        if (fBiasData.getSize()!=1440*sizeof(float))
     215            return;
     216
     217        // -------- Check age of last stored event --------
     218
     219        // Must be called in this order
     220        const int tsec = fBiasData.getTimestamp();
     221        const int tms  = fBiasData.getTimestampMillisecs();
     222
     223        const Time tm(tsec, tms*1000);
     224
     225        if (Time()-fBiasLast>boost::posix_time::seconds(30))
     226        {
     227            Warn("Last received event data older than 30s... resetting average calculation.");
     228            ResetData();
     229        }
     230        fBiasLast = tm;
     231
     232        // -------- Store new event --------
     233
     234        fData[fCursor%fData.size()].assign(reinterpret_cast<float*>(fBiasData.getData()),
     235                                           reinterpret_cast<float*>(fBiasData.getData())+1440);
     236
     237        if (++fCursor<fData.size())
     238            return;
     239
     240        // -------- Calculate statistics --------
     241
     242        valarray<double> med(1440);
     243
     244        for (int ch=0; ch<1440; ch++)
     245        {
     246            vector<float> arr(fData.size());
     247            for (size_t i=0; i<fData.size(); i++)
     248                arr[i] = fData[i][ch];
     249
     250            sort(arr.begin(), arr.end());
     251
     252            med[ch] = arr[arr.size()/2];
     253        }
     254
     255        /*
    309256            vector<float> med(1440);
    310257            vector<float> rms(1440);
    … …  
    322269            */
    323270
    324             vector<double> avg(BIAS::kNumChannels);
    325             vector<int>    num(BIAS::kNumChannels);
    326             for (int i=0; i<1440; i++)
    327             {
    328                 const PixelMapEntry &ch = fMap.hw(i);
    329 
    330                 // FIXME: Add a consistency check if the median makes sense...
    331                 // FIXME: Add a consistency check to remove pixels with bright stars (median?)
    332 
    333                 avg[ch.hv()] += med[i];
    334                 num[ch.hv()]++;
    335             }
    336 
    337             for (int i=0; i<BIAS::kNumChannels; i++)
    338             {
    339                 if (num[i])
    340                     avg[i] /= num[i];
    341 
    342             }
    343 
    344             // -------- Calculate correction --------
    345 
    346             // http://bestune.50megs.com/typeABC.htm
    347 
    348             // CO: Controller output
    349             // PV: Process variable
    350             // SP: Set point
    351             // T:  Sampling period (loop update period)
    352             // e = SP - PV
    353             //
    354             // Kp : No units
    355             // Ki : per seconds
    356             // Kd : seconds
    357 
    358             // 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) ]
    359 
    360             if (fCursor%fData.size()==0)
    361             {
    362                 // FIXME: Take out broken / dead boards.
    363 
    364                 const Time tm0 = Time();
    365 
    366                 /*const*/ double T21 = fT>0 ? fT : (tm0-fStartTime).total_microseconds()/1000000.;
    367                 const double T10 = fT21;
    368                 fT21 = T21;
    369 
    370                 fStartTime = tm0;
    371 
    372                 ostringstream out;
    373                 out << "New " << fData.size() << " event received: " << fCursor << " / " << setprecision(3) << T21 << "s";
    374                 Info(out);
    375 
    376                 if (fPV[0].size()==0)
    377                 {
    378                     fPV[0].resize(avg.size());
    379                     fPV[0] = valarray<double>(avg.data(), avg.size());
    380                 }
    381                 else
    382                     if (fPV[1].size()==0)
    383                     {
    384                         fPV[1].resize(avg.size());
    385                         fPV[1] = valarray<double>(avg.data(), avg.size());
    386                     }
    387                     else
    388                         if (fPV[2].size()==0)
    389                         {
    390                             fPV[2].resize(avg.size());
    391                             fPV[2] = valarray<double>(avg.data(), avg.size());
    392                         }
    393                         else
    394                         {
    395                             fPV[0] = fPV[1];
    396                             fPV[1] = fPV[2];
    397 
    398                             fPV[2].resize(avg.size());
    399                             fPV[2] = valarray<double>(avg.data(), avg.size());
    400 
    401                             if (T10<=0 || T21<=0)
    402                                 return;
    403 
    404                             cout << "Calculating (" << fCursor << ":" << T21 << ")... " << endl;
    405 
    406                             // fKi[j] = response[j]*gain;
    407                             // Kp = 0;
    408                             // Kd = 0;
    409 
    410                             // => Kp = 0.01 * gain     = 0.00005
    411                             // => Ki = 0.8  * gain/20s = 0.00025
    412                             // => Kd = 0.1  * gain/20s = 0.00003
    413 
    414                             fKp = 0;
    415                             fKd = 0;
    416                             fKi = 0.00003*20;
    417                             T21 = 1;
    418 
    419                             //valarray<double> correction = - Kp*(PV[2] - PV[1]) + Ki * dT * (SP-PV[2]) - Kd/dT * (PV[2] - 2*PV[1] + PV[0]);
    420                             //valarray<double> correction =
    421                             // -      Kp * (PV[2] - PV[1])
    422                             // + dT * Ki * (SP    - PV[2])
    423                             // - Kd / dT * (PV[2] - 2*PV[1] + PV[0]);
    424                             //
    425                             // - (Kp+Kd/dT1) * (PV[2] - PV[1])
    426                             // + dT2 * Ki * (SP    - PV[2])
    427                             // + Kd / dT1 * (PV[1] - PV[0]);
    428                             //
    429                             // - Kp * (PV[2] - PV[1])
    430                             // + Ki * (SP    - PV[2])*dT
    431                             // - Kd * (PV[2] - PV[1])/dT
    432                             // + Kd * (PV[1] - PV[0])/dT;
    433                             //
    434                             //valarray<double> correction =
    435                             //    - Kp*(PV[2] - PV[1]) + Ki * T21 * (SP-PV[2]) - Kd*(PV[2]-PV[1])/T21 - Kd*(PV[0]-PV[1])/T01;
    436                             const valarray<double> correction = fGain/1000*
    437                                 (
    438                                  - (fKp+fKd/T21)*(fPV[2] - fPV[1])
    439                                  +  fKi*T21*(fSP-fPV[2])
    440                                  +  fKd/T10*(fPV[1]-fPV[0])
    441                                 );
    442 
    443                             /*
    444                              integral = 0
    445                              start:
    446                                  integral += (fSP - fPV[2])*dt
    447 
    448                                  output = Kp*(fSP - fPV[2]) + Ki*integral - Kd*(fPV[2] - fPV[1])/dt
    449 
    450                                  wait(dt)
    451 
    452                                  goto start
    453                              */
    454 
    455                             vector<float> vec(2*BIAS::kNumChannels);
    456                             for (int i=0; i<BIAS::kNumChannels; i++)
    457                                 vec[i] = fPV[2][i]-fSP[i];
    458 
    459                             for (int i=0; i<BIAS::kNumChannels; i++)
    460                                 vec[i+416] = avg[i]<5*2.5 ? 0 : correction[i];
    461 
    462                             fDimDeviation.Update(vec);
    463 
    464                             if (fOutputEnabled && fStatusBias.second==BIAS::kVoltageOn)
    465                             {
    466                                 Info("Sending correction to feedback.");
    467 
    468                                 DimClient::sendCommandNB((char*)"BIAS_CONTROL/ADD_REFERENCE_VOLTAGES",
    469                                                          (void*)(vec.data()+416), 416*sizeof(float));
    470 
    471                                 /*
    472                                 if (!Dim::SendCommand("BIAS_CONTROL/ADD_REFERENCE_VOLTAGES",
    473                                                       (const void*)(vec.data()+416), 416*sizeof(float)))
    474                                 {
    475                                     Error("Sending correction to bias control failed... switching off.");
    476                                     fOutputEnabled=false;
    477                                 }
    478                                 else
    479                                    Info("Success!");
    480                                 */
    481                             }
    482                         }
    483 
    484             }
    485         }
    486 
     271        vector<double> avg(BIAS::kNumChannels);
     272        vector<int>    num(BIAS::kNumChannels);
     273        for (int i=0; i<1440; i++)
     274        {
     275            const PixelMapEntry &ch = fMap.hw(i);
     276
     277            // FIXME: Add a consistency check if the median makes sense...
     278            // FIXME: Add a consistency check to remove pixels with bright stars (median?)
     279
     280            avg[ch.hv()] += med[i];
     281            num[ch.hv()]++;
     282        }
     283
     284        for (int i=0; i<BIAS::kNumChannels; i++)
     285        {
     286            if (num[i])
     287                avg[i] /= num[i];
     288
     289        }
     290
     291        // -------- Calculate correction --------
     292
     293        // http://bestune.50megs.com/typeABC.htm
     294
     295        // CO: Controller output
     296        // PV: Process variable
     297        // SP: Set point
     298        // T:  Sampling period (loop update period)
     299        // e = SP - PV
     300        //
     301        // Kp : No units
     302        // Ki : per seconds
     303        // Kd : seconds
     304
     305        // 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) ]
     306
     307        if (fCursor%fData.size()>0)
     308            return;
     309
     310        // FIXME: Take out broken / dead boards.
     311
     312        const Time tm0 = Time();
     313
     314        /*const*/ double T21 = fT>0 ? fT : (tm0-fStartTime).total_microseconds()/1000000.;
     315        const double T10 = fT21;
     316        fT21 = T21;
     317
     318        fStartTime = tm0;
     319
     320        ostringstream out;
     321        out << "New " << fData.size() << " event received: " << fCursor << " / " << setprecision(3) << T21 << "s";
     322        Info(out);
     323
     324        if (fPV[0].size()==0)
     325        {
     326            fPV[0].resize(avg.size());
     327            fPV[0] = valarray<double>(avg.data(), avg.size());
     328            return;
     329        }
     330
     331        if (fPV[1].size()==0)
     332        {
     333            fPV[1].resize(avg.size());
     334            fPV[1] = valarray<double>(avg.data(), avg.size());
     335            return;
     336        }
     337
     338        if (fPV[2].size()==0)
     339        {
     340            fPV[2].resize(avg.size());
     341            fPV[2] = valarray<double>(avg.data(), avg.size());
     342            return;
     343        }
     344
     345        fPV[0] = fPV[1];
     346        fPV[1] = fPV[2];
     347
     348        fPV[2].resize(avg.size());
     349        fPV[2] = valarray<double>(avg.data(), avg.size());
     350
     351        if (T10<=0 || T21<=0)
     352            return;
     353
     354        //cout << "Calculating (" << fCursor << ":" << T21 << ")... " << endl;
     355
     356        // fKi[j] = response[j]*gain;
     357        // Kp = 0;
     358        // Kd = 0;
     359
     360        // => Kp = 0.01 * gain     = 0.00005
     361        // => Ki = 0.8  * gain/20s = 0.00025
     362        // => Kd = 0.1  * gain/20s = 0.00003
     363
     364        /*
     365        fKp = 0;
     366        fKd = 0;
     367        fKi = 0.00003*20;
     368        T21 = 1;
     369        */
     370
     371        //valarray<double> correction = - Kp*(PV[2] - PV[1]) + Ki * dT * (SP-PV[2]) - Kd/dT * (PV[2] - 2*PV[1] + PV[0]);
     372        //valarray<double> correction =
     373        // -      Kp * (PV[2] - PV[1])
     374        // + dT * Ki * (SP    - PV[2])
     375        // - Kd / dT * (PV[2] - 2*PV[1] + PV[0]);
     376        //
     377        // - (Kp+Kd/dT1) * (PV[2] - PV[1])
     378        // + dT2 * Ki * (SP    - PV[2])
     379        // + Kd / dT1 * (PV[1] - PV[0]);
     380        //
     381        // - Kp * (PV[2] - PV[1])
     382        // + Ki * (SP    - PV[2])*dT
     383        // - Kd * (PV[2] - PV[1])/dT
     384        // + Kd * (PV[1] - PV[0])/dT;
     385        //
     386        //valarray<double> correction =
     387        //    - Kp*(PV[2] - PV[1]) + Ki * T21 * (SP-PV[2]) - Kd*(PV[2]-PV[1])/T21 - Kd*(PV[0]-PV[1])/T01;
     388        const valarray<double> correction = 1./fGain/1000*
     389            (
     390             - (fKp+fKd/T21)*(fPV[2] - fPV[1])
     391             +  fKi*T21*(fSP-fPV[2])
     392             +  fKd/T10*(fPV[1]-fPV[0])
     393            );
     394
     395        /*
     396         integral = 0
     397         start:
     398         integral += (fSP - fPV[2])*dt
     399
     400         output = Kp*(fSP - fPV[2]) + Ki*integral - Kd*(fPV[2] - fPV[1])/dt
     401
     402         wait(dt)
     403
     404         goto start
     405         */
     406
     407        vector<float> vec(2*BIAS::kNumChannels);
     408        for (int i=0; i<BIAS::kNumChannels; i++)
     409            vec[i] = fPV[2][i]-fSP[i];
     410
     411        for (int i=0; i<BIAS::kNumChannels; i++)
     412            vec[i+416] = avg[i]<5*2.5 ? 0 : correction[i];
     413
     414        fDimDeviation.Update(vec);
     415
     416        if (!fOutputEnabled || fStatusBias.second!=BIAS::kVoltageOn)
     417            return;
     418
     419        Info("Sending correction to feedback.");
     420
     421        DimClient::sendCommandNB((char*)"BIAS_CONTROL/ADD_REFERENCE_VOLTAGES",
     422                                 (void*)(vec.data()+416), 416*sizeof(float));
     423    }
     424
     425    void HandleGlobalFeedback()
     426    {
     427        if (fBiasData.getSize()!=1440*sizeof(float))
     428            return;
     429
     430        // -------- Store new event --------
     431
     432        vector<float> arr(reinterpret_cast<float*>(fBiasData.getData()),
     433                          reinterpret_cast<float*>(fBiasData.getData())+1440);
     434
     435        sort(arr.begin(), arr.end());
     436
     437        const float med = arr[arr.size()/2];
     438
     439        fData[fCursor%fData.size()].resize(1); //assign(&med, &med);
     440        fData[fCursor%fData.size()][0] = med;  //assign(&med, &med);
     441
     442        if (++fCursor<fData.size())
     443            return;
     444
     445        // -------- Calculate statistics --------
     446
     447        double avg=0;
     448        double rms=0;
     449        for (size_t i=0; i<fData.size(); i++)
     450        {
     451            avg += fData[i][0];
     452            rms += fData[i][0]*fData[i][0];
     453        }
     454
     455        avg /= fData.size();
     456        rms /= fData.size();
     457
     458        rms  = sqrt(rms-avg*avg);
     459
     460        // -------- Calculate correction --------
     461
     462        if (fCursor%fData.size()!=0)
     463            return;
     464
     465        Out() << "Amplitude: " << avg << " +- " << rms << endl;
     466
     467        // FIXME: Take out broken / dead boards.
     468
     469        /*
     470        ostringstream out;
     471        out << "New " << fData.size() << " event received: " << fCursor << " / " << setprecision(3) << T21 << "s";
     472        Info(out);
     473        */
     474
     475        if (fPV[0].size()==0)
     476        {
     477            fPV[0].resize(1);
     478            fPV[0] = valarray<double>(&avg, 1);
     479            return;
     480        }
     481
     482        if (fPV[1].size()==0)
     483        {
     484            fPV[1].resize(1);
     485            fPV[1] = valarray<double>(&avg, 1);
     486            return;
     487        }
     488
     489        if (fPV[2].size()==0)
     490        {
     491            fPV[2].resize(1);
     492            fPV[2] = valarray<double>(&avg, 1);
     493            return;
     494        }
     495
     496        fPV[0] = fPV[1];
     497        fPV[1] = fPV[2];
     498
     499        fPV[2].resize(1);
     500        fPV[2] = valarray<double>(&avg, 1);
     501
     502        const double T21 = 1; // feedback is  1s
     503        const double T10 = 1; // feedback is 20s
     504
     505        // => Kp = 0.01 * gain     = 0.00005
     506        // => Ki = 0.8  * gain/20s = 0.00025
     507        // => Kd = 0.1  * gain/20s = 0.00003
     508
     509        /*
     510        fKp = 0;
     511        fKd = 0;
     512        fKi = 0.00003*20;
     513        */
     514
     515        // correction = (fSP[0]-fPV[2])*fKi
     516        const valarray<double> correction = 1./fGain/1000*
     517            (
     518             - (fKp+fKd/T21)*(fPV[2] - fPV[1])
     519             +  fKi*T21*(fSP[0]-fPV[2])
     520             +  fKd/T10*(fPV[1]-fPV[0])
     521            );
     522
     523        Out() << "Correction: " << correction[0] << "V (" << fSP[0] << ")" << endl;
     524
     525        const int nch = BIAS::kNumChannels;
     526
     527        // FIXME: Sanity check!
     528
     529        vector<float> vec;
     530        vec.reserve(2*nch);
     531        vec.insert(vec.begin(),     nch, fPV[2][0]-fSP[0]);
     532        vec.insert(vec.begin()+nch, nch, correction[0]);
     533
     534        fDimDeviation.Update(vec);
     535
     536        if (!fOutputEnabled || fStatusBias.second!=BIAS::kVoltageOn)
     537            return;
     538
     539        Info("Sending global correction to feedback.");
     540        DimClient::sendCommandNB((char*)"BIAS_CONTROL/ADD_REFERENCE_VOLTAGES",
     541                                 (void*)(vec.data()+416), 416*sizeof(float));
     542    }
     543
     544    void infoHandler()
     545    {
     546        DimInfo *curr = getInfo(); // get current DimInfo address
     547        if (!curr)
     548            return;
     549
     550        if (curr==&fBias)
     551        {
     552            fStatusBias = GetNewState(fBias);
     553            return;
     554        }
     555
     556        if (curr==&fFAD)
     557        {
     558            fStatusFAD = GetNewState(fFAD);
     559            return;
     560        }
     561
     562        if (curr==&fFSC)
     563        {
     564            fStatusFSC = GetNewState(fFSC);
     565            return;
     566        }
     567
     568        if (curr==&fDim)
     569        {
     570            fStatusDim = GetNewState(fDim);
     571            fStatusDim.second = curr->getSize()==4 ? curr->getInt() : 0;
     572            return;
     573        }
     574
     575        if (curr==&fCameraTemp)
     576            HandleCameraTemp();
     577
     578        if (curr==&fBiasA && fControlType==kTemp && GetCurrentState()==kStateCalibrating)
     579            HandleCalibration();
     580
     581        if (curr==&fBiasData && fControlType==kFeedback)
     582            HandleFeedback();
     583
     584        if (curr==&fBiasData && fControlType==kFeedbackGlobal)
     585            HandleGlobalFeedback();
    487586    }
    488587
    … …  
    574673    }
    575674
     675    void ResetData()
     676    {
     677        fData.clear();
     678        fData.resize(500);
     679        fCursor = 0;
     680        fStartTime = Time();
     681
     682        fSP = valarray<double>(0., 416);
     683
     684        vector<float> vec(2*BIAS::kNumChannels);
     685        fDimDeviation.Update(vec);
     686
     687        fPV[0].resize(0);
     688        fPV[1].resize(0);
     689        fPV[2].resize(0);
     690
     691        if (fKp==0 && fKi==0 && fKd==0)
     692            Warn("Control loop parameters are all set to zero.");
     693    }
     694
    576695    int StartFeedback()
    577696    {
    … …  
    579698
    580699        fControlType = kFeedback;
     700
     701        return GetCurrentState();
     702    }
     703
     704    int StartFeedbackGlobal()
     705    {
     706        ResetData();
     707        fData.resize(5);
     708
     709        fControlType = kFeedbackGlobal;
    581710
    582711        return GetCurrentState();
    … …  
    587716        if (!CheckEventSize(evt.GetSize(), "StartTempCtrl", 4))
    588717            return kSM_FatalError;
     718
     719        fBiasOffset = evt.GetFloat();
     720        fControlType = kTemp;
    589721
    590722        ostringstream out;
    591723        out << "Starting temperature feedback with an offset of " << fBiasOffset << "V";
    592724        Message(out);
    593 
    594         fBiasOffset = evt.GetFloat();
    595         fControlType = kTemp;
    596725
    597726        return GetCurrentState();
    … …  
    648777            vec[i] = fSP[i] = val;
    649778        fDimReference.Update(vec);
     779
     780        Out() << "New global reference value: " << val << "mV" << endl;
    650781
    651782        return GetCurrentState();
    … …  
    736867        }
    737868*/
    738         if (fControlType==kFeedback)
     869        if (fControlType==kFeedback || fControlType==kFeedbackGlobal)
    739870            return fOutputEnabled ? kStateFeedbackCtrlRunning : kStateFeedbackCtrlIdle;
    740871
    … …  
    809940            ("Start the feedback control loop");
    810941
     942        AddEvent("START_GLOBAL_FEEDBACK", kStateConnected)
     943            (bind(&StateMachineFeedback::StartFeedbackGlobal, this))
     944            ("Start the global feedback control loop");
     945
    811946        AddEvent("START_TEMP_CONTROL", "F:1", kStateConnected)
    812947            (bind(&StateMachineFeedback::StartTempCtrl, this, placeholders::_1))
    … …  
    8741009        }
    8751010
    876         //                    -110 / -110 (-23 DAC / -0.51V)
    877         // Reference voltage: -238 / -203
    878         //                    -360 / -343 ( 23 DAC /  0.51V)
    879 
    880         // 0.005 A/V
    881         // 220 Amplitude / 1V
    882 
    883         // Gain = 1V / 200 = 0.005
    884 
    885         fGain = 5; // (BIAS)V / (DRS)V     ( 1V / 0.22V )
     1011        fGain = 0.1; // V(Amplitude) / V(Bias)
     1012
     1013        // 148 -> 248
     1014
     1015        // 33 : 10s  < 2%
     1016        // 50 :  5s  < 2%
     1017        // 66 :  3s  < 2%
     1018        // 85 :  2s  < 2%
    8861019
    8871020        fKp = 0;
    8881021        fKd = 0;
    889         fKi = 0.12;
     1022        fKi = 0.66;
    8901023        fT  = 1;
     1024
     1025        // Is that independent of the aboslute real amplitude of
     1026        // the light pulser?
    8911027
    8921028        ostringstream msg;
    … …  
    8971033        else
    8981034            msg << "<auto>";
    899         msg << ", Gain(BIAS/DRS)=" << fGain << "V/V";
     1035        msg << ", Gain(DRS/BIAS)=" << fGain << "V/V";
    9001036
    9011037        Message(msg);
Note: See TracChangeset for help on using the changeset viewer.