/* * File: Loop.cc * Author: lpiccoli * * Created on June 7, 2010, 3:29 PM */ #include "Loop.h" #include "TimeUtil.h" #include "ExecConfiguration.h" #include USING_FF_NAMESPACE /** * Loop constructor specifying the LoopConfiguration, PatternMask and Algorithm. * The set of measurements, actuators and states is retrieved from the * LoopConfiguration in the constructor. * * TODO: remove this constructor * * @param configuration configuration for this loop * @param patternMask the pattern mask for this loop * @param algorithm algorithm used by this loop for the specified pattern mask * to calculate new actuator and state values * @author L.Piccoli */ Loop::Loop(LoopConfiguration *configuration, PatternMask patternMask, Algorithm *algorithm) : _configuration(configuration), _patternMask(patternMask), _algorithm(algorithm), _calculateCount(0), _calculateFailCount(0), _deviceFailCount(0), _actuatorsSkipCount(0), _lowTmitCount(0), _pulseIdMismatchCount(0), _actuatorMismatchCount(0), _calculateStats(50, "Loop Processing Stats (calculate method)"), _skipTmit(false), _firstPass(true), _previousHadBeam(false) { } /** * Loop constructor specifying the LoopConfiguration and PatternMask. * The set of measurements, actuators and states is retrieved from the * LoopConfiguration in the constructor. * * The Algorithm used by this loop is retrieved from the LoopConfiguration * based on the PatternMask. * * @param configuration configuration for this loop * @param patternMask the pattern mask for this loop * @author L.Piccoli */ Loop::Loop(LoopConfiguration *configuration, PatternMask patternMask) : _configuration(configuration), _patternMask(patternMask), _calculateCount(0), _calculateFailCount(0), _deviceFailCount(0), _actuatorsOutsideLimitsCount(0), _measurementsOutsideLimitsCount(0), _actuatorsSkipCount(0), _lowTmitCount(0), _calculateStats(50, "Loop Processing Stats"), _skipTmit(false), _firstPass(true), _previousHadBeam(false) { // Get the algorithm and configure it configureAlgorithm(); } /** * Configure the loop - calls Loop::reconfigure() passing its own _patternMask * as parameter. * * @return 0 on success -1 on failure * @author L.Piccoli */ int Loop::configureAlgorithm() { return reconfigure(_patternMask); } /** * Configure the loop using the passed patternMask. This method selects only * those devices that are enabled (USED PV) to be added to the list of * measurements, actuators and states used for the feedback calculations. * * The selected measurements are also added to the MeasurementCollector * singleton, which reads new values at the beginning of each feedback cycle. * * Finally, the algorithm is also configured, i.e. the selected devices are * passed down to the algorithm. * * @return 0 on success, -1 on failure * @author L.Piccoli */ int Loop::reconfigure(PatternMask patternMask) { _patternMask = patternMask; _configuration->_logger << "Reconfiguring Loop " << _configuration->_loopNamePv.getValue().c_str() << Log::flushpvnoalarm; // Select only the subset of devices that are actually used // Select MeasurementDevices if (selectUsedDevices (_measurements, _configuration->_measurements, CommunicationChannel::READ_ONLY, _configuration->_measNumPv.getValue()) != 0) { return -1; } // Select ActuatorDevices if (selectUsedDevices (_actuators, _configuration->_actuators, CommunicationChannel::WRITE_ONLY, _configuration->_actNumPv.getValue()) != 0) { return -1; } // Select StateDevices #ifdef LINUX CommunicationChannel::AccessType accessType = CommunicationChannel::NO_CHANNEL; #else // If code is compiled for RTEMS use FCOM channel to output state values CommunicationChannel::AccessType accessType = CommunicationChannel::WRITE_ONLY; #endif if (selectUsedDevices (_states, _configuration->_states, accessType, _configuration->_stateNumPv.getValue()) != 0) { return -1; } // Add selected measurement devices to the MeasurementCollector list // TODO: can't we remove only those not used and add only the new ones? //*** MeasurementCollector LOCK // MeasurementCollector::getInstance().lock(); << Moved to LoopThread::reconfigure() MeasurementSet::iterator it; for (it = _measurements.begin(); it != _measurements.end(); ++it) { if (MeasurementCollector::getInstance().add(*it) != 0) { _configuration->_logger << "ERROR: Failed to add device to collector" << Log::flush; } } // MeasurementCollector::getInstance().unlock(); //*** MeasurementCollector UNLOCK // Get the algorithm and configure it AlgorithmMap::iterator algoIterator; algoIterator = _configuration->_algorithms.find(patternMask); if (algoIterator == _configuration->_algorithms.end()) { _algorithm = NULL; } else { _algorithm = algoIterator->second; _algorithm->configure(_configuration, &_measurements, &_actuators, &_states); } // If measurements are read from files, skip TMIT checking if (_measurements.size() > 0) { MeasurementSet::iterator it; it = _measurements.begin(); MeasurementDevice *measurementDevice; measurementDevice = *it; if (measurementDevice->isFile()) { _skipTmit = true; } } // Flag calculate() to get the latest measurements _firstPass = true; return 0; } /** * Template method used for selecting only those devices that have been selected * durning configuration time. The selected devices are added to the specified * device set. * * The selected devices also are configured, i.e. the proper communication * channel is instantiated. * * @return 0 on success, -1 on failure * @author L.Piccoli */ template int Loop::selectUsedDevices(DeviceSet &deviceSet, DeviceMap &deviceMap, CommunicationChannel::AccessType accessType, long maxDevices) { DeviceMapIterator mapIterator; mapIterator = deviceMap.find(_patternMask); if (mapIterator == deviceMap.end()) { return -1; } DeviceSet *fullDeviceSet = mapIterator->second; // Find what is the pattern index for the pattern mask of this Loop // -> Needed for devices using FileChannel int patternIndex = 1; if (_configuration->_totalPoiPv == 1) { patternIndex = -1; } long deviceCount = 0; for (DeviceSetIterator devIterator = fullDeviceSet->begin(); devIterator != fullDeviceSet->end(); ++devIterator, ++deviceCount) { // If Loop is configured with longitudital algorithm, then // select all devices available, even if xxUSED PV is set to false // Maximum allowed number of selected devices is given by the // xxxNUM PVs (e.g. MEASUSED) if (((*devIterator)->isUsed() || _configuration->_algorithmNamePv == LONGITUDINAL_ALGORITHM) && (*devIterator)->getDeviceName().size() > 2) { deviceSet.insert(*devIterator); _configuration->_logger << "Connecting to " << (*devIterator)->getDeviceName().c_str() << Log::flushpvnoalarm; int res = (*devIterator)->configure(accessType, patternIndex); if (res != 0) { _configuration->_logger << "Feedback is OFF - Cannot connect to " << (*devIterator)->getDeviceName().c_str() << " (code: " << res << "). " << "Push \"Start\" to retry." << Log::flushpv; _configuration->_logger << "ERROR: Failed to configure device " << (*devIterator)->getName().c_str() << Log::flush; return -1; } deviceCount++; } else { deviceSet.erase(*devIterator); } } if (_configuration->_algorithmNamePv == LONGITUDINAL_ALGORITHM) { std::cout << "Selected " << deviceCount << " devices." << std::endl; } return 0; } /** * Invoke the algorithm to calculate new actuator and state values. The new * values are set() by the algorithm but are not sent to the actual devices. * * The Loop is responsible for making sure the new values are correct and * can be sent out. * * @param pattern the pattern received from the EVR and passed along the * PATTERN_EVENT * @return 0 on success, -1 on failure or -2 if there is an error due to a * pulse id mismatch. * @author L.Piccoli */ int Loop::calculate(Pattern pattern) { _configuration->_loopCounter = _configuration->_loopCounter.getValue() + 1; // If there is no beam, this loop must be running on the second pattern. // In this case, the actuator will receive the last value calculated for // the first pattern plus the offset between patterns. If there is beam, // then the actuator value is calculated. if (!pattern.hasBeam() && !_configuration->_fakeBeam) { setActuatorsReference(); return 0; } if (_algorithm == NULL) { _configuration->_logger << "ERROR: Loop has NULL algorithm" << Log::flush; return -1; } if (_firstPass) { resetMeasurements(); _firstPass = false; } // TMIT is not checked if Longitudital is the used algorithm // _skipTmitCheck is set when the Loop is (re)configured if (!_configuration->_skipTmitCheck) { if (checkTmit() != 0) { _lowTmitCount++; return -1; } } // Check if there is at least one Measurement with status different than // DataPoint::READ. In that case the latest measurements are read out, // discarded and the calculation returns an error. if (checkMeasurementStatus(pattern.getPulseId()) != 0) { _pulseIdMismatchCount++; discardLatestMeasurements(); return -2; } _calculateStats.start(); int status = _algorithm->calculate(*_configuration, _measurements, _actuators, _states); _calculateStats.end(); // At this point, if _algorithm->calculate() took too long to finish then // there may be several PATTERN_EVENT and MEASUREMENT_EVENT queued up. // Writing to actuators must be skipped in this case, otherwise they will // receive values based on measurements from some cycles in the past. bool skip = false; if (_configuration->_eventReceiver->getPendingEvents() > 0) { std::cout << "WARN: discarding " << _configuration->_eventReceiver->getPendingEvents() << " events... "; skip = true; _configuration->_eventReceiver->clear(); std::cout << " done." << std::endl; } if (status == 0) { status = setDevices(skip); if (status == 0) { _calculateCount++; return 0; } else { _deviceFailCount++; } } else { // If ACTUATOR_LIMIT was hit, then must go into COMPUTE mode if (status == ACTUATOR_LIMIT) { _configuration->_logger << "Actuator values out of range, no change" << Log::flushpvonly; _actuatorsOutsideLimitsCount++; ActuatorSet::iterator it; for (it = _actuators.begin(); it != _actuators.end(); ++it) { ActuatorDevice *actuator = (*it); actuator->setGroupLimit(); } } } _calculateFailCount++; return status; } /** * This method is invoked when patterns with POCKCEL_PERM set start to * be received. * * Actuators for this pattern have values set to the values calculated for * the P1 pattern plus the offset between patterns. * * @author L.Piccoli */ void Loop::setActuatorsReference() { ActuatorSet::iterator it; bool setActuatorWithNoBeam = true; for (it = _actuators.begin(); it != _actuators.end(); ++it) { ActuatorDevice *actuator = (*it); actuator->getInitialSettingFromReference(setActuatorWithNoBeam); } } /** * Check whether there is any measurument device whose TMIT is below the * acceptable threshold. This means there is no beam and the feedback calculation * should not take place. * * @return 0 if TMIT values are above limit, -1 if there is at least one value * below the threshold (which is given by SIOC:SYS0:FB00:TMITLOW PV) * @author L.Piccoli */ int Loop::checkTmit() { if (_skipTmit) { return 0; } MeasurementSet::iterator it; MeasurementDevice *measurementDevice; double lastLowTmit = -1; int lowTmitBpms = 0; for (it = _measurements.begin(); it != _measurements.end(); ++it) { measurementDevice = *it; if (measurementDevice->getTmit() <= ExecConfiguration::getInstance()._tmitLowPv.getValue()) { lowTmitBpms++; lastLowTmit = measurementDevice->getTmit(); } } if (lowTmitBpms > 0) { _configuration->_logger << "TMIT too low on one or more BPMs, no change" << Log::flushpvonly; // must skip Measurements on this cycle discardLatestMeasurements(); return -1; } return 0; } /** * This method is called by calculate() after the algorithm has updated the * actuators and measurements. * * Values are sent out to the devices using the default communication channel. * * The PulseId of the actuators are checked against the current PulseId. * * @return 0 on success, -1 if there is a problem sending data out to at least * one device. * @author L.Piccoli */ int Loop::setDevices(bool skip) { // If Loop MODE is set to Compute (false), then skip actuator setting. bool send = true; if (_configuration->_mode == false || skip) { send = false; _actuatorsSkipCount++; } // Check if PulseId is current (RTEMS only) ActuatorSet::iterator actIt; #ifdef RTEMS bool pulseMismatch = false; for (actIt = _actuators.begin(); actIt != _actuators.end(); ++actIt) { ActuatorDevice *actuator = (*actIt); // Only check the PulseId if datum is going to be written out if (actuator->peekStatus() == DataPoint::SET) { if (actuator->peekPulseId() != _configuration->_pulseIdPv.getValue()) { pulseMismatch = true; std::cout << actuator->peekPulseId() << " != " << _configuration->_pulseIdPv.getValue() << std::endl; _configuration->_logger << "Actuator pulse id mismatch, no change" << Log::flushpvonly; // TODO: If there is a mismatch, then we better check for LoopThread // input queue. There may be events not handled and out of sync } } } if (pulseMismatch) { send = false; _actuatorMismatchCount++; } #endif for (actIt = _actuators.begin(); actIt != _actuators.end(); ++actIt) { (*actIt)->write(send); } StateSet::iterator stateIt; for (stateIt = _states.begin(); stateIt != _states.end(); ++stateIt) { (*stateIt)->write(); // TODO: need to implement skip in StateDevice::write (see ActuatorDevice::write(bool)) } _configuration->_logger << Log::clearpv; return 0; } /** * Prints out information about the Loop. * * @author L.Piccoli */ void Loop::showDebug() { std::cout << &_patternMask << std::endl; std::cout << " -> Algorithm: "; if (_algorithm != NULL) { std::cout << _algorithm->getName() << std::endl; _algorithm->showDebug(); } else { std::cout << "none" << std::endl; } } /** * Prints out information about the Loop. * * @author L.Piccoli */ void Loop::show() { std::cout << &_patternMask << std::endl; std::cout << " -> Algorithm: "; if (_algorithm != NULL) { std::cout << _algorithm->getName() << std::endl; _algorithm->show(); } else { std::cout << "none" << std::endl; } std::cout << " -> Counters (ok: " << _calculateCount << ", fail: " << _calculateFailCount << ", set fail: " << _deviceFailCount << ", act lim: " << _actuatorsOutsideLimitsCount << ", act skip: " << _actuatorsSkipCount << ", low tmit: " << _lowTmitCount << ", ts mismatch: " << _pulseIdMismatchCount << ", act mismatch: " << _actuatorMismatchCount << ")" << std::endl; _calculateStats.show(" "); } /** * Set the FBCK PV to ON or OFF for all selected Actuators and Measurements, * according to the specified parameters. * * @param measurements true indicates measurement FBCK PVs should be turned ON, * if false FBCK PV is turned OFF * @param actuators true indicates actuator FBCK PVs should be turned ON, * if false FBCK PV is turned OFF * @return 0 if all PVs were set, -1 if there was an error setting one PV. * @author L.Piccoli */ int Loop::setFeedbackDevices(bool measurements, bool actuators) { ActuatorSet::iterator actIt; for (actIt = _actuators.begin(); actIt != _actuators.end(); ++actIt) { ActuatorDevice *actuator = *actIt; if (actuator->setFeedbackPv(actuators) != 0) { _configuration->_logger << "Cannot set " << actuator->getDeviceName().c_str() << " FBCK PV" << Log::flushpv; return -1; } } MeasurementSet::iterator measIt; for (measIt = _measurements.begin(); measIt != _measurements.end(); ++measIt) { MeasurementDevice *measurement = *measIt; if (measurement->setFeedbackPv(measurements) != 0) { _configuration->_logger << "Cannot set " << measurement->getDeviceName().c_str() << " FBCK PV" << Log::flushpv; return -1; } } return 0; } /** * Disconnect all devices used by the Loop. This method is used when * the STATE of a Loop goes from ON to OFF. * * Devices will be reconnected when the STATE transitions from OFF to ON. * * MeasurementDevices are also removed from the MeasurementCollector. * * @author L.Piccoli */ void Loop::disconnectDevices() { ActuatorSet::iterator actIt; for (actIt = _actuators.begin(); actIt != _actuators.end(); ++actIt) { ActuatorDevice *actuator = *actIt; actuator->disconnect(); } MeasurementSet::iterator measIt; for (measIt = _measurements.begin(); measIt != _measurements.end(); ++measIt) { MeasurementDevice *measurement = *measIt; measurement->disconnect(); } MeasurementSet::iterator it; // MeasurementCollector::getInstance().lock(); <<< Moved to LoopThread::disconnectDevices() for (it = _measurements.begin(); it != _measurements.end(); ++it) { MeasurementCollector::getInstance().remove(*it); } // MeasurementCollector::getInstance().unlock(); } /** * Check if at least one of the latest available measurements have a status * indicating a problem with the value. * * @param patternPulseId the pulseId for the current pattern, which must match * the pulse id of the measurement. * @return 0 if all measurements have DataPoint::READ status (i.e. ready to * be read), -1 if there is at least one value with status other than * DataPoint::READ or there is a pulseId mismatch * @author L.Piccoli */ int Loop::checkMeasurementStatus(epicsUInt32 patternPulseId) { #ifdef DEBUG_PULSEID Log::getInstance() << "INFO: Loop::checkMeasStatus pulseId=" << (int) patternPulseId << " measurements=" << (int) _measurements.size() << Log::flush; #endif MeasurementSet::iterator measIt; for (measIt = _measurements.begin(); measIt != _measurements.end(); ++measIt) { MeasurementDevice *measurement = *measIt; if (measurement->peekStatus() != DataPoint::READ) { Log::getInstance() << "ERROR: Measurement " << measurement->getDeviceName().c_str() << " status is " << (int) measurement->peekStatus() << Log::flush; return -1; } if (measurement->isFcom() && !measurement->isFile()) { if (measurement->peekPulseId() != patternPulseId) { return -1; } } } return 0; } /** * This method is invoked if at least one of the latest measurements has * invalid status. * * @author L.Piccoli */ void Loop::discardLatestMeasurements() { MeasurementSet::iterator it; for (MeasurementSet::iterator it = _measurements.begin(); it != _measurements.end(); ++it) { double meas; epicsTimeStamp timestamp; MeasurementDevice *measurement = (*it); measurement->get(meas, timestamp); } } /** * This method is called by calculate() when it is invoked for the first * time. MeasurementDevices may have been read several times before * Loop::calculate() runs, meaning that old measurements my be used * for the calculations. * * @author L.Piccoli */ void Loop::resetMeasurements() { MeasurementSet::iterator it; for (MeasurementSet::iterator it = _measurements.begin(); it != _measurements.end(); ++it) { MeasurementDevice *measurement = (*it); measurement->resetNextRead(); } } /** * This method is invoked when the $(LOOP):MODE PV switches from * COMPUTE to ENABLE, iff $(LOOP):STATE is ON. * * Current actuator values are retrieved from the respective IOCs * such that the feedback loop starts controlling actuators from their * current values. * * @return 0 * @author L.Piccoli */ int Loop::resetActuators() { ActuatorSet::iterator it; int status = 0; for (ActuatorSet::iterator it = _actuators.begin(); it != _actuators.end(); ++it) { ActuatorDevice *actuator = (*it); if (actuator->getInitialSetting() != 0) { status = -1; } } return status; } /** * This method invokes the reset() method of the used algorithm. It is * invoked by the LoopThread when the $(LOOP):MODE PV switches from * COMPUTE to ENALE. * * @return 0 * @author L.Piccoli */ int Loop::resetAlgorithm() { if (_algorithm != NULL) { _algorithm->reset(); } return 0; }