Echtzeitdatenverarbeitung/Bearbeiten2.c

236 lines
5.4 KiB
C
Raw Normal View History

2020-10-19 18:46:24 +02:00
#include <rtai_mbx.h>
#include <rtai_sched.h>
#include <uint128.h>
#include <sys/rtai_modbus.h>
2020-10-26 10:38:48 +01:00
#include "include/rtai_lxrt.h"
#include "include/rtai_modbus.h"
#include "include/rtai_sem.h"
#include "include/rtai_mbx.h"
2020-10-19 18:46:24 +02:00
2020-10-19 19:58:28 +02:00
#define SENSOR_PART_TURNTABLE 1<<0
#define SENSOR_PART_DRILL 1<<1
#define SENSOR_PART_MESURING 1<<2
#define SENSOR_DRILL_UP 1<<3
#define SENSOR_DRILL_DOWN 1<<4
#define SENSOR_TURNTABLE_POS 1<<5
#define SENSOR_PART_TEST 1<<6
#define ACTOR_DRILL 1<<0
#define ACTOR_TURNTABLE 1<<1
#define ACTOR_DRILL_DOWN 1<<2
#define ACTOR_DRILL_UP 1<<3
#define ACTOR_PART_HOLD 1<<4
#define ACTOR_TESTER 1<<5
#define ACTOR_EXIT 1<<6
#define ACTOR_ENTRANCE 1<<7
2020-10-19 18:46:24 +02:00
MODULE_LICENSE("GPL");
2020-10-21 16:15:24 +02:00
static RT_TASK turntable_task, drill_task, tester_task, output_task;
2020-10-26 10:38:48 +01:00
static MBX tester_mbx, output_mbx;
2020-10-21 16:15:24 +02:00
static SEM semaphore;
2020-10-19 18:46:24 +02:00
2020-10-26 10:38:48 +01:00
int connection;
2020-10-21 16:15:24 +02:00
char node[] = "modbus-node";
2020-10-26 10:38:48 +01:00
void fail() {
rt_modbus_disconnect(connection);
rt_task_delete(&turntable_task);
rt_task_delete(&tester_task);
rt_task_delete(&drill_task);
rt_task_delete(&output_task);
rt_mbx_delete(&tester_mbx);
rt_mbx_delete(&output_mbx);
rt_sem_delete(&semaphore);
stop_rt_timer();
rt_printk("failed to read/send Modbus message.\n");
rt_printk("module needs to be restarted.\n");
}
2020-10-21 16:15:24 +02:00
/**
2020-10-26 10:38:48 +01:00
* readData all bits of the specified type
2020-10-21 16:15:24 +02:00
* this function is *not* thread safe.
* @param type (DIGITAL_IN, DIGITAL_OUT, ANALOG_IN, ANALOG_OUT)
* @param connection modbus connection id
* @param result value to write the result to
* @return status code(0: success, 1: failure)
*/
2020-10-26 10:38:48 +01:00
int readAll(int type, int *result) {
return rt_modbus_get(connection, type, 0, (unsigned short *) &result));
2020-10-19 19:58:28 +02:00
}
2020-10-21 16:15:24 +02:00
/**
*
* this function is thread safe.
* @param connection
* @param part
* @param result value to write the result to
* @return status code(0: success, 1: failure)
*/
2020-10-26 10:38:48 +01:00
int readData(int part, int *result) {
2020-10-21 16:15:24 +02:00
rt_sem_wait(&semaphore);
2020-10-19 19:58:28 +02:00
int value;
2020-10-26 10:38:48 +01:00
int code = readAll(DIGITAL_IN, &value);
2020-10-19 19:58:28 +02:00
*result = (value & part) != 0;
2020-10-21 16:15:24 +02:00
rt_sem_signal(&semaphore);
2020-10-19 19:58:28 +02:00
return code;
}
2020-10-21 16:15:24 +02:00
/**
* disable a specified actor of the system.
* this function is thread safe.
* @param connection modbus connection id
* @param actor bitmask of the actor to disable
*/
2020-10-26 10:38:48 +01:00
void disable(int actor) {
2020-10-21 16:15:24 +02:00
rt_sem_wait(&semaphore);
2020-10-19 19:58:28 +02:00
int value;
2020-10-26 10:38:48 +01:00
int code = readAll(DIGITAL_OUT, &value);
if(code) fail();
int result = rt_modbus_set(connection, DIGITAL_OUT, value &= ~actor)
2020-10-21 16:15:24 +02:00
rt_sem_signal(&semaphore);
2020-10-26 10:38:48 +01:00
if (result) fail();
2020-10-19 19:58:28 +02:00
}
2020-10-21 16:15:24 +02:00
/**
* enable one specified actor of the system.
* this function is thread safe.
* @param connection modbus connection id
* @param actor bitmask of the actor to enable
*/
2020-10-26 10:38:48 +01:00
void enable(int actor) {
2020-10-21 16:15:24 +02:00
rt_sem_wait(&semaphore);
int output;
2020-10-26 10:38:48 +01:00
int code = readAll(DIGITAL_OUT, &output);
if (code) fail();
2020-10-21 16:15:24 +02:00
int result = rt_modbus_set(connection, DIGITAL_OUT, output |= actor)
rt_sem_signal(&semaphore);
2020-10-26 10:38:48 +01:00
if(result) fail();
2020-10-19 19:58:28 +02:00
}
2020-10-26 10:38:48 +01:00
void sleepStuff() {
2020-10-21 16:15:24 +02:00
rt_sleep(1000 * nano2count(1000000));
}
static void turntable(long x) {
2020-10-19 19:58:28 +02:00
rt_printk("turntable: MODBUS communication opened\n");
2020-10-26 10:38:48 +01:00
disable(ACTOR_TURNTABLE);
2020-10-21 16:15:24 +02:00
2020-10-19 19:58:28 +02:00
while (1) {
int partOnTable;
2020-10-26 10:38:48 +01:00
readData(SENSOR_PART_TURNTABLE, &partOnTable);
2020-10-19 19:58:28 +02:00
if (partOnTable) {
2020-10-26 10:38:48 +01:00
enable(ACTOR_TURNTABLE);
2020-10-19 19:58:28 +02:00
} else {
2020-10-26 10:38:48 +01:00
disable(ACTOR_TURNTABLE);
2020-10-19 19:58:28 +02:00
}
2020-10-21 16:15:24 +02:00
rt_task_resume(&drill_task);
rt_task_resume(&tester_task);
rt_task_resume(&output_task);
2020-10-19 19:58:28 +02:00
}
2020-10-19 18:46:24 +02:00
}
2020-10-21 16:15:24 +02:00
static void drill(long x) {
rt_printk("drill: MODBUS communication opened\n");
2020-10-26 10:38:48 +01:00
disable(ACTOR_DRILL);
disable(ACTOR_PART_HOLD);
disable(ACTOR_DRILL_DOWN);
2020-10-21 16:15:24 +02:00
2020-10-26 10:38:48 +01:00
enable(ACTOR_DRILL_UP);
rt_sem_wait(&semaphore);
2020-10-21 16:15:24 +02:00
int drill_up = 0;
2020-10-26 10:38:48 +01:00
readData(SENSOR_DRILL_UP, drill_up);
2020-10-21 16:15:24 +02:00
if (!drill_up) {
do {
2020-10-26 10:38:48 +01:00
readData(SENSOR_DRILL_UP, drill_up);
2020-10-21 16:15:24 +02:00
} while (drill_up == 0);
}
2020-10-26 10:38:48 +01:00
rt_sem_signal(&semaphore);
disable(ACTOR_DRILL_UP);
2020-10-21 16:15:24 +02:00
while (1) {
2020-10-26 10:38:48 +01:00
sleepStuff();
2020-10-21 16:15:24 +02:00
}
}
static void tester(long x) {
while (1) {
2020-10-26 10:38:48 +01:00
sleepStuff();
2020-10-21 16:15:24 +02:00
}
}
static void output(long x) {
while (1) {
2020-10-26 10:38:48 +01:00
sleepStuff();
2020-10-21 16:15:24 +02:00
}
2020-10-19 18:46:24 +02:00
}
static int __init
2020-10-19 19:58:28 +02:00
example_init(void) {
rt_set_oneshot_mode();
start_rt_timer(0);
modbus_init();
2020-10-26 10:38:48 +01:00
rt_printk("init: task started\n");
if ((connection = rt_modbus_connect(node)) == -1) {
rt_printk("init: could not connect to %s\n", node);
return;
}
2020-10-21 16:15:24 +02:00
rt_sem_init(&semaphore, 1);
2020-10-26 10:38:48 +01:00
if (rt_task_init(&turntable_task, turntable, 0, 1024, 10, 0, NULL)) {
2020-10-21 16:15:24 +02:00
printk("turntable: cannot initialize task\n");
goto fail;
}
if (rt_task_init(&drill_task, drill, 0, 1024, 0, 0, NULL)) {
printk("drill: cannot initialize task\n");
goto fail;
}
if (rt_task_init(&turntable_task, tester, 0, 1024, 0, 0, NULL)) {
printk("tester: cannot initialize task\n");
goto fail;
}
if (rt_task_init(&turntable_task, output, 0, 1024, 0, 0, NULL)) {
printk("output: cannot initialize task\n");
2020-10-19 19:58:28 +02:00
goto fail;
}
2020-10-21 16:15:24 +02:00
rt_task_resume(&turntable_task);
2020-10-19 19:58:28 +02:00
printk("loaded module Bearbeiten2\n");
return (0);
fail:
stop_rt_timer();
return (1);
2020-10-19 18:46:24 +02:00
}
2020-10-26 10:38:48 +01:00
static void __exit
example_exit(void) {
rt_task_delete(&turntable_task);
rt_task_delete(&drill_task);
rt_task_delete(&tester_task);
rt_task_delete(&output_task);
stop_rt_timer();
rt_sem_delete(&semaphore);
printk("module Bearbeiten2 unloaded\n");
}
2020-10-19 18:46:24 +02:00
module_exit(example_exit)
module_init(example_init)