/* * File: Longitudinal.cc * Author: lpiccoli * * Created on November 18, 2010, 13:30 */ #include "LongitudinalChirp.h" #include #include #include #include #include #include #include #include "epicsTime.h" #include "ExecConfiguration.h" #include "MeasurementAverage.h" #include "PatternManager.h" USING_FF_NAMESPACE using namespace boost::numeric::ublas; const double PI = 3.14159265; /** * Class constructor. Only initializes attributes. * * @author L.Piccoli */ LongitudinalChirp::LongitudinalChirp(std::string algorithmName) : Longitudinal(algorithmName), _previousBc2Energy(0), _previousBc2Chirp(0), _previousBc2FbckPv(false), _chirpControl(false), _bc2EnergyState(NULL), _bc2BunchState(NULL) { _previousActuator.resize(LONGITUDINAL_MATRIX_DIMENSION); _actuatorValue.resize(LONGITUDINAL_MATRIX_DIMENSION); for (unsigned int i = 0; i < _previousActuator.size(); ++i) { _previousActuator[i] = 0; _actuatorValue[i] = 0; } } LongitudinalChirp::~LongitudinalChirp() { } /** * 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 LongitudinalChirp::configure(LoopConfiguration *configuration, MeasurementSet *measurements, ActuatorSet *actuators, StateSet *states) throw (Exception) { _firstCalculate = true; if (Longitudinal::configure(configuration, measurements, actuators, states) != 0) { return -1; } _chirpControl = configuration->_chirpControlPv.getValue(); StateSet::iterator sIt = states->begin(); std::advance(sIt, 3); _bc2EnergyState = (*sIt); std::advance(sIt, 1); _bc2BunchState = (*sIt); ActuatorSet::iterator aIt = actuators->begin(); std::advance(aIt, 3); _bc2Amplitude = (*aIt); std::advance(aIt, 1); _bc2Phase = (*aIt); return 0; } /* * Must override this method in order to have the bc2State USEDBYLOOP Pv * set if the chirp is ON! */ int LongitudinalChirp::selectStates() throw (Exception) { int val = Longitudinal::selectStates(); if (_chirpControl) { _bc2BunchState->setUsedBy(true); // This UNSET by Longitudinal::calculate()! } // FBCK PV of BC2 Amplitude and Phase must be set to ON if: // _chirpControl is true or // _bc2EnergyState->isUsed() is true or // _bc2BunchState->isUsed() is true bool fbckPv = true; // FBCK PV for BC2 Amplitude and Phase must be set to OFF iff: // _bc2BunchState->isUsed() is false AND // _bc2EnergyState->isUsed() is false if (!_bc2EnergyState->isUsed() && !_bc2BunchState->isUsed()) { fbckPv = false; } if (_chirpControl) { fbckPv = true; } // Must set the FBCK PV only if the loop is ENABLED! if (_loopConfiguration->_mode == true) { // If FBCK PV won't change, then don't set it again! if (_previousBc2FbckPv != fbckPv) { _previousBc2FbckPv = fbckPv; Log::getInstance() << Log::flagFbckPv << Log::dpInfo << "LongitudinalChirp::selectStates() FBCK PV toggle for BC2" << Log::dp; _bc2Amplitude->setFeedbackPv(fbckPv); _bc2Phase->setFeedbackPv(fbckPv); } } return val; } /** * Implementation of the Longitudinal algorithm. * * * @return 0 on success, -1 on failure * @author L.Piccoli */ int LongitudinalChirp::calculate() throw (Exception) { checkChirpControl(); // Calculate initial Energy/Chirp from the initial actuator // values. // The value of _firstCalculate is set to true in the // Longitudital::configure() method. if (_firstCalculate) { Log::getInstance() << Log::flagAlgo << Log::dpInfo << "LongitudinalChirp::calculate() - First Cycle" << Log::dp; calculateInitialStates(); } // Proceed with feedback iff CHIRPSTATE (_chirpControl) is ON // and CHIRPREADY is YES *or* CHIRPSTATE is OFF and CHIRPREADY is NO. // CHIRPREADY is set by the sequencer based on the CHIRPSTATE PV. if ((_loopConfiguration->_chirpReadyPv.getValue() == true && _chirpControl == true) || (_loopConfiguration->_chirpReadyPv.getValue() == false && _chirpControl == false)) { return Longitudinal::calculate(); } return 0; } /** * Make sure the BLEN measurement is valid if chirp is * enabled. * * @return 0 on success, -1 if Longitudinal::checkMeasurementStatus() * fails. * @author L.Piccoli */ int LongitudinalChirp::checkMeasurementStatus() { if (Longitudinal::checkMeasurementStatus() != 0) { return -1; } if (_chirpControl) { _measurementStatus[M6] = VALID; } return 0; } /** * Set the CHIRPSTATE PV and call the Longitudinal::checkConfiguration() method. * * @return 0 * @author L.Piccoli */ int LongitudinalChirp::checkChirpControl() { bool chirp = false; if (_bc2EnergyState != NULL && _bc2BunchState != NULL) { if (_bc2EnergyState->isUsed() && !_bc2BunchState->isUsed()) { chirp = true; } } _loopConfiguration->_chirpStatePv = chirp; _loopConfiguration->_chirpStatePv.scanIoRequest(); // Set STATUSSTR message if chirpControl changed value if (chirp != _chirpControl) { _loopConfiguration->_logger << Log::showtime << "Chirp is currently "; if (chirp) { _loopConfiguration->_logger << "ON"; } else { _loopConfiguration->_logger << "OFF"; } _loopConfiguration->_logger << Log::flushpvnoalarm; } // Set the CHIRPCTRL PV according to S4USED and S5USED PVs // It is ON iff S4USED=1 and S5USED=0 _chirpControl = chirp; // TODO: At this point the current chirp value from FBCK:LI22:1:CHACT // should be copied to FBCK:FB04:LG01:CHIRPDES return 0; } /** * Calculate initial Energy/Chirp for values from initias Phase/Amplitude * values read from the actuator devices for BC2. * * DL1, BC1 and DL2 use Phase/Amplitude. * * @return always 0 * @author L.Piccoli */ int LongitudinalChirp::calculateInitialStates() { ActuatorSet::iterator it = _actuators->begin(); // Fill in the current time and pulseId epicsTimeStamp timestamp; epicsTimeGetCurrent(×tamp); #ifdef RTEMS evrTimePutPulseID(×tamp, _loopConfiguration->_pulseIdPv.getValue()); #endif // DL - Must make sure the initial actiator setting is valid, so read // the latest from its reference. That is done for all actuators. (*it)->getInitialSettingFromReference(false, timestamp); _previousActuator[DL1] = (*it)->getLastValueSet(); // BC1 Amplitude ++it; (*it)->getInitialSettingFromReference(false, timestamp); double bc1Amplitude = (*it)->getLastValueSet(); _previousActuator[BC1_ENERGY] = bc1Amplitude; // BC1 Phase ++it; (*it)->getInitialSettingFromReference(false, timestamp); double bc1Phase = (*it)->getLastValueSet(); _previousActuator[BC1_CHIRP] = bc1Phase; // BC2 Energy ++it; (*it)->getInitialSettingFromReference(false, timestamp); double bc2Amplitude = (*it)->getLastValueSet(); ++it; (*it)->getInitialSettingFromReference(false, timestamp); double bc2Phase = (*it)->getLastValueSet(); _previousActuator[BC2_ENERGY] = bc2Amplitude * std::cos(bc2Phase * (PI/180)); // BC2 Chirp - _previousActuator[BC2_CHIRP] must be // equal to _loopConfiguration->_bc2ChirpPv.getValue(); _previousActuator[BC2_CHIRP] = bc2Amplitude * std::sin(bc2Phase * (PI/180)); // DL2 ++it; (*it)->getInitialSettingFromReference(false, timestamp); _previousActuator[DL2] = (*it)->getLastValueSet(); Log::getInstance() << Log::flagAlgo << Log::dpInfo << "LongitudinalChirp::calcInitialStates() [" << _previousActuator[DL1] << ", " << _previousActuator[BC1_ENERGY] << ", " << _previousActuator[BC1_CHIRP] << ", " << _previousActuator[BC2_ENERGY] << " (" << bc2Amplitude << "), " << _previousActuator[BC2_CHIRP] << " (" << bc2Phase << "), " << _previousActuator[DL2] << "]" << Log::dp; _previousBc2FbckPv = true; if (!_bc2EnergyState->isUsed() && !_bc2BunchState->isUsed()) { _previousBc2FbckPv = false; } if (_chirpControl) { _previousBc2FbckPv = true; } return 0; } /** * Update the values for the actuators using latests Energy/Chirps * * 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 LongitudinalChirp::updateActuators() throw (Exception) { unsigned int limitFlag = 0; double values[10]; // Calculate values stored in the vector _delta. updateActuatorsDelta(); // _delta contains Energy/Chirp values, these must be converted // to Amplitude/Phase before being sent to actuators calculateAmplitudeAndPhase(); // 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; for (ActuatorSet::iterator it = _actuators->begin(); it != _actuators->end(); ++it, ++i, ++stateIt) { ActuatorDevice *actuator = (*it); double value = 0; if ((*stateIt)->getUsedBy()) { // Change the actuator value only if MODE PV is ENABLE (true) if (_loopConfiguration->_mode == true) { value = _actuatorValue[i]; values[i] = value; if (actuator->set(value, timestamp) != 0) { limitFlag |= (0x01 << i); actuatorLimit = true; } } } else { value = actuator->getLastValueSet(); actuator->set(value, DataPoint::SET_SKIP); } } if (actuatorLimit) { i = 0; _exceptionMessage = "Actuator(s) out of range:"; for (ActuatorSet::iterator it = _actuators->begin(); it != _actuators->end(); ++it, ++i) { if (limitFlag & (0x01 << i)) { std::ostringstream strstream; strstream << values[i]; _exceptionMessage += ", "; _exceptionMessage += (*it)->getDeviceName(); _exceptionMessage += " ("; _exceptionMessage += strstream.str(); _exceptionMessage += ")"; } } throw Exception(_exceptionMessage, Exception::ACTUATOR_LIMIT); return -1; } // Save new actuator values (phase/amplitude) to be used on the next iteration _previousActuator = _actuatorValue; // Restore the Energy/Chirp for BC2 - we don't want to save the phase/amplitude // for BC2, therefore the Energy/Chirp are restored here. _previousActuator[BC2_ENERGY] = _previousBc2Energy; _previousActuator[BC2_CHIRP] = _previousBc2Chirp; Log::getInstance() << Log::flagAlgo << Log::dpInfo << "LongitudinalChirp::updateActuators(P" << _patternIndex << ") [" << _actuatorValue[0] << ", " << _actuatorValue[1] << ", " << _actuatorValue[2] << ", " << _actuatorValue[3] << " [e=" << _previousBc2Energy << "], " << _actuatorValue[4] << " [c=" << _previousBc2Chirp << "], " << _actuatorValue[5] << ", " << Log::dp; return 0; } int LongitudinalChirp::calculateAmplitudeAndPhase() { // OVER/UNDER COMPRESSED - change signal here depending on the setpoint signal // 09/30/2011 - Isn't this done already in the Longitudinal.cc? // Amplitude at DL1 _actuatorValue[DL1] = _previousActuator[DL1] - _delta[DL1]; // Amplitude at BC1 _actuatorValue[BC1_AMPLITUDE] = _previousActuator[BC1_ENERGY] - _delta[BC1_ENERGY]; // Phase at BC1 _actuatorValue[BC1_PHASE] = _previousActuator[BC1_CHIRP] + _delta[BC1_CHIRP]; // Amplitude at BC2 = sqrt(E_bc2^2 + C_bc2^2) /* std::cout << ">>> DELTAS: " << std::endl; std::cout << " _previousActuator[BC2_ENERGY]= " << _previousActuator[BC2_ENERGY] << std::endl; std::cout << " _previousActuator[BC2_CHIRP]= " << _previousActuator[BC2_CHIRP] << std::endl; std::cout << " _delta[BC2_ENERGY]= " << _delta[BC2_ENERGY] << std::endl; std::cout << " _delta[BC2_CHIRP]= " << _delta[BC2_CHIRP] << std::endl; */ double chirp = _loopConfiguration->_bc2ChirpPv.getValue(); // std::cout << " chirp= " << chirp << std::endl; _previousBc2Energy = _previousActuator[BC2_ENERGY] - _delta[BC2_ENERGY]; if (_chirpControl) { _actuatorValue[BC2_AMPLITUDE] = std::sqrt(((_previousActuator[BC2_ENERGY] - _delta[BC2_ENERGY])* (_previousActuator[BC2_ENERGY] - _delta[BC2_ENERGY])) + (chirp * chirp)); /* std::cout << " _actuatorValue[BC2_AMPLITUDE]= " << _actuatorValue[BC2_AMPLITUDE] << std::endl; */ } else { _actuatorValue[BC2_AMPLITUDE] = std::sqrt(((_previousActuator[BC2_ENERGY] - _delta[BC2_ENERGY])* (_previousActuator[BC2_ENERGY] - _delta[BC2_ENERGY])) + ((_previousActuator[BC2_CHIRP] + _delta[BC2_CHIRP])* (_previousActuator[BC2_CHIRP] + _delta[BC2_CHIRP]))); // std::cout << " _actuatorValue[BC2_AMPLITUDE]= " << _actuatorValue[BC2_AMPLITUDE] << std::endl; } // Phase at BC2 = 180/pi * (atan2 (C_bc2, E_bc2)) if (_chirpControl) { _previousBc2Chirp = chirp; _actuatorValue[BC2_PHASE] = 180/PI * (std::atan2(chirp, _previousActuator[BC2_ENERGY] - _delta[BC2_ENERGY])); // std::cout << " _actuatorValue[BC2_PHASE]= " << _actuatorValue[BC2_PHASE] << std::endl; } else { _previousBc2Chirp = _previousActuator[BC2_CHIRP] + _delta[BC2_CHIRP]; _actuatorValue[BC2_PHASE] = 180/PI * (std::atan2(_previousActuator[BC2_CHIRP] + _delta[BC2_CHIRP], _previousActuator[BC2_ENERGY] - _delta[BC2_ENERGY])); // std::cout << " _actuatorValue[BC2_PHASE]= " << _actuatorValue[BC2_PHASE] << std::endl; } // Amplitude at DL2 _actuatorValue[DL2] = _previousActuator[DL2] - _delta[DL2]; return 0; } /** * Calculate the Phase/Amplitude values to be applied to the actuators. * * For BC2, the calculated values are handled is Energy/Chirp and must be * converted back to Phase/Amplitude at this point. * * @author L.Piccoli * / int LongitudinalChirp::calculateAmplitudeAndPhase() { // Amplitude at DL1 _actuatorValue[DL1] = _previousActuator[DL1] - _delta[DL1]; // Amplitude at BC1 _actuatorValue[BC1_AMPLITUDE] = _previousActuator[BC1_ENERGY] - _delta[BC1_ENERGY]; // Phase at BC1 _actuatorValue[BC1_PHASE] = _previousActuator[BC1_CHIRP] + _delta[BC1_CHIRP]; // Amplitude at BC2 = sqrt(E_bc2^2 + C_bc2^2) double chirp = _loopConfiguration->_bc2ChirpPv.getValue(); _previousBc2Energy = _previousActuator[BC2_ENERGY] - _delta[BC2_ENERGY]; // If chirp is on, use value from the CHIRPDES PV if (_chirpControl) { // _actuatorValue[BC2_AMPLITUDE] = std::sqrt(((_previousActuator[BC2_ENERGY] - _delta[BC2_ENERGY])* // (_previousActuator[BC2_ENERGY] - _delta[BC2_ENERGY])) + // (chirp * chirp)); _actuatorValue[BC2_AMPLITUDE] = std::sqrt(_previousBc2Energy * _previousBc2Energy + chirp * chirp); } else { double chirpDiff = _previousActuator[BC2_CHIRP] + _delta[BC2_CHIRP]; _actuatorValue[BC2_AMPLITUDE] = std::sqrt(_previousBc2Energy * _previousBc2Energy + chirpDiff * chirpDiff); } // Phase at BC2 = 180/pi * (atan2 (C_bc2, E_bc2)) // If chirp is on, use value from the CHIRPDES PV if (_chirpControl) { _previousBc2Chirp = chirp; } else { _previousBc2Chirp = _previousActuator[BC2_CHIRP] + _delta[BC2_CHIRP]; } _actuatorValue[BC2_PHASE] = 180/PI * (std::atan2(_previousBc2Chirp, _previousBc2Energy)); // Amplitude at DL2 _actuatorValue[DL2] = _previousActuator[DL2] - _delta[DL2]; return 0; } */ int LongitudinalChirp::reset() { Longitudinal::reset(); return 0; }