/* Called by PLC thread when debug_publish finished
* This is supposed to unlock debugger thread in WaitDebugData*/
void InitiateDebugTransfer()
{
char msg = DEBUG_PENDING_DATA;
/* remember tick */
__debug_tick = __tick;
/* signal debugger thread it can read data */
rt_pipe_write(&WaitDebug_pipe, &msg, sizeof(msg), P_NORMAL);
}
void LeaveDebugSection(void)
{
if(AtomicCompareExchange( &debug_state,
DEBUG_BUSY, DEBUG_FREE) == DEBUG_BUSY){
char msg = DEBUG_UNLOCK;
/* signal to NRT for wakeup */
rt_pipe_write(&Debug_pipe, &msg, sizeof(msg), P_NORMAL);
}
}
void AuxTaskNonRT::schedule(void* ptr, size_t size){
#ifdef XENOMAI_SKIN_native
int ret = rt_pipe_write(&pipe, ptr, size, P_NORMAL);
#endif
#ifdef XENOMAI_SKIN_posix
int ret = __wrap_sendto(pipeSocket, ptr, size, 0, NULL, 0);
#endif
if(ret < 0)
{
rt_fprintf(stderr, "Error while sending to pipe from %s: (%d) %s (size: %d)\n", name, errno, strerror(errno), size);
}
}
void UnLockPython(void)
{
if(AtomicCompareExchange(
&python_state,
PYTHON_BUSY,
PYTHON_FREE) == PYTHON_BUSY){
if(rt_task_self()){/*is that the real time task ?*/
char cmd = UNLOCK_PYTHON;
rt_pipe_write(&Python_pipe, &cmd, sizeof(cmd), P_NORMAL);
}/* otherwise, no signaling from non real time */
} /* as plc does not wait for lock. */
}
static void klat_server(void *cookie)
{
int err;
for (;;) {
err = rt_dev_ioctl(fd, RTTST_RTIOC_INTERM_BENCH_RES, &pkt.res);
if (err) {
if (err != -EIDRM)
printk("rt_dev_ioctl(RTTST_RTIOC_INTERM_BENCH_RES): %d",
err);
return;
}
/* Do not check rt_pipe_write return value, the pipe may well be
full. */
rt_pipe_write(&klat_pipe, &pkt, sizeof(pkt), P_NORMAL);
}
}
void realtimeLoop(void *arg)
{
Q_UNUSED(arg);
int xenoRet = 0;
char bufChar[MAX_MESSAGE_LENGTH];
int bufInt[MAX_MESSAGE_LENGTH];
RealtimeController::Mode bufMode[MAX_MESSAGE_LENGTH];
RealtimeController::State bufState[MAX_MESSAGE_LENGTH];
int kp = 0, ki = 0, kd = 0;
int setValue = 0;
int actuatorVoltage = 0;
int actuatorVoltageToSend = 0;
int sensorVoltage = 0;
int sensorVoltageToSend = 0;
RealtimeController::Mode mode = RealtimeController::MODE_MANUAL;
RealtimeController::Mode modeToSend;
RealtimeController::State state = RealtimeController::STOPPED;
RealtimeController::State stateToSend;
int output = 0;
PiezoSensor* sensor;
PiezoActuator* actuator;
// varataan muisti anturille ja alustetaa se
sensor = new PiezoSensor();
sensor->init();
// varataan muisti toimilaitteelle ja alustetaan se
actuator = new PiezoActuator();
actuator->init();
// määritetään jaksonaika 500us eli 500000ns
RTIME interval = 500000;
// määritetään taskia kutsuttavaksi jaksonajan välein
xenoRet = rt_task_set_periodic(NULL, TM_NOW, interval);
while(1)
{
// ajastaa silmukan kulkemaan määritetyissä jaksonajoissa
if(rt_task_wait_period(NULL) < 0)
qDebug("task error in realtime loop");
if(state == RealtimeController::STARTED)
{
sensor->getValue(sensorVoltage);
sensorVoltage *= -1; // kytkentä laboratoriolaitteistolla on väärinpäin
// tarkastetaan ajetaanko PID-säätöä vai suoraa ohjausta
if( mode == RealtimeController::MODE_MANUAL)
{
// suorassa jänniteohjauksessa laitetaan suoraan käyttäjän syöttämä skaalattu arvo
// ulostuloon
output = actuatorVoltage;
}
else
{
// PID-säädössä ajetaan algoritmi ja asetetaan ulostulo
output = pid(setValue/1000.0, sensorVoltage/1000.0, kp/1000.0, ki/1000.0, kd/1000.0);
actuatorVoltage = output;
}
}
// asetetaan ulostulo oikeasti toimilaitteelle
actuator->setValue(output);
// qDebug()<< "Set " << output<< "to actuator";
// luetaan käyttöliittymästä mahdollisesti tullut viesti
xenoRet = 0;
xenoRet = rt_pipe_read(&pipe_desc, bufChar, sizeof(bufChar), TM_NONBLOCK);
// jos oltiin saatu viesti, luetaan sen sisältö ja muutetaan tai
// palautetaan lukuarvoja ja tiloja tarvittaessa
// mutexia ei tarvita, koska tämä tehdään joka kierroksella
// säätölaskennan ja säädön jälkeen
if(xenoRet > 0)
{
if(!strcmp(bufChar, "reaSen"))
{
sensorVoltageToSend = sensorVoltage;
int *pBuf = &sensorVoltageToSend;
if(rt_pipe_write(&pipe_desc, (void*)pBuf, sizeof(bufInt), P_NORMAL) < 1)
qDebug("rt write error");
bufChar[0] = '0';
}
else if(!strcmp(bufChar, "reaOut"))
{
actuatorVoltageToSend = actuatorVoltage;
int *pBuf = &actuatorVoltageToSend;
if(rt_pipe_write(&pipe_desc, (void*)pBuf, sizeof(bufInt), P_NORMAL) < 1)
qDebug("rt write error");
bufChar[0] = '0';
}
else if(!strcmp(bufChar, "reaMod"))
{
modeToSend = mode;
RealtimeController::Mode *pBuf = &modeToSend;
if(rt_pipe_write(&pipe_desc, (void*)pBuf, sizeof(bufInt), P_NORMAL) < 1)
qDebug("rt write error");
bufChar[0] = '0';
}
else if(!strcmp(bufChar, "reaSta"))
{
stateToSend = state;
RealtimeController::State *pBuf = &stateToSend;
if(rt_pipe_write(&pipe_desc, (void*)pBuf, sizeof(bufInt), P_NORMAL) < 1)
qDebug("rt write error");
bufChar[0] = '0';
}
else if (!strcmp(bufChar, "setSta"))
{
if(rt_pipe_read(&pipe_desc, bufState, sizeof(bufState), TM_INFINITE) < 1)
qDebug("rt read error");
state = *(RealtimeController::State*)bufState;
bufChar[0] = '0';
qDebug("Set state: %d", state);
}
else if (!strcmp(bufChar, "setPID"))
{
if(rt_pipe_read(&pipe_desc, bufInt, sizeof(bufInt), TM_INFINITE) < 1)
qDebug("rt read error");
kp = *(int*)bufInt;
if(rt_pipe_read(&pipe_desc, bufInt, sizeof(bufInt), TM_INFINITE) < 1)
qDebug("rt read error");
ki = *(int*)bufInt;
if(rt_pipe_read(&pipe_desc, bufInt, sizeof(bufInt), TM_INFINITE) < 1)
qDebug("rt read error");
kd = *(int*)bufInt;
bufChar[0] = '0';
qDebug("Set PID-parameters p: %d, i: %d, d: %d", kp,ki,kd);
}
else if (!strcmp(bufChar, "setSet"))
{
if(rt_pipe_read(&pipe_desc, bufInt, sizeof(bufInt), TM_INFINITE) < 1)
qDebug("rt read error");
setValue = *(int*)bufInt;
bufChar[0] = '0';
qDebug("Set setValue: %d", setValue);
}
else if (!strcmp(bufChar, "setOut"))
{
if(rt_pipe_read(&pipe_desc, bufInt, sizeof(bufInt), TM_INFINITE) < 1)
qDebug("rt read error");
actuatorVoltage = *(int*)bufInt;
bufChar[0] = '0';
qDebug("Set actuatorVoltage: %d", actuatorVoltage);
}
else if (!strcmp(bufChar, "setSca"))
{
if(rt_pipe_read(&pipe_desc, bufInt, sizeof(bufInt), TM_INFINITE) < 1)
qDebug("rt read error");
sensor->setScale(*(int*)bufInt);
if(rt_pipe_read(&pipe_desc, bufInt, sizeof(bufInt), TM_INFINITE) < 1)
qDebug("rt read error");
actuator->setScale(*(int*)bufInt);
bufChar[0] = '0';
qDebug("Set sensor and, actuator scales");
}
else if (!strcmp(bufChar, "setMod"))
{
if(rt_pipe_read(&pipe_desc, bufMode, sizeof(bufMode), TM_INFINITE) < 1)
qDebug("rt read error");
mode = *(RealtimeController::Mode*)bufMode;
bufChar[0] = '0';
qDebug("Set mode to: %d", mode);
}
else if(!strcmp(bufChar, "theEnd"))
{
qDebug("Bye!");
break;
}
else
qDebug("rt read error: unknown string");
}
}
}
/* Called by PLC thread on each new python command*/
void UnBlockPythonCommands(void)
{
char msg = PYTHON_PENDING_COMMAND;
rt_pipe_write(&WaitPython_pipe, &msg, sizeof(msg), P_NORMAL);
}
