410 lines
17 KiB
C++
410 lines
17 KiB
C++
/**
|
|
* @file JARTStateMachineGAM.cpp
|
|
* @brief Source file for class JARTStateMachineGAM
|
|
* @date Nov 26, 2018
|
|
* @author aneto
|
|
*
|
|
* @copyright Copyright 2015 F4E | European Joint Undertaking for ITER and
|
|
* the Development of Fusion Energy ('Fusion for Energy').
|
|
* Licensed under the EUPL, Version 1.1 or - as soon they will be approved
|
|
* by the European Commission - subsequent versions of the EUPL (the "Licence")
|
|
* You may not use this work except in compliance with the Licence.
|
|
* You may obtain a copy of the Licence at: http://ec.europa.eu/idabc/eupl
|
|
*
|
|
* @warning Unless required by applicable law or agreed to in writing,
|
|
* software distributed under the Licence is distributed on an "AS IS"
|
|
* basis, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express
|
|
* or implied. See the Licence permissions and limitations under the Licence.
|
|
|
|
* @details This source file contains the definition of all the methods for
|
|
* the class JARTStateMachineGAM (public, protected, and private). Be aware that some
|
|
* methods, such as those inline could be defined on the header file, instead.
|
|
*/
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
/* Standard header includes */
|
|
/*---------------------------------------------------------------------------*/
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
/* Project header includes */
|
|
/*---------------------------------------------------------------------------*/
|
|
|
|
#include "JARTStateMachineGAM.h"
|
|
|
|
#include "AdvancedErrorManagement.h"
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
/* Static definitions */
|
|
/*---------------------------------------------------------------------------*/
|
|
static MARTe::uint64 getCurrentTimeUs() {
|
|
using namespace MARTe;
|
|
return static_cast<uint64>(HighResolutionTimer::Counter() * HighResolutionTimer::Period() * 1e6f + 0.5f);
|
|
}
|
|
/*---------------------------------------------------------------------------*/
|
|
/* Method definitions */
|
|
/*---------------------------------------------------------------------------*/
|
|
|
|
JARTStateMachineGAM::JARTStateMachineGAM() {
|
|
currentState = WaitTrigger; // Set Entry state.
|
|
plcOnTime = 0; // Triggered time holder.
|
|
|
|
//Output and condition in a given state.
|
|
conditionTrigger = 1;
|
|
aps_hvon = 0;
|
|
aps_swon = 0;
|
|
bps_hvon = 0;
|
|
bps_swon = 0;
|
|
mhvps_hvon = 0;
|
|
|
|
// Parameters which get from Input signals.
|
|
triggerSignal = NULL_PTR(MARTe::uint32 *);
|
|
currentTime = NULL_PTR(MARTe::uint32 *);
|
|
turn_off_delay = 2000; //us
|
|
|
|
triggerDelay_mhvps_hvon = NULL_PTR(MARTe::uint32 *);
|
|
triggerDelay_aps_hvon = NULL_PTR(MARTe::uint32 *);
|
|
triggerDelay_aps_swon = NULL_PTR(MARTe::uint32 *);
|
|
triggerDelay_bps_hvon = NULL_PTR(MARTe::uint32 *);
|
|
triggerDelay_bps_swon = NULL_PTR(MARTe::uint32 *);
|
|
triggerDelay_shotlen = NULL_PTR(MARTe::uint32 *);
|
|
|
|
stopRequest = NULL_PTR(MARTe::uint32 *);
|
|
modePulseLengthLimit = NULL_PTR(MARTe::uint32 *);
|
|
short_pulse_mode = NULL_PTR(MARTe::uint32 *);
|
|
|
|
modulation = NULL_PTR(MARTe::uint32 *);
|
|
|
|
// write out target.
|
|
outputSignal = NULL_PTR(MARTe::uint32 *);
|
|
outputBeamON = NULL_PTR(MARTe::uint32 *);
|
|
outputHVArmed = NULL_PTR(MARTe::uint32 *);
|
|
outputHVInjection = NULL_PTR(MARTe::uint32 *);
|
|
outputRFON = NULL_PTR(MARTe::uint32 *);
|
|
outputBeamONTime = NULL_PTR(MARTe::uint32 *);
|
|
outputRFONTime = NULL_PTR(MARTe::uint32 *);
|
|
shotCounter = NULL_PTR(MARTe::uint32 *);
|
|
|
|
mhvps_hvon_is_on = false;
|
|
aps_hvon_is_on = false;
|
|
aps_swon_is_on = false;
|
|
bps_hvon_is_on = false;
|
|
bps_swon_is_on = false;
|
|
|
|
apsSwonHighResolutionTime = 0;
|
|
|
|
aps_hvon_state=0;
|
|
aps_swon_state=0;
|
|
mhvps_hvon_state=0;
|
|
bps_hvon_state=0;
|
|
bps_swon_state=0;
|
|
}
|
|
|
|
JARTStateMachineGAM::~JARTStateMachineGAM() {
|
|
}
|
|
|
|
bool JARTStateMachineGAM::Initialise(MARTe::StructuredDataI & data) {
|
|
using namespace MARTe;
|
|
bool ok = GAM::Initialise(data);
|
|
if (ok) {
|
|
ok = data.Read("ConditionTrigger", conditionTrigger);
|
|
if (!ok) {
|
|
REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "The Condition1 shall be specified");
|
|
}
|
|
}
|
|
if (ok) {
|
|
ok = data.Read("mhvps_hvon", mhvps_hvon);
|
|
if (!ok) {
|
|
REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "The mhvps_hvon shall be specified");
|
|
}
|
|
}
|
|
if (ok) {
|
|
ok = data.Read("aps_hvon", aps_hvon);
|
|
if (!ok) {
|
|
REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "The aps_hvon shall be specified");
|
|
}
|
|
}
|
|
if (ok) {
|
|
ok = data.Read("aps_swon", aps_swon);
|
|
if (!ok) {
|
|
REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "The aps_swon shall be specified");
|
|
}
|
|
}
|
|
if (ok) {
|
|
ok = data.Read("bps_hvon", bps_hvon);
|
|
if (!ok) {
|
|
REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "The bps_hvon shall be specified");
|
|
}
|
|
}
|
|
if (ok) {
|
|
ok = data.Read("bps_swon", bps_swon);
|
|
if (!ok) {
|
|
REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "The bps_swon shall be specified");
|
|
}
|
|
}
|
|
return ok;
|
|
}
|
|
|
|
bool JARTStateMachineGAM::PrepareNextState(const MARTe::char8 * const currentStateName, const MARTe::char8 * const nextStateName) {
|
|
|
|
return true;
|
|
}
|
|
|
|
bool JARTStateMachineGAM::Setup() {
|
|
using namespace MARTe;
|
|
bool ok = (numberOfInputSignals == 13u);
|
|
if (ok) {
|
|
ok = (numberOfOutputSignals == 16u);
|
|
if (!ok) {
|
|
REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "Seven output signals shall be defined %d",numberOfOutputSignals);
|
|
}
|
|
}
|
|
else {
|
|
REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "Nine input signals shall be defined");
|
|
}
|
|
if (ok) {
|
|
uint32 c;
|
|
for (c = 0u; c < numberOfInputSignals; c++) {
|
|
TypeDescriptor inputType = GetSignalType(InputSignals, c);
|
|
ok = (inputType == UnsignedInteger32Bit);
|
|
if (!ok) {
|
|
StreamString signalName;
|
|
(void) GetSignalName(InputSignals, c, signalName);
|
|
REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "Signal %s shall be defined as uint32", signalName.Buffer());
|
|
}
|
|
|
|
}
|
|
}
|
|
if (ok) {
|
|
uint32 c;
|
|
for (c = 0u; c < numberOfOutputSignals; c++) {
|
|
TypeDescriptor outputType = GetSignalType(OutputSignals, c);
|
|
ok = (outputType == UnsignedInteger32Bit || outputType == UnsignedInteger8Bit);
|
|
if (!ok) {
|
|
StreamString signalName;
|
|
(void) GetSignalName(InputSignals, c, signalName);
|
|
REPORT_ERROR(MARTe::ErrorManagement::ParametersError, "Signal %s shall be defined as uint32", signalName.Buffer());
|
|
}
|
|
}
|
|
}
|
|
if (ok) {
|
|
currentTime = reinterpret_cast<uint32 *>(GetInputSignalMemory(0));
|
|
triggerSignal = reinterpret_cast<uint32 *>(GetInputSignalMemory(1));
|
|
triggerDelay_mhvps_hvon = reinterpret_cast<uint32 *>(GetInputSignalMemory(2));
|
|
triggerDelay_aps_hvon = reinterpret_cast<uint32 *>(GetInputSignalMemory(3));
|
|
triggerDelay_aps_swon = reinterpret_cast<uint32 *>(GetInputSignalMemory(4));
|
|
triggerDelay_bps_hvon = reinterpret_cast<uint32 *>(GetInputSignalMemory(5));
|
|
triggerDelay_bps_swon = reinterpret_cast<uint32 *>(GetInputSignalMemory(6));
|
|
triggerDelay_shotlen = reinterpret_cast<uint32 *>(GetInputSignalMemory(7));
|
|
stopRequest = reinterpret_cast<uint32 *>(GetInputSignalMemory(8));
|
|
modePulseLengthLimit = reinterpret_cast<uint32 *>(GetInputSignalMemory(9));
|
|
short_pulse_mode = reinterpret_cast<uint32 *>(GetInputSignalMemory(10));
|
|
modulation = reinterpret_cast<uint32 *>(GetInputSignalMemory(11));
|
|
pauseSet = reinterpret_cast<uint32 *>(GetInputSignalMemory(12));
|
|
|
|
|
|
outputSignal = reinterpret_cast<uint32 *>(GetOutputSignalMemory(0));
|
|
outputBeamON = reinterpret_cast<uint32 *>(GetOutputSignalMemory(1));
|
|
outputHVArmed = reinterpret_cast<uint32 *>(GetOutputSignalMemory(2));
|
|
outputHVInjection = reinterpret_cast<uint32 *>(GetOutputSignalMemory(3));
|
|
outputRFON = reinterpret_cast<uint32 *>(GetOutputSignalMemory(4));
|
|
outputBeamONTime = reinterpret_cast<uint32 *>(GetOutputSignalMemory(5));
|
|
outputRFONTime = reinterpret_cast<uint32 *>(GetOutputSignalMemory(6));
|
|
shotCounter = reinterpret_cast<uint32 *>(GetOutputSignalMemory(7));
|
|
|
|
outputAPSHVON = reinterpret_cast<uint32 *>(GetOutputSignalMemory(8));
|
|
outputAPSSWON = reinterpret_cast<uint32 *>(GetOutputSignalMemory(9));
|
|
outputBPSHVON = reinterpret_cast<uint32 *>(GetOutputSignalMemory(10));
|
|
outputBPSSWON = reinterpret_cast<uint32 *>(GetOutputSignalMemory(11));
|
|
outputMHVPSON = reinterpret_cast<uint32 *>(GetOutputSignalMemory(12));
|
|
|
|
outputSignalNI6259 = reinterpret_cast<uint32 *>(GetOutputSignalMemory(13));
|
|
outputSignalNI6528P3 = reinterpret_cast<uint8 *>(GetOutputSignalMemory(14));
|
|
outputSignalNI6528P4 = reinterpret_cast<uint8 *>(GetOutputSignalMemory(15));
|
|
|
|
*shotCounter = 0;
|
|
}
|
|
return ok;
|
|
}
|
|
|
|
bool JARTStateMachineGAM::Execute() {
|
|
using namespace MARTe;
|
|
|
|
if (currentState == WaitTrigger) {
|
|
|
|
//State Transition condition
|
|
if ((*triggerSignal == conditionTrigger)) {
|
|
//REPORT_ERROR(ErrorManagement::Debug, "Start beam-on sequence at %d.", *currentTime);
|
|
plcOnTime = *currentTime; //Save pulse start time.
|
|
//*outputBeamON = 0;
|
|
//State transition.
|
|
currentState = SwitchingHVPS;
|
|
}
|
|
}
|
|
else if (currentState == SwitchingHVPS) {
|
|
|
|
//Actions in this state.
|
|
if (*stopRequest != 0 || *triggerSignal != conditionTrigger) {
|
|
*outputSignal = 0;
|
|
currentState = HVTerminate;
|
|
}
|
|
if (*currentTime >= (plcOnTime + *triggerDelay_bps_hvon) && bps_hvon_is_on == false){
|
|
//Do action
|
|
*outputSignal += bps_hvon;
|
|
bps_hvon_is_on = true; bps_hvon_state=1;
|
|
//REPORT_ERROR(ErrorManagement::Debug, "bps_hvon was set to outputSignal at %d.", *currentTime);
|
|
*outputBPSHVON=1;
|
|
}
|
|
if (*currentTime >= (plcOnTime + *triggerDelay_aps_hvon) && aps_hvon_is_on == false) {
|
|
//Do action
|
|
*outputSignal += aps_hvon;
|
|
aps_hvon_is_on = true; aps_hvon_state=1;
|
|
//REPORT_ERROR(ErrorManagement::Debug, "aps_hvon was set to outputSignal at %d.", *currentTime);
|
|
*outputAPSHVON=1;
|
|
}
|
|
if (*currentTime >= (plcOnTime + *triggerDelay_bps_swon) && bps_swon_is_on==false){
|
|
//Do action
|
|
*outputSignal += bps_swon;
|
|
bps_swon_is_on = true; bps_swon_state=1;
|
|
//REPORT_ERROR(ErrorManagement::Debug, "bps_swon was set to outputSignal at %d.", *currentTime);
|
|
*outputBPSSWON=1;
|
|
}
|
|
if (*currentTime >= (plcOnTime + *triggerDelay_mhvps_hvon) && mhvps_hvon_is_on==false) {
|
|
//Do action
|
|
*outputSignal += mhvps_hvon;
|
|
mhvps_hvon_is_on = true; mhvps_hvon_state=1;
|
|
//REPORT_ERROR(ErrorManagement::Debug, "mhvps_hvon was set to outputSignal at %d.", *currentTime);
|
|
*outputMHVPSON=1;
|
|
}
|
|
if (bps_swon_is_on && mhvps_hvon_is_on && *currentTime >= (plcOnTime + *triggerDelay_aps_swon)){
|
|
//Do action
|
|
*outputSignal += aps_swon;
|
|
aps_swon_is_on = true; aps_swon_state=1;
|
|
apsSwonHighResolutionTime = getCurrentTimeUs();
|
|
apsSwonTime = *currentTime;
|
|
//REPORT_ERROR(ErrorManagement::Debug, "aps_swon was set to outputSignal at %d.", *currentTime);
|
|
*outputAPSSWON=1;
|
|
}
|
|
*outputBeamONTime = *currentTime - plcOnTime; //Save RFON start time.
|
|
if (bps_hvon_is_on && aps_hvon_is_on){
|
|
*outputHVArmed = 1;
|
|
}
|
|
if (bps_swon_is_on || mhvps_hvon_is_on){
|
|
*outputHVInjection = 1;
|
|
}
|
|
|
|
//State transition condition
|
|
if (aps_swon_is_on){
|
|
currentState = RFON;
|
|
*outputRFON = 0;
|
|
*outputBeamON = 1;
|
|
*shotCounter += 1;
|
|
//REPORT_ERROR(ErrorManagement::Debug, "state was changed to RFON");
|
|
}
|
|
}
|
|
else if (currentState == RFON) {
|
|
|
|
//Action in this state.
|
|
if (*stopRequest != 0 || *triggerSignal != conditionTrigger) {
|
|
//debug
|
|
//if((*stopRequest != 0)){
|
|
//REPORT_ERROR(ErrorManagement::Debug, "Stop request was called.!!!");
|
|
//} else {
|
|
//REPORT_ERROR(ErrorManagement::Debug, "PLC_ON was reset.!!!");
|
|
//}
|
|
*outputSignal -= aps_swon;
|
|
mhvps_hvon_is_on = false; mhvps_hvon_state=0;
|
|
aps_hvon_is_on = false; aps_hvon_state=0;
|
|
aps_swon_is_on = false; aps_swon_state=0;
|
|
bps_hvon_is_on = false; bps_hvon_state=0;
|
|
bps_swon_is_on = false; bps_swon_state=0;
|
|
currentState = HVTerminate;
|
|
*outputAPSHVON=0;
|
|
*outputAPSSWON=0;
|
|
*outputBPSHVON=0;
|
|
*outputBPSSWON=0;
|
|
*outputMHVPSON=0;
|
|
}
|
|
uint32 updatePeriod = 100; // in microsecnds (get this from Timer)
|
|
if ((*modePulseLengthLimit == 1u) || (getCurrentTimeUs() + updatePeriod >= (apsSwonHighResolutionTime + *triggerDelay_shotlen))) {
|
|
|
|
// Now we do busy wait
|
|
while (getCurrentTimeUs() < (apsSwonHighResolutionTime + *triggerDelay_shotlen)) {
|
|
//REPORT_ERROR(ErrorManagement::Debug, "!");
|
|
}
|
|
// We stop busy waiting #executionOverhead before expected pulse off time
|
|
//debug
|
|
//if((*modePulseLengthLimit == 1u)){
|
|
// REPORT_ERROR(ErrorManagement::Debug, "Mode limit detected.!!!");
|
|
//} else {
|
|
// REPORT_ERROR(ErrorManagement::Debug, "Shot length reached to the setpoint.!!!");
|
|
//}
|
|
//debug end.
|
|
//Do action
|
|
*outputSignal -= aps_swon; //Turn off only APS_SWON first.
|
|
mhvps_hvon_is_on = false;
|
|
aps_hvon_is_on = false;
|
|
aps_swon_is_on = false; aps_swon_state=0;
|
|
bps_hvon_is_on = false;
|
|
bps_swon_is_on = false;
|
|
*outputAPSHVON=0;
|
|
*outputAPSSWON=0;
|
|
*outputBPSHVON=0;
|
|
*outputBPSSWON=0;
|
|
*outputMHVPSON=0;
|
|
//REPORT_ERROR(ErrorManagement::Debug, "0 was set to outputSignal at %d.", *currentTime);
|
|
}
|
|
*outputRFON = 1;
|
|
*outputBeamONTime = *currentTime - plcOnTime;
|
|
*outputRFONTime = *currentTime - apsSwonTime;
|
|
|
|
|
|
//State transition condition
|
|
if (!aps_swon_is_on && !bps_swon_is_on && !mhvps_hvon_is_on) {
|
|
currentState = HVTerminate;
|
|
apsSwoffTime = *currentTime;
|
|
//REPORT_ERROR(ErrorManagement::Debug, "state was changed to HVTerminate");
|
|
}
|
|
}
|
|
else if (currentState == HVTerminate) {
|
|
//In the HVTerminate state, turn APS_SWON off first, and wait 1ms. Finally turn other PS off.
|
|
//Action in this state.
|
|
*outputBeamON = 0;
|
|
*outputHVArmed = 0;
|
|
*outputHVInjection = 0;
|
|
*outputRFON = 0;
|
|
|
|
// State transition condition.
|
|
if (*currentTime - apsSwoffTime >= turn_off_delay){
|
|
*outputSignal = 0;
|
|
mhvps_hvon_state=0;
|
|
aps_hvon_state=0;
|
|
bps_hvon_state=0;
|
|
bps_swon_state=0;
|
|
}
|
|
if (*triggerSignal == false){
|
|
//Check PLC_ON is reset
|
|
currentState = WaitTrigger;
|
|
*outputSignal = 0;
|
|
//REPORT_ERROR(ErrorManagement::Debug, "PLC_ON was reset. The State was changed to WaitTrigger at %d.", *currentTime);
|
|
}
|
|
}
|
|
if(*short_pulse_mode == 1){
|
|
p3Value = 1*aps_hvon_state + 8*bps_hvon_state +16*bps_swon_state + 64*(*outputBeamON);
|
|
*outputSignalNI6259 = 1*aps_swon_state;
|
|
*outputSignalNI6528P3 = ~p3Value;
|
|
//REPORT_ERROR(ErrorManagement::Debug, "short pulse mode with p3: %d.", p3Value);
|
|
} else {
|
|
p3Value = 1*aps_hvon_state +2*aps_swon_state + 8*bps_hvon_state +16*bps_swon_state + 64*(*outputBeamON);
|
|
*outputSignalNI6528P3 = ~p3Value;
|
|
//REPORT_ERROR(ErrorManagement::Debug, "long pulse mode with p3: %d.", p3Value);
|
|
}
|
|
p4Value = 8*mhvps_hvon_state;
|
|
if (modulation) p4Value += 32;
|
|
if (pauseSet) p4Value += 1;
|
|
//*outputSignalNI6528P4 = ~(*ni6528p4Value | p4Value);
|
|
*outputSignalNI6528P4 = ~p4Value;
|
|
return true;
|
|
}
|
|
|
|
CLASS_REGISTER(JARTStateMachineGAM, "1.0")
|