Changeset 13060 for trunk/FACT++
- Timestamp:
- 03/11/12 13:10:02 (13 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/FACT++/src/feedback.cc
r12994 r13060 54 54 kStateConnectedFAD, 55 55 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 61 63 }; 62 64 … … 66 68 kTemp, 67 69 kFeedback, 68 kFeedbackGlobal 70 kFeedbackGlobal, 71 kCurrents, 69 72 }; 70 73 … … 100 103 vector<vector<float>> fData; 101 104 102 uint64_t fCursor; 105 uint64_t fCursorCur; 106 uint64_t fCursorAmpl; 103 107 104 108 Time fBiasLast; … … 117 121 118 122 double fBiasOffset; 123 124 uint16_t fCurrentRequestInterval; 119 125 120 126 bool fOutputEnabled; … … 160 166 vec[i+416] = diff; 161 167 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 } 164 189 165 190 fDimDeviation.setQuality(fControlType); … … 175 200 } 176 201 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 177 224 void HandleCalibration() 178 225 { 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) 195 231 { 196 232 DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0); 197 233 return; 198 234 } 235 236 if (rc==0) 237 return; 199 238 200 239 fCalibration.resize(416*2); 201 240 for (int i=0; i<416; i++) 202 241 { 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]); 205 244 } 206 245 … … 235 274 // -------- Store new event -------- 236 275 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()) 241 280 return; 242 281 … … 308 347 // 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) ] 309 348 310 if (fCursor %fData.size()>0)349 if (fCursorAmpl%fData.size()>0) 311 350 return; 312 351 … … 322 361 323 362 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"; 325 364 Info(out); 326 365 … … 441 480 const float med = arr[arr.size()/2]; 442 481 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()) 447 486 return; 448 487 … … 464 503 // -------- Calculate correction -------- 465 504 466 if (fCursor %fData.size()!=0)505 if (fCursorAmpl%fData.size()!=0) 467 506 return; 468 507 … … 504 543 fPV[2] = valarray<double>(&avg, 1); 505 544 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 -------- 515 555 516 556 // correction = (fSP[0]-fPV[2])*fKi … … 531 571 const valarray<double> correction = 1./fGain/1000* 532 572 ( 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]) 534 575 ); 535 576 … … 588 629 } 589 630 590 if (curr==&fCameraTemp )631 if (curr==&fCameraTemp && (fControlType==kTemp || fControlType==kCurrents)) 591 632 HandleCameraTemp(); 592 633 593 634 if (curr==&fBiasA && fControlType==kTemp && GetCurrentState()==kStateCalibrating) 594 635 HandleCalibration(); 636 637 if (curr==&fBiasA && (fControlType==kFeedbackGlobal || fControlType==kCurrents)) 638 AverageCurrents(); 595 639 596 640 if (curr==&fBiasData && fControlType==kFeedback) … … 692 736 fData.assign(n>0 ? n : fData.size(), vector<float>(0)); 693 737 694 fCursor = 0; 738 fCursorAmpl = 0; 739 fCursorCur = 0; 740 695 741 fStartTime = Time(); 696 742 … … 705 751 fPV[2].resize(0); 706 752 753 fCurrentsAvg.assign(416, 0); 754 fCurrentsRms.assign(416, 0); 755 707 756 if (fKp==0 && fKi==0 && fKd==0) 708 757 Warn("Control loop parameters are all set to zero."); … … 743 792 ostringstream out; 744 793 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"; 745 821 Message(out); 746 822 … … 815 891 fBiasOffset = -2; 816 892 fControlType = kTemp; 817 fCursor = 0;893 fCursorCur = 0; 818 894 fCurrentsAvg.assign(416, 0); 819 895 fCurrentsRms.assign(416, 0); … … 823 899 824 900 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(); 825 913 } 826 914 … … 901 989 if (fControlType==kTemp) 902 990 { 903 if (GetCurrentState()==kStateCalibrating && fCursor <100)991 if (GetCurrentState()==kStateCalibrating && fCursorCur<100) 904 992 return GetCurrentState(); 905 993 906 994 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; 907 1007 } 908 1008 } … … 938 1038 fDimCalibration("FEEDBACK/CALIBRATION", "F:416;F:416", 939 1039 "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)"), 942 1042 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) 944 1046 { 945 1047 // ba::io_service::work is a kind of keep_alive for the loop. … … 971 1073 AddStateName(kStateFeedbackCtrlIdle, "FeedbackIdle", 972 1074 "Feedback control activated, but voltage output disabled."); 973 974 AddStateName(kStateTempCtrlIdle, "FeedbackIdle", 1075 AddStateName(kStateTempCtrlIdle, "TempCtrlIdle", 975 1076 "Temperature control activated, but voltage output disabled."); 1077 AddStateName(kStateCurrentCtrlIdle, "CurrentCtrlIdle", 1078 "Current control activated, but voltage output disabled."); 976 1079 977 1080 AddStateName(kStateFeedbackCtrlRunning, "FeedbackControl", 978 1081 "Feedback control activated and voltage output enabled."); 979 980 1082 AddStateName(kStateTempCtrlRunning, "TempControl", 981 1083 "Temperature control activated and voltage output enabled."); 982 1084 AddStateName(kStateCurrentCtrlRunning, "CurrentControl", 1085 "Current/Temp control activated and voltage output enabled."); 983 1086 AddStateName(kStateCalibrating, "Calibrating", 984 1087 "Calibrating current offsets."); … … 999 1102 "|offset[V]:Offset from the nominal temperature corrected value in Volts"); 1000 1103 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 1001 1109 // kStateTempCtrlIdle, kStateFeedbackCtrlIdle, kStateTempCtrlRunning, kStateFeedbackCtrlRunning 1002 1110 AddEvent("STOP") … … 1035 1143 (bind(&StateMachineFeedback::CalibrateCurrents, this)) 1036 1144 (""); 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."); 1037 1149 1038 1150 // Verbosity commands … … 1087 1199 Message(msg); 1088 1200 1201 fCurrentRequestInterval = conf.Get<uint16_t>("current-request-interval"); 1202 1089 1203 return -1; 1090 1204 } … … 1106 1220 control.add_options() 1107 1221 ("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.") 1108 1223 ; 1109 1224
Note:
See TracChangeset
for help on using the changeset viewer.