/* * File: Longitudinal.cc * Author: lpiccoli * * Created on November 18, 2010, 13:30 */ #include "Longitudinal.h" #include #include #include #include #include #include #include #include "epicsTime.h" #include "ExecConfiguration.h" #include "MeasurementAverage.h" USING_FF_NAMESPACE using namespace boost::numeric::ublas; /** * Class constructor. Only initializes attributes. * * @author L.Piccoli */ Longitudinal::Longitudinal(std::string algorithmName) : Algorithm(algorithmName), _dl2EnergyActuator(NULL), _firstCalculate(true), _blenBc1Average(BLEN_AVERAGE_ELEMENTS, "BC1 BLEN Average"), _blenBc2Average(BLEN_AVERAGE_ELEMENTS, "BC2 BLEN Average"), _dl1Path(NO_MEASUREMENT), _dl2Path(NO_MEASUREMENT), _checkMeasurementsStats(50, "Long: checkMeasurements"), _selectStatesStats(50, "Long: selectStates"), _configureMatrixStats(50, "Long: confMatrix"), _calculateEnergyCurrentStats(50, "Long: calculateEnergyCurrent"), _updateStatesStats(50, "Long: updateStates"), _updateActuatorsStats (50, "Long: updateActuators") { for (int i = 0; i < MAX_LONGITUDINAL_MEASUREMENTS; ++i) { _measurementStatus[i] = INVALID; } // Initialize current state of the FBCK PV to false (OFF). // The FBCK PV is initialized AFTER the configure() method // is invoked. for (int i = 0; i < MAX_LONGITUDINAL_MEASUREMENTS; ++i) { _actuatorFbckState[i] = false; } _energyAverage[DL1_ENERGY] = new MeasurementAverage(INTEGRAL_AVERAGE_COUNT, "DL1 Energy"); _energyAverage[BC1_ENERGY] = new MeasurementAverage(INTEGRAL_AVERAGE_COUNT, "BC1 Energy"); _energyAverage[BC2_ENERGY] = new MeasurementAverage(INTEGRAL_AVERAGE_COUNT, "BC2 Energy"); _energyAverage[DL2_ENERGY] = new MeasurementAverage(INTEGRAL_AVERAGE_COUNT, "DL2 Energy"); _currentAverage[BC1_CURRENT] = new MeasurementAverage(INTEGRAL_AVERAGE_COUNT, "BC1 Current"); _currentAverage[BC2_CURRENT] = new MeasurementAverage(INTEGRAL_AVERAGE_COUNT, "BC2 Current"); } Longitudinal::~Longitudinal() { for (int i = 0; i < INTEGRAL_ENERGY_ELEMENTS; ++i) { delete _energyAverage[i]; } for (int i = 0; i < INTEGRAL_CURRENT_ELEMENTS; ++i) { delete _currentAverage[i]; } } using namespace boost::numeric::ublas; /** * Initialize algorithm parameters based on data stored in the LoopConfiguration. * * @param configuration pointer to the LoopConfiguration * @return 0 on success, -1 on failure * @author L.Piccoli */ int Longitudinal::configure(LoopConfiguration *configuration, MeasurementSet *measurements, ActuatorSet *actuators, StateSet *states) { _loopConfiguration = configuration; _measurements = measurements; _actuators = actuators; _states = states; // Save reference to actuator A6 for setting DL2 energy limits on every cycle. ActuatorSet::iterator it = _actuators->end(); --it; _dl2EnergyActuator = (*it); // Make sure the correct devices names are in the right slot // in the measurements, actuators and states sets -> order is important! // Clear averages reset(); // F and G matrices must have same dimensions if (_loopConfiguration->_fMatrix.size2() != _loopConfiguration->_gMatrix.size2() || _loopConfiguration->_fMatrix.size1() != _loopConfiguration->_gMatrix.size1()) { _loopConfiguration->_logger << "Dimensions of F and G matrices not " << "the same, can't configure" << Log::flushpv; return -1; } // Multiply F and G matrix (element-by-element) _fgMatrix = boost::numeric::ublas::element_prod(_loopConfiguration->_fMatrix, _loopConfiguration->_gMatrix); // Reset IMAX running average (10 point sample) _loopConfiguration->_logger << "INFO: Loop configured with " << (int) _measurements->size() << " measurements, " << (int) _actuators->size() << " actuators, and " << (int) _states->size() << " states." << Log::flush; _pDelta.resize(LONGITUDINAL_MATRIX_DIMENSION); _iDelta.resize(LONGITUDINAL_MATRIX_DIMENSION); _delta.resize(LONGITUDINAL_MATRIX_DIMENSION); _latestMeasurements.resize(LONGITUDINAL_MATRIX_DIMENSION); _latestMeasurementsSum.resize(LONGITUDINAL_MATRIX_DIMENSION); _usedFgMatrix.resize(LONGITUDINAL_MATRIX_DIMENSION, LONGITUDINAL_MATRIX_DIMENSION); for (int i = 0; i < LONGITUDINAL_MATRIX_DIMENSION; ++i) { _latestMeasurements[i] = 0; _latestMeasurementsSum[i] = 0; } for (int i = 0; i < MAX_LONGITUDINAL_MEASUREMENTS; ++i) { _actuatorFbckState[i] = false; } _firstCalculate = true; return 0; } /** * Clears all averages kept by the algorithm. * * @return always 0 * @author L.Piccoli */ int Longitudinal::reset() { for (int i = 0; i < INTEGRAL_ENERGY_ELEMENTS; ++i) { _energyAverage[i]->clear(); } for (int i = 0; i < INTEGRAL_CURRENT_ELEMENTS; ++i) { _currentAverage[i]->clear(); } _blenBc1Average.clear(); _blenBc2Average.clear(); return 0; } /** * Invokes the calculate() with no parameters. All parameters passed * to this method have already been passed through configure(). * * @author L.Piccoli */ int Longitudinal::calculate(LoopConfiguration &configuration, MeasurementSet &measurements, ActuatorSet &actuators, StateSet &states) { return calculate(); }; /** * Implementation of the Longitudinal algorithm. * * * @return 0 on success, -1 on failure * @author L.Piccoli */ int Longitudinal::calculate() { // Save state of actuators FBCK PV on the first call to calculate() if (_firstCalculate) { int i = 0; for (ActuatorSet::iterator it = _actuators->begin(); it != _actuators->end() && i < MAX_LONGITUDINAL_MEASUREMENTS; ++it, ++i) { _actuatorFbckState[i] = (*it)->getFeedbackPv(); } _firstCalculate = false; } // First verify current energy and calculate actuator limits. checkActuatorLimits(); // Gather current measurements and select state based on readings // and USED PVs. if (checkConfiguration() != 0) { return -1; } // Zero out rows/cols of those states not in use _configureMatrixStats.start(); configureMatrix(); _configureMatrixStats.end(); _calculateEnergyCurrentStats.start(); if (calculateEnergiesAndCurrents() != 0) { _calculateEnergyCurrentStats.end(); return -1; } _calculateEnergyCurrentStats.end(); // Calculate states based on the energies and currents _updateStatesStats.start(); if (updateStates() != 0) { _updateStatesStats.end(); return -1; } _updateStatesStats.end(); // Calculate new actuator values _updateActuatorsStats.start(); if (updateActuators() != 0) { _updateActuatorsStats.end(); return ACTUATOR_LIMIT; } _updateActuatorsStats.end(); return 0; } /** * Verify the current energy and set the correct limits for L3. * * @author L.Piccoli */ void Longitudinal::checkActuatorLimits() { double loloLimit = ExecConfiguration::getInstance()._dl2EnLoloPv.getValue(); double hihiLimit = ExecConfiguration::getInstance()._dl2EnHihiPv.getValue(); if (_dl2EnergyActuator != NULL) { _dl2EnergyActuator->setLimits(hihiLimit, loloLimit); } } /** * Collect current measurements and select states based on the readings and * user configuration (SxxUSED PVs) * * @return 0 on success, -1 on failure * @author L.Piccoli */ int Longitudinal::checkConfiguration() { // Check which measurements are valid _checkMeasurementsStats.start(); if (checkMeasurementStatus() != 0) { _checkMeasurementsStats.end(); return -1; } _checkMeasurementsStats.end(); // Check the USEDBY PV for the states based on the measurement status // and SxxUSED PVs _selectStatesStats.start(); if (selectStates() != 0) { _selectStatesStats.end(); return -1; } _selectStatesStats.end(); // Check if beam has changed direction and clean averages if so. setPaths(); return 0; } /** * Generate an F.*G matrix with rows/cols zeroed for those states not in * use for the current cycle. * * @author L.Piccoli */ void Longitudinal::configureMatrix() { StateSet::iterator stateRowIt = _states->begin(); for (int i = 0; i < (int) _fgMatrix.size1(); ++i, ++stateRowIt) { StateDevice *rowState = (*stateRowIt); StateSet::iterator stateColIt = _states->begin(); for (int j = 0; j < (int) _fgMatrix.size2(); ++j, ++stateColIt) { StateDevice *colState = (*stateColIt); if (!rowState->getUsedBy()) { _usedFgMatrix(i, j) = 0; } else if (!colState->getUsedBy()) { _usedFgMatrix(i, j) = 0; } else { _usedFgMatrix(i, j) = _fgMatrix(i, j); } } } } /** * Calculate the energies and currents at DL1, BC1, BC2 and DL2. * * Energies and currents are stored in the _energyAverage and _currentAverage * attributes. * * @return always 0 * @author L.Piccoli */ int Longitudinal::calculateEnergiesAndCurrents() { StateSet::iterator stateIt = _states->begin(); MeasurementSet::iterator measurementIt = _measurements->begin(); // Energy at DL1 MeasurementDevice *m1 = *measurementIt; ++measurementIt; // Go to M2 MeasurementDevice *m2 = *measurementIt; double res = calculateEnergyDl1(m1, m2, *stateIt); if (std::isnan(res)) { return -1; } // Energy at BC1 ++measurementIt; // Go to M3 ++stateIt; // S2 res = calculateEnergyBc1(*measurementIt, *stateIt); if (std::isnan(res)) { return -1; } // Current at BC1 ++measurementIt; // Go to M4, it's a BLEN ++stateIt; // Go to S3 res = calculateCurrentBc1(*stateIt); if (std::isnan(res)) { return -1; } // Energy at BC2 ++measurementIt; // Go to M5 ++stateIt; // Go to S4 res = calculateEnergyBc2(*measurementIt, *stateIt); if (std::isnan(res)) { return -1; } // Current at BC2 ++measurementIt; // Go to M6, it's a BLEN ++stateIt; // Go to S5 res = calculateCurrentBc2(*stateIt); if (std::isnan(res)) { return -1; } // Energy at DL2 ++measurementIt; // Go to M7 MeasurementDevice *m7 = *measurementIt; ++measurementIt; // Go to M8 MeasurementDevice *m8 = *measurementIt; ++measurementIt; // Go to M9 MeasurementDevice *m9 = *measurementIt; ++measurementIt; // Go to M10 MeasurementDevice *m10 = *measurementIt; ++stateIt; // Go to S6 res = calculateEnergyDl2(m7, m8, m9, m10, *stateIt); if (std::isnan(res)) { return -1; } return 0; } /** * Calculate the energy at DL1. Beam must be either going through M1 or M2. * This method should not be called if there is no beam. * * @param m1 BPMS:IN20:731 * @param m2 BPMS:IN20:981 * @param s1 State at DL1, used to retrieve the S1 setpoint * @return energy at DL1 * @author L.Piccoli */ double Longitudinal::calculateEnergyDl1(MeasurementDevice *m1, MeasurementDevice *m2, StateDevice *s1) { if (!s1->getUsedBy()) { _latestMeasurements[0] = 0; _latestMeasurementsSum[0] = 0; return 0; } double dispersion = 0; MeasurementDevice *measurement = m1; // Do we have beam on M1? if (_measurementStatus[M1] == VALID) { dispersion = _loopConfiguration->_dispersions[M1]; }// If not on M1 it must be going through M2 else { measurement = m2; dispersion = _loopConfiguration->_dispersions[M2]; } double energy = (measurement->peek() / dispersion + 1) * (135.0 / s1->getSetpoint()) - 1; _latestMeasurements[0] = energy; _energyAverage[DL1_ENERGY]->add(energy); _latestMeasurementsSum[0] = _energyAverage[DL1_ENERGY]->getSum(); if (std::isnan(_latestMeasurementsSum[0])) { // FIXME: DEBUG std::cout << "*** DL1 - std::isnan(_latestMeasurementsSum[0]) (head=" << _energyAverage[DL1_ENERGY]->getHead() << ", sum=" << _energyAverage[DL1_ENERGY]->getSum() << ", M1/2 = " << measurement->peek() << ", energy=" << energy << ")" << std::endl; return _latestMeasurementsSum[0]; } return energy; } /** * Calculate the energy at BC1. Beam must be going through BC1. * * The calculated energy is added to the _energyAverage[BC1_ENERGY], which * mainains an average of the last 3 calculated energies. * * @param m3 BPMS:LI21:233 * @param s2 State at BC1 energy, used to retrieve S2 setpoint * @return energy at BC1 * @author L.Piccoli */ double Longitudinal::calculateEnergyBc1(MeasurementDevice* m3, StateDevice* s2) { if (!s2->getUsedBy()) { _latestMeasurements[1] = 0; _latestMeasurementsSum[1] = 0; } double energy = (m3->peek() / _loopConfiguration->_dispersions[M3] + 1) * (220.0 / s2->getSetpoint()) - 1; _latestMeasurements[1] = energy; _energyAverage[BC1_ENERGY]->add(energy); _latestMeasurementsSum[1] = _energyAverage[BC1_ENERGY]->getSum(); if (std::isnan(_latestMeasurementsSum[1])) { // FIXME: DEBUG std::cout << "*** BC1 Energy - std::isnan(_latestMeasurementsSum[1])" << " (head=" << _energyAverage[BC1_ENERGY]->getHead() << ", sum=" << _energyAverage[BC1_ENERGY]->getSum() << ", M3 = " << m3->peek() << ", energy=" << energy << ")" << std::endl; // FIXME: DEBUG return _latestMeasurementsSum[1]; } return energy; } /** * Calculate the current at BC1 based on the average of the last 10 BLEN readings. * The average is retrieved from the _blenBc1Average attribute, that's why there * is no MeasurementDevice* parameter as in the calculateEnergy*() methods. * * The calculated current is added to the _currentAverage[BC1_CURRENT], which * mainains an average of the last 3 calculated energies. * * @param s3 State at BC1 current, used to retrieve S3 setpoint * @return current at BC1 * @author L.Piccoli */ double Longitudinal::calculateCurrentBc1(StateDevice *s3) { if (!s3->getUsedBy()) { _latestMeasurements[2] = 0; _latestMeasurementsSum[2] = 0; } double sign = 0; double setpoint = s3->getSetpoint(); if (setpoint > 0) { sign = 1; } else if (setpoint < 0) { sign = -1; } double current = (_blenBc1Average.getAverage() - std::abs(setpoint)) / std::abs(setpoint); current *= sign; _latestMeasurements[2] = current; _currentAverage[BC1_CURRENT]->add(current); _latestMeasurementsSum[2] = _currentAverage[BC1_CURRENT]->getSum(); if (std::isnan(_latestMeasurementsSum[2])) { // FIXME: DEBUG std::cout << "*** BC1 Current - std::isnan(_latestMeasurementsSum[2])" << " (head=" << _currentAverage[BC1_CURRENT]->getHead() << ", sum=" << _currentAverage[BC1_CURRENT]->getSum() << ", M4=" << _blenBc1Average.getLatestValue() << ", M4AVG=" << _blenBc1Average.getAverage() << ", current=" << current << ")" << std::endl; // FIXME: DEBUG return _latestMeasurementsSum[2]; } return current; } /** * Calculate the energy at BC1. Beam must be going through BC1. * * The calculated energy is added to the _energyAverage[BC1_ENERGY], which * mainains an average of the last 3 calculated energies. * * @param m5 BPMS:LI24:801 * @param s4 State at BC1 energy, used to retrieve S2 setpoint * @return energy at BC1 * @author L.Piccoli */ double Longitudinal::calculateEnergyBc2(MeasurementDevice* m5, StateDevice* s4) { if (!s4->getUsedBy()) { _latestMeasurements[3] = 0; _latestMeasurementsSum[3] = 0; } double energy = (m5->peek() / _loopConfiguration->_dispersions[M5] + 1) * (4700.0 / s4->getSetpoint()) - 1; _latestMeasurements[3] = energy; _energyAverage[BC2_ENERGY]->add(energy); _latestMeasurementsSum[3] = _energyAverage[BC2_ENERGY]->getSum(); if (std::isnan(_latestMeasurementsSum[3])) { // FIXME: DEBUG std::cout << "*** BC2 Energy - std::isnan(_latestMeasurementsSum[3])" << " (head=" << _energyAverage[BC2_ENERGY]->getHead() << ", sum=" << _energyAverage[BC2_ENERGY]->getSum() << ", M5 = " << m5->peek() << ", energy=" << energy << ")" << std::endl; // FIXME: DEBUG return _latestMeasurementsSum[3]; } return energy; } /** * Calculate the current at BC2 based on the average of the last 10 BLEN readings. * The average is retrieved from the _blenBc2Average attribute, that's why there * is no MeasurementDevice* parameter as in the calculateEnergy*() methods. * * The calculated current is added to the _currentAverage[BC2_CURRENT], which * mainains an average of the last 3 calculated energies. * * @param s5 State at BC2 current, used to retrieve S3 setpoint * @return current at BC2 * @author L.Piccoli */ double Longitudinal::calculateCurrentBc2(StateDevice *s5) { if (!s5->getUsedBy()) { _latestMeasurements[4] = 0; _latestMeasurementsSum[4] = 0; } double sign = 0; double setpoint = s5->getSetpoint(); if (setpoint > 0) { sign = 1; } else if (setpoint < 0) { sign = -1; } double current = (_blenBc2Average.getAverage() - std::abs(setpoint)) / std::abs(setpoint); current *= sign; _latestMeasurements[4] = current; _currentAverage[BC2_CURRENT]->add(current); _latestMeasurementsSum[4] = _currentAverage[BC2_CURRENT]->getSum(); if (std::isnan(_latestMeasurementsSum[4])) { // FIXME: DEBUG std::cout << "*** BC2 Curr - std::isnan(_latestMeasurementsSum[4])" << " (head=" << _currentAverage[BC2_CURRENT]->getHead() << ", sum=" << _currentAverage[BC2_CURRENT]->getSum() << ", M6=" << _blenBc2Average.getLatestValue() << ", M6AVG=" << _blenBc2Average.getAverage() << ", current=" << current << ")" << std::endl; // FIXME: DEBUG return _latestMeasurementsSum[4]; } return current; } /** * Calculate the energy at DL2. Beam must be either to BSY, BYKIK or * LTU. * * @param m7 BPMS:BSY0:52 * @param m8 BPMS:LTU1:250 * @param m9 BPMS:LTU1:450 * @param m10 BPMS:LTU0:170 * @param s6 State at DL2 energy, used to retrieve S6 setpoint * @return energy at DL2 * @author L.Piccoli */ double Longitudinal::calculateEnergyDl2(MeasurementDevice* m7, MeasurementDevice* m8, MeasurementDevice* m9, MeasurementDevice* m10, StateDevice* s6) { if (!s6->getUsedBy()) { _latestMeasurements[5] = 0; _latestMeasurementsSum[5] = 0; } double energy = 0; double by1Bdes = ExecConfiguration::getInstance()._by1BdesPv.getValue(); by1Bdes *= 1000; // Beam is going through BSY if (_measurementStatus[M7] == VALID) { energy = (m7->peek() / _loopConfiguration->_dispersions[M7] + 1) * (by1Bdes / s6->getSetpoint()) - 1; } else { // Energy problem, trying to recover with M10 if (_measurementStatus[M10] == M10ENERGY) { energy = (m10->peek() / _loopConfiguration->_dispersions[M10] + 1) * (by1Bdes / s6->getSetpoint()) - 1; } else { // Beam is going through BYKIK if (_measurementStatus[M8] == VALID && _measurementStatus[M9] == INVALID) { energy = (m8->peek() / _loopConfiguration->_dispersions[M8] + 1) * (by1Bdes / s6->getSetpoint()) - 1; } else { // Beam is going through LTU energy = m8->peek() / _loopConfiguration->_dispersions[M8]; energy += m9->peek() / _loopConfiguration->_dispersions[M9]; energy /= 2; energy += 1; energy *= (by1Bdes / s6->getSetpoint()); energy -= 1; } } } _latestMeasurements[5] = energy; _energyAverage[DL2_ENERGY]->add(energy); _latestMeasurementsSum[5] = _energyAverage[DL2_ENERGY]->getSum(); if (std::isnan(_latestMeasurementsSum[5])) { // FIXME: DEBUG std::cout << "*** DL2 - std::isnan(_latestMeasurementsSum[5])" << " (head=" << _energyAverage[DL2_ENERGY]->getHead() << ", sum=" << _energyAverage[DL2_ENERGY]->getSum() << ", M7=" << m7->peek() << ", M8=" << m8->peek() << ", M9=" << m9->peek() << ", M10=" << m10->peek() << ", energy=" << energy << ")" << std::endl; // FIXME: DEBUG return _latestMeasurementsSum[5]; } return energy; } /** * Calculate states for DL1, BC1, BC2 and DL2 based on the latest calculated * energies and currents. * * If a state is not used a new value is not calculated. * * @return always 0 * @author L.Piccoli */ int Longitudinal::updateStates() { StateSet::iterator stateIt = _states->begin(); // Energy at DL1 calculateState(*stateIt, _energyAverage[DL1_ENERGY]->getLatestValue()); // Energy at BC1 ++stateIt; calculateState(*stateIt, _energyAverage[BC1_ENERGY]->getLatestValue()); // Current at BC1 ++stateIt; calculateState(*stateIt, _currentAverage[BC1_CURRENT]->getLatestValue()); // Energy at BC2 ++stateIt; calculateState(*stateIt, _energyAverage[BC2_ENERGY]->getLatestValue()); // Current at BC2 ++stateIt; calculateState(*stateIt, _currentAverage[BC2_CURRENT]->getLatestValue()); // Energy at DL2 ++stateIt; calculateState(*stateIt, _energyAverage[DL2_ENERGY]->getLatestValue()); return 0; } /** * Calculate a new state value based on the latest energy or current. The state * value is computed only if the state is in use, i.e. there is beam going * through the section and the user has selected the state. * * @param state pointer to the state device to be updated. * @param latestValue lastest energy or current * @return always 0 * @author L.Piccoli */ int Longitudinal::calculateState(StateDevice *state, double latestValue) { if (state->getUsedBy()) { double stateValue = 1 + latestValue; stateValue *= state->getSetpoint(); state->set(stateValue); } return 0; } /** * Verify which measurements are currently valid. The _measurementStatus * array is updated accordingly. * * A BPM measurement is VALID if TMIT is above the threshold (TMITLOW PV). * * A BLEN measurement is VALID if IMAX is greater than ZERO. * * These are all the measurement devices in the Longitudinal feedback: * * M1 BPMS:IN20:731:X --\ * M2 BPMS:IN20:981:X ---+---> Beam goes through one BPM or the other * M3 BPMS:LI21:233:X * M4 BLEN:LI21:265:AIMAXF2 * M5 BPMS:LI24:801:X * M6 BLEN:LI24:886:BIMAXF2 * M7 BPMS:BSY0:52:X * M8 BPMS:LTU1:250:X ---+---> These two BPMs are combined * M9 BPMS:LTU1:450:X --/ * M10 BPMS:LTU0:170:Y -> Used when there are problems setting up the energy * * Additionally, this method keeps track on whether the beam is going * through different beam lines at DL1 and BSY. If there is a change * then the DL1 and DL2 integral energies must be cleared out. * * @return 0 if at least one measurement device is valid, -1 if all have * invalid measurements * @author L.Piccoli */ int Longitudinal::checkMeasurementStatus() { MeasurementDevice *measurementDevice; double value; epicsTimeStamp timestamp; int i = 0; bool allInvalid = false;//true; for (MeasurementSet::iterator it = _measurements->begin(); it != _measurements->end() && i < MAX_LONGITUDINAL_MEASUREMENTS; ++it, ++i) { measurementDevice = (*it); measurementDevice->get(value, timestamp); _measurementStatus[i] = INVALID; if (measurementDevice->isBpm()) { if (measurementDevice->getTmit() > ExecConfiguration::getInstance()._tmitLowPv.getValue()) { _measurementStatus[i] = VALID; allInvalid = false; } } else if (measurementDevice->isBlen()) { if (value > 0 && !std::isnan(value)) { _measurementStatus[i] = VALID; allInvalid = false; // Copy latest BLEN values to the appropriate average holder if (i == M4) { _blenBc1Average.add(value); } else if (i == M6) { _blenBc2Average.add(value); } } } measurementDevice->setUsedBy(_measurementStatus[i]); } if (allInvalid) { _loopConfiguration->_logger << "No valid measurements, no beam" << Log::flushpvonly; return -1; } else { return 0; } } /** * Select the states to be used in the current cycle based on the status * of BPM/BLEN measurements. If a state is used the USEDBY PV is set to true. * * These are the state devices: * * S1 DL1.Energy - USEDBY defined by S1USED & M1 TMIT or M2 TMIT * S2 BC1.Energy - USEDBY defined by S2USED & M3 TMIT * S3 BC1.Current - USEDBY defined by S3USED & M4 IMAX * S4 BC2.Energy - USEDBY defined by S4USED & M5 TMIT * S5 BC2.Current - USEDBY defined by S5USED & M6 IMAX * S6 DL2.Energy - USEDBY defined by S6USED & M7-M8/M9/M10 TMITs * * If a state USED PV and the actuator FBCK PV have different values * the state of the FBCK PV changes to the value given by the state USED PV. * This allows actuators to be released from the feedback control once the * operators disable the equivalent state calculation. * * @return 0 on success, -1 if the measurements status do not reflect a valid * configuration (e.g. TMIT of M7 and M8 are above TMITLOW PV) * @author L.Piccoli */ int Longitudinal::selectStates() { setActuatorFeedbackPvs(); StateSet::iterator it = _states->begin(); StateDevice *stateDevice; ActuatorSet::iterator actIt = _actuators->begin(); ActuatorDevice *actuatorDevice; bool stateUsed = false; bool hasState = false; // M1 and M2 define whether S1 is enabled stateDevice = (*it); actuatorDevice = (*actIt); if ((_measurementStatus[M1] == VALID || _measurementStatus[M2] == VALID) && stateDevice->isUsed()) { stateUsed = true; hasState = true; } stateDevice->setUsedBy(stateUsed); actuatorDevice->setUsedBy(stateUsed); // M3 defines if S2 is enabled ++it; stateDevice = (*it); ++actIt; actuatorDevice = (*actIt); stateUsed = false; if (_measurementStatus[M3] == VALID && stateDevice->isUsed()) { stateUsed = true; hasState = true; } stateDevice->setUsedBy(stateUsed); actuatorDevice->setUsedBy(stateUsed); // M3 also defines if S3 is enabled // We cannot rely on M4 BLEN measurement ++it; stateDevice = (*it); ++actIt; actuatorDevice = (*actIt); stateUsed = false; if (_measurementStatus[M3] == VALID && stateDevice->isUsed()) { stateUsed = true; hasState = true; } stateDevice->setUsedBy(stateUsed); actuatorDevice->setUsedBy(stateUsed); // M5 defines if S4 is enabled ++it; stateDevice = (*it); ++actIt; actuatorDevice = (*actIt); stateUsed = false; if (_measurementStatus[M5] == VALID && stateDevice->isUsed()) { stateUsed = true; hasState = true; } stateDevice->setUsedBy(stateUsed); actuatorDevice->setUsedBy(stateUsed); // M5 defines if S5 is enabled // We cannot rely on M6 BLEN measurement ++it; stateDevice = (*it); ++actIt; actuatorDevice = (*actIt); stateUsed = false; if (_measurementStatus[M5] == VALID && stateDevice->isUsed()) { stateUsed = true; hasState = true; } stateDevice->setUsedBy(stateUsed); actuatorDevice->setUsedBy(stateUsed); // Now figure out the USEDBYLOOP PV for the last state (L3). // It is defined by the state of three BPMs: M7, M8 and M10 // Beam must be going though at least one ofe the above BPMs, // but not through all. If low tmit is present on all, // L3 state is off. ++it; stateDevice = (*it); ++actIt; actuatorDevice = (*actIt); stateUsed = true; if (_measurementStatus[M7] == VALID && _measurementStatus[M8] == VALID) { stateDevice->setUsedBy(false); return -1; } if (_measurementStatus[M7] == INVALID && _measurementStatus[M8] == INVALID) { if (_measurementStatus[M10] == VALID) { stateUsed = true; _measurementStatus[M10] = M10ENERGY; } else { stateUsed = false; } } if (!stateDevice->isUsed()) { stateUsed = false; } if (stateUsed) { hasState = true; } stateDevice->setUsedBy(stateUsed); actuatorDevice->setUsedBy(stateUsed); // Return error if there is no State Device in use! if (!hasState) { return -1; } return 0; } /** * If a state USED PV and the actuator FBCK PV have different values * the state of the FBCK PV changes to the value given by the state USED PV. * This allows actuators to be released from the feedback control once the * operators disable the equivalent state calculation. * * @author L.Piccoli */ void Longitudinal::setActuatorFeedbackPvs() { int i = 0; StateSet::iterator sIt = _states->begin(); for (ActuatorSet::iterator aIt = _actuators->begin(); aIt != _actuators->end() && sIt != _states->end() && i < MAX_LONGITUDINAL_MEASUREMENTS; ++aIt, ++sIt, ++i) { if (_actuatorFbckState[i] != (*sIt)->isUsed()) { _actuatorFbckState[i] = (*sIt)->isUsed(); (*aIt)->setFeedbackPv(_actuatorFbckState[i]); } } } /** * Update the values for the actuators using latests energies and currents. * * Changes in actuator values are added to the last values successfully written * to the ActuatorDevices. * * @return 0 if all actuator values are within limits, -1 if at least * one of the values is outside the limits * @author L.Piccoli */ int Longitudinal::updateActuators() { bool debug = false; if (debug) { std::cout << std::endl; std::cout << "Meas: " << _latestMeasurements << std::endl; std::cout << "MeasSum: " << _latestMeasurementsSum << std::endl; std::cout << "Matrix: " << _usedFgMatrix << std::endl; } // Proportional _pDelta = prod(_usedFgMatrix, _latestMeasurements); if (debug) { std::cout << "PDelta: " << _pDelta << std::endl; } // Integral _iDelta = prod(_usedFgMatrix, _latestMeasurementsSum); if (debug) { std::cout << "IDelta: " << _iDelta << std::endl; } double pGain = _loopConfiguration->_pGainPv.getValue(); double iGain = _loopConfiguration->_iGainPv.getValue(); _delta = (_pDelta * pGain); _delta += (_iDelta * iGain); if (debug) { std::cout << "Delta: " << _delta << std::endl; } // Fill in the current time and pulseId epicsTimeStamp timestamp; epicsTimeGetCurrent(×tamp); #ifdef RTEMS evrTimePutPulseID(×tamp, _loopConfiguration->_pulseIdPv.getValue()); #endif // Update actuators bool actuatorLimit = false; StateSet::iterator stateIt = _states->begin(); int i = 0; if (debug) { std::cout << "Actuators: "; } for (ActuatorSet::iterator it = _actuators->begin(); it != _actuators->end(); ++it, ++i, ++stateIt) { ActuatorDevice *actuator = (*it); double value = actuator->getLastValueSet(); if (debug) { std::cout << value << " "; } if ((*stateIt)->getUsedBy()) { // Change the actuator value only if MODE PV is ENABLE (true) if (_loopConfiguration->_mode == true) { value -= _delta[i]; if (debug) { std::cout << "[" << value - _delta[i] << "], "; } } if (actuator->set(value, timestamp) != 0) { actuatorLimit = true; } } else { actuator->set(value, DataPoint::SET_SKIP); } } if (debug) { std::cout << std::endl; } if (actuatorLimit) { return -1; } return 0; } /** * Check throught which BPMs the beam is passing through and set the * _dl1Path and _dl2Path accordingly. * * @author L.Piccoli */ void Longitudinal::setPaths() { // DL1 Path if (_measurementStatus[M1] == VALID) { if (_dl1Path != M1) { _energyAverage[DL1_ENERGY]->clear(); } _dl1Path = M1; } else if (_measurementStatus[M2] == VALID) { if (_dl1Path != M2) { _energyAverage[DL1_ENERGY]->clear(); } _dl1Path = M2; } else { _dl1Path = NO_MEASUREMENT; _energyAverage[DL1_ENERGY]->clear(); } // DL2 Path if (_measurementStatus[M7] == VALID) { if (_dl2Path != M7) { _energyAverage[DL2_ENERGY]->clear(); } _dl2Path = M7; } else if (_measurementStatus[M8] == VALID || _measurementStatus[M9] == VALID) { if (_dl2Path != M8) { _energyAverage[DL2_ENERGY]->clear(); } _dl2Path = M8; } else { if (_measurementStatus[M10] == VALID) { _dl2Path = M10; _energyAverage[DL2_ENERGY]->clear(); } else { _dl2Path = NO_MEASUREMENT; _energyAverage[DL2_ENERGY]->clear(); } } } void Longitudinal::show() { if (_dl1Path == NO_MEASUREMENT) { std::cout << " -> No beam" << std::endl; } else if (_dl1Path == M2) { std::cout << " -> Beam dump (going through M2)" << std::endl; } else { std::cout << " -> Beam through M1 "; if (_dl2Path == M7) { std::cout << "and M7 to A-Line"; } else if (_dl2Path == M8 && _measurementStatus[M9] == INVALID) { std::cout << "and to BYKIK"; } else if (_dl2Path == M8 && _measurementStatus[M9] == VALID) { std::cout << "and to LTU"; } else if (_dl2Path == M10 && _measurementStatus[M10] == VALID) { std::cout << "through BPMVB3, not reaching LTU"; } else { std::cout << "only"; } std::cout << std::endl; } std::cout << " / M3 \\ / M5 \\ /- M7 ------->> " << std::endl; std::cout << " - (S1) -+-- M1 (S2) -/ \\- M4 (S4) (S5) -/ \\- M6 (S6) -+-- M10 - M8 +- M9 ->> " << std::endl; std::cout << " \\- M2 --->> \\----->> " << std::endl; std::cout << " -> Measurement status: "; for (int i = 0; i < MAX_LONGITUDINAL_MEASUREMENTS; ++i) { std::cout << "M" << i+1; if (_measurementStatus[i] == VALID) { std::cout << "+ "; } else { std::cout << "- "; } } std::cout << std::endl; std::cout << " -> States used: "; for (StateSet::iterator it = _states->begin(); it != _states->end(); ++it) { if ((*it)->getUsedBy()) { std::cout << (*it)->getName() << " "; } } std::cout << std::endl; } /** * Show time statistics. * * @author L.Piccoli */ void Longitudinal::showDebug() { _checkMeasurementsStats.show(" "); _selectStatesStats.show(" "); _configureMatrixStats.show(" "); _calculateEnergyCurrentStats.show(" "); _updateStatesStats.show(" "); _updateActuatorsStats.show(" "); }