RealtimeController::RealtimeController()
{
// lukitaan ohjelman nykyinen ja tuleva muisti niin että se pysyy RAM:ssa kokoajan
mlockall(MCL_CURRENT | MCL_FUTURE);
int xenoError = 0;
// luodaan task-handle reaaliaikasäikeelle
// antamalla T_FPU | T_JOINABLE saattaisi olla mahdollista käyttää
// mittausarvoja doubleina Voltteina koko ohjelmassa
xenoError = rt_task_create(&task_desc, NULL, 0, 99, T_JOINABLE);
xenoError = rt_pipe_create(&pipe_desc, NULL, 0, 0);
if(xenoError != 0)
qDebug("rt init error");
// käynnistetään säie
rt_task_start(&task_desc, &realtimeLoop, NULL);
// luodaan pipe reaaliaikasäikeen kanssa kommunikointiin
pipefd = open("/dev/rtp0", O_RDWR, 0);
if (pipefd < 0)
qDebug("Creating pipe failed");
// pysäytetään säätö oletuksena
stop();
}
int rt_pipe_create(RT_PIPE *pipe, const char *name,
int minor, size_t poolsize)
{
int ret;
ret = __CURRENT(rt_pipe_create(pipe, name, minor, poolsize));
return ret < 0 ? ret : 0;
}
void AuxTaskNonRT::__create(){
// create the xenomai task
int priority = 0;
int stackSize = 65536 * 4;
#ifdef XENOMAI_SKIN_native //posix skin does evertything in one go below
if (int ret = rt_task_create(&task, name, stackSize, priority, T_JOINABLE))
{
fprintf(stderr, "Unable to create AuxTaskNonRT %s: %i\n", name, ret);
return;
}
#endif
// create an rt_pipe
char p_name [30];
sprintf (p_name, "p_%s", name);
#ifdef XENOMAI_SKIN_native
rt_pipe_delete(&pipe);
int ret = rt_pipe_create(&pipe, p_name, P_MINOR_AUTO, 0);
if(ret < 0)
#endif
#ifdef XENOMAI_SKIN_posix
int pipeSize = 65536 * 10;
int ret = createXenomaiPipe(p_name, pipeSize);
pipeSocket = ret;
if(ret <= 0)
#endif
{
fprintf(stderr, "Unable to create AuxTaskNonRT %s pipe %s: (%i) %s\n", name, p_name, ret, strerror(ret));
return;
}
// start the xenomai task
#ifdef XENOMAI_SKIN_native
if (int ret = rt_task_start(&task, AuxTaskNonRT::loop, this))
#endif
#ifdef XENOMAI_SKIN_posix
if(int ret = create_and_start_thread(&thread, name, priority, stackSize, (pthread_callback_t*)AuxTaskNonRT::loop, this))
#endif
{
fprintf(stderr, "Unable to start AuxTaskNonRT %s: %i, %s\n", name, ret, strerror(ret));
return;
}
}
static int __init klat_mod_init(void)
{
char devname[RTDM_MAX_DEVNAME_LEN + 1];
unsigned dev_nr;
int err;
err = rt_pipe_create(&klat_pipe, "klat_pipe", pipe, 4096);
if (err) {
printk("rt_pipe_create(klat_pipe): %d\n", err);
return err;
}
err = rt_task_create(&klat_srvr, "klat_srvr", 0, 0, 0);
if (err) {
printk("rt_task_create(klat_srvr): %d\n", err);
goto err_close_pipe;
}
pkt.config.mode = mode;
pkt.config.priority = priority;
pkt.config.period = period * 1000;
pkt.config.warmup_loops = 1;
pkt.config.histogram_size = 0;
pkt.config.freeze_max = freeze_max;
for (dev_nr = 0; dev_nr < DEV_NR_MAX; dev_nr++) {
snprintf(devname, sizeof(devname),
"rttest-timerbench%d",
dev_nr);
fd = rt_dev_open(devname, O_RDONLY);
if (fd < 0)
continue;
err = rt_dev_ioctl(fd, RTTST_RTIOC_TMBENCH_START, &pkt.config);
if (err == -ENOTTY) {
rt_dev_close(fd);
continue;
}
if (err < 0) {
printk("rt_dev_ioctl(RTTST_RTIOC_TMBENCH_START): %d\n",
err);
goto err_destroy_task;
}
break;
}
if (fd < 0) {
printk("rt_dev_open: could not find rttest device\n"
"(modprobe timerbench?)");
err = fd;
goto err_destroy_task;
}
err = rt_task_start(&klat_srvr, &klat_server, NULL);
if (err) {
printk("rt_task_start: %d\n", err);
goto err_close_dev;
}
return 0;
err_close_dev:
rt_dev_close(fd);
err_destroy_task:
rt_task_delete(&klat_srvr);
err_close_pipe:
rt_pipe_delete(&klat_pipe);
return err;
}
int main(void)
{
unsigned long long before;
RT_ALARM nalrm;
RT_BUFFER nbuf;
RT_COND ncond;
RT_EVENT nevt;
RT_HEAP nheap;
RT_MUTEX nmtx;
RT_PIPE npipe;
RT_QUEUE nq;
RT_SEM nsem;
RT_TASK ntsk;
int failed = 0;
mlockall(MCL_CURRENT|MCL_FUTURE);
rt_print_auto_init(1);
rt_fprintf(stderr, "Checking for leaks in native skin services\n");
before = get_used();
check_native(rt_alarm_create(&nalrm, NULL));
check_native(rt_alarm_delete(&nalrm));
check_used("alarm", before, failed);
before = get_used();
check_native(rt_buffer_create(&nbuf, NULL, 16384, B_PRIO));
check_native(rt_buffer_delete(&nbuf));
check_used("buffer", before, failed);
before = get_used();
check_native(rt_cond_create(&ncond, NULL));
check_native(rt_cond_delete(&ncond));
check_used("cond", before, failed);
before = get_used();
check_native(rt_event_create(&nevt, NULL, 0, EV_PRIO));
check_native(rt_event_delete(&nevt));
check_used("event", before, failed);
before = get_used();
check_native(rt_heap_create(&nheap, "heap", 16384, H_PRIO | H_SHARED));
check_native(rt_heap_delete(&nheap));
check_used("heap", before, failed);
before = get_used();
check_native(rt_mutex_create(&nmtx, NULL));
check_native(rt_mutex_delete(&nmtx));
check_used("mutex", before, failed);
before = get_used();
check_native(rt_pipe_create(&npipe, NULL, P_MINOR_AUTO, 0));
check_native(rt_pipe_delete(&npipe));
check_used("pipe", before, failed);
before = get_used();
check_native(rt_queue_create(&nq, "queue", 16384, Q_UNLIMITED, Q_PRIO));
check_native(rt_queue_delete(&nq));
check_used("queue", before, failed);
before = get_used();
check_native(rt_sem_create(&nsem, NULL, 0, S_PRIO));
check_native(rt_sem_delete(&nsem));
check_used("sem", before, failed);
before = get_used();
check_native(rt_task_spawn(&ntsk, NULL, 0, 1, T_JOINABLE, empty, NULL));
check_native(rt_task_join(&ntsk));
sleep(1); /* Leave some time for xnheap
* deferred free */
check_used("task", before, failed);
return failed ? EXIT_FAILURE : EXIT_SUCCESS;
}
int init_mpu9150_irq(void)
{
void __iomem *mem;
int retval, irq;
uint regval;
rtdm_printk("MPU9150-IRQ: Initializing interrupt interface...\n");
// Preliminary Setup: Step 1: Create a message pipe for use within the ISR
retval = rt_pipe_create(&pipe_desc, "mpu9150", P_MINOR_AUTO, MPU9150_FIFO_DEPTH+synchSize);
if( retval )
{
rtdm_printk("MPU9150-IRQ: Error: Failed to create data flow pipe\n");
return retval;
}
// 1st Step: Request the GPIO lines for the IRQ channels and create the IRQ object
retval = gpio_request_one(mpu9150_irq_desc.gpio_desc.gpio, mpu9150_irq_desc.gpio_desc.flags, mpu9150_irq_desc.gpio_desc.label);
if(retval != 0)
{
irq_gpio_requested = 0;
rtdm_printk("MPU9150-IRQ: Error: Failed to request GPIO pin#%i for IRQ (error %i)\n", mpu9150_irq_desc.gpio_desc.gpio, retval);
return retval;
}
irq_gpio_requested = 1;
irq = gpio_to_irq( mpu9150_irq_desc.gpio_desc.gpio );
if(irq >= 0)
{
irq_requested = 1;
if((retval = rtdm_irq_request(&mpu9150_irq, irq, mpu9150_irq_desc.isr, 0, mpu9150_irq_desc.gpio_desc.label, NULL)) < 0)
{
switch( retval )
{
case -EINVAL:
rtdm_printk("MPU9150-IRQ: ERROR: Invalid parameter was given to rtdm_irq_request().\n");
break;
case -EBUSY:
rtdm_printk("MPU9150-IRQ: ERROR: The requested IRQ line#%i from GPIO#%i is already in use\n", irq, mpu9150_irq_desc.gpio_desc.gpio);
break;
default:
rtdm_printk("MPU9150-IRQ: Unknown error occurred while requesting RTDM IRQ.\n");
}
return retval;
}
rtdm_printk("MPU9150-IRQ: Initialized GPIO #%i to IRQ #%i\n", mpu9150_irq_desc.gpio_desc.gpio, irq);
}
else
{
irq_requested = 0;
rtdm_printk("MPU9150-IRQ: ERROR: Failed to obtain IRQ number for GPIO #%i (error %i)\n", mpu9150_irq_desc.gpio_desc.gpio, retval);
return retval;
}
// 2nd Step: Configure the pin for GPIO input
rtdm_printk("MPU9150-IRQ: Configuring IRQ GPIO pin as follows: SLEW_FAST | INPUT_EN | PULLUP | PULLUPDOWN_DIS | MODE_7....\n");
mem = ioremap(GPMC_CONF_ADDR_START, GPMC_CONF_ADDR_SIZE);
if( !mem )
{
rtdm_printk("MPU9150-IRQ: ERROR: Failed to remap memory for IRQ pin configuration.\n");
return -ENOMEM;
}
// Configure GPIO2_4 pin 0-7 output
// Write the pin configuration
iowrite8((SLEW_FAST | INPUT_EN | PULLUP | PULLUPDOWN_DIS | M7), mem + CONF_GPMC_WEN);
// close the pin conf address space
iounmap( mem );
// GPIO Bank 2: Setup the IRQ registers with the appropriate values
mem = ioremap(GPIO2_START_ADDR, GPIO2_SIZE);
if( !mem )
{
rtdm_printk("MPU9150-IRQ: ERROR: Failed to remap memory for GPIO Bank 2 IRQ pin configuration.\n");
return -ENOMEM;
}
// Enable the IRQ ability for GPIO2_4
regval = ioread32(mem + GPIO_IRQSTATUS_0);
regval |= GPIO_4;
iowrite32(regval, mem + GPIO_IRQSTATUS_0);
// Set Pin 4 for rising- and falling- edge detection
regval = ioread32(mem + GPIO_RISINGDETECT);
regval |= GPIO_4;
iowrite32(regval, mem + GPIO_RISINGDETECT);
// Release the mapped memory
iounmap( mem );
// 3rd Step: Setup the IRQ controller to read the H/W interrupts
mem = ioremap(INTC_REG_START, INTC_REG_SIZE);
if( !mem )
{
rtdm_printk("MPU9150-IRQ: ERROR: Failed to remap IRQ Control Registers memory\n");
return -ENOMEM;
}
// Disable clock gating strategy for the. Will use more
// power with this strategy disabled, but will decrease
// latency.
iowrite32(0x0, mem + INTC_SYSCONFIG);
// Disable the input synchronizer auto-gating and the
// functional clock auto-idle modes. This will use more
// power, but will decrease latency.
iowrite32(0x1, mem + INTC_IDLE);
// For each IRQ line, set the highest priority (0) and
// route to the IRQ, not the FIQ (GPIOs cannot be routed
// to FIQ)
iowrite8(0x0, mem + INTC_ILR68);
// Unmask the the GPIO lines to enable interrupts on those GPIO lines
iowrite32(GPIO_4, mem + INTC_MIR_CLEAR2);
// Close the IRQ Control registers memory
iounmap( mem );
rtdm_printk("MPU9150-IRQ: Interrupt interface initialization complete!\n");
return 0;
}
int startPLC(int argc,char **argv)
{
signal(SIGINT, catch_signal);
/* no memory swapping for that process */
mlockall(MCL_CURRENT | MCL_FUTURE);
PLC_shutdown = 0;
/*** RT Pipes creation and opening ***/
/* create Debug_pipe */
if(rt_pipe_create(&Debug_pipe, "Debug_pipe", DEBUG_PIPE_MINOR, PIPE_SIZE))
goto error;
PLC_state |= PLC_STATE_DEBUG_PIPE_CREATED;
/* open Debug_pipe*/
if((Debug_pipe_fd = open(DEBUG_PIPE_DEVICE, O_RDWR)) == -1) goto error;
PLC_state |= PLC_STATE_DEBUG_FILE_OPENED;
/* create Python_pipe */
if(rt_pipe_create(&Python_pipe, "Python_pipe", PYTHON_PIPE_MINOR, PIPE_SIZE))
goto error;
PLC_state |= PLC_STATE_PYTHON_PIPE_CREATED;
/* open Python_pipe*/
if((Python_pipe_fd = open(PYTHON_PIPE_DEVICE, O_RDWR)) == -1) goto error;
PLC_state |= PLC_STATE_PYTHON_FILE_OPENED;
/* create WaitDebug_pipe */
if(rt_pipe_create(&WaitDebug_pipe, "WaitDebug_pipe", WAITDEBUG_PIPE_MINOR, PIPE_SIZE))
goto error;
PLC_state |= PLC_STATE_WAITDEBUG_PIPE_CREATED;
/* open WaitDebug_pipe*/
if((WaitDebug_pipe_fd = open(WAITDEBUG_PIPE_DEVICE, O_RDWR)) == -1) goto error;
PLC_state |= PLC_STATE_WAITDEBUG_FILE_OPENED;
/* create WaitPython_pipe */
if(rt_pipe_create(&WaitPython_pipe, "WaitPython_pipe", WAITPYTHON_PIPE_MINOR, PIPE_SIZE))
goto error;
PLC_state |= PLC_STATE_WAITPYTHON_PIPE_CREATED;
/* open WaitPython_pipe*/
if((WaitPython_pipe_fd = open(WAITPYTHON_PIPE_DEVICE, O_RDWR)) == -1) goto error;
PLC_state |= PLC_STATE_WAITPYTHON_FILE_OPENED;
/*** create PLC task ***/
if(rt_task_create(&PLC_task, "PLC_task", 0, 50, T_JOINABLE)) goto error;
PLC_state |= PLC_STATE_TASK_CREATED;
if(__init(argc,argv)) goto error;
/* start PLC task */
if(rt_task_start(&PLC_task, &PLC_task_proc, NULL)) goto error;
return 0;
error:
PLC_cleanup_all();
return 1;
}
