335 lines
8.6 KiB
C
335 lines
8.6 KiB
C
#include <rtai_mbx.h>
|
|
#include <rtai_sched.h>
|
|
#include <rtai_sem.h>
|
|
#include <sys/rtai_modbus.h>
|
|
|
|
//#include "include/rtai_mbx.h"
|
|
//#include "include/rtai_sched.h"
|
|
//#include "include/rtai_sem.h"
|
|
//#include "include/rtai_modbus.h"
|
|
//#include <stdbool.h>
|
|
|
|
#include <uint128.h>
|
|
|
|
#define SENSOR_PART_TURNTABLE 1<<0
|
|
#define SENSOR_PART_DRILL 1<<1
|
|
#define SENSOR_PART_TESTER 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
|
|
|
|
MODULE_LICENSE("GPL");
|
|
|
|
static RT_TASK turntable_task, drill_task, tester_task, output_task;
|
|
static MBX turntable_status_mbx, tester_status_mbx, output_status_mbx, drill_status_mbx;
|
|
static MBX output_data_mbx, drill_data_mbx, turntable_data_mbx;
|
|
static SEM semaphore;
|
|
|
|
int connection;
|
|
|
|
char node[] = "modbus-node";
|
|
|
|
void end(bool 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(&turntable_status_mbx);
|
|
rt_mbx_delete(&tester_status_mbx);
|
|
rt_mbx_delete(&output_status_mbx);
|
|
rt_mbx_delete(&drill_status_mbx);
|
|
rt_mbx_delete(&output_data_mbx);
|
|
rt_mbx_delete(&drill_data_mbx);
|
|
|
|
rt_sem_delete(&semaphore);
|
|
|
|
stop_rt_timer();
|
|
|
|
if(fail) {
|
|
rt_printk("failed to read/send Modbus message.\n");
|
|
rt_printk("module needs to be restarted.\n");
|
|
}
|
|
}
|
|
|
|
/**
|
|
* readData all bits of the specified type
|
|
* 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)
|
|
*/
|
|
int readAll(int type, int *result) {
|
|
int res = rt_modbus_get(connection, type, 0, (unsigned short *) result);
|
|
return res;
|
|
}
|
|
|
|
/**
|
|
*
|
|
* this function is thread safe.
|
|
* @param part
|
|
* @return
|
|
*/
|
|
int readData(int part) {
|
|
rt_sem_wait(&semaphore);
|
|
int value = 0;
|
|
int code = readAll(DIGITAL_IN, &value);
|
|
int result = (part & value);
|
|
rt_sem_signal(&semaphore);
|
|
if(code) end(true);
|
|
return result;
|
|
}
|
|
|
|
/**
|
|
* 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
|
|
*/
|
|
void disable(int actor) {
|
|
rt_sem_wait(&semaphore);
|
|
int value = 0;
|
|
int code = readAll(DIGITAL_OUT, &value);
|
|
if(code) end(true);
|
|
int result = rt_modbus_set(connection, DIGITAL_OUT, 0, value &= ~actor);
|
|
rt_sem_signal(&semaphore);
|
|
if (result) end(true);
|
|
}
|
|
|
|
/**
|
|
* 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
|
|
*/
|
|
void enable(int actor) {
|
|
rt_sem_wait(&semaphore);
|
|
int output = 0;
|
|
int code = readAll(DIGITAL_OUT, &output);
|
|
if (code) end(true);
|
|
int result;
|
|
result = rt_modbus_set(connection, DIGITAL_OUT, 0, output |= actor);
|
|
rt_sem_signal(&semaphore);
|
|
if(result) end(true);
|
|
}
|
|
|
|
void sendMail(MBX * mailbox, int msg) {
|
|
int mbxStatus = rt_mbx_send(mailbox, &msg, sizeof(int));
|
|
if(mbxStatus == EINVAL) end(true);
|
|
}
|
|
|
|
void sendMailNonBlocking(MBX * mailbox, int msg) {
|
|
int mbxStatus = rt_mbx_send_if(mailbox, &msg, sizeof(int));
|
|
if(mbxStatus == EINVAL) end(true);
|
|
}
|
|
|
|
int receiveMail(MBX * mailbox) {
|
|
int msg = 0;
|
|
int mbxStatus = rt_mbx_receive(mailbox, &msg, sizeof(int));
|
|
if(mbxStatus == EINVAL) end(true);
|
|
return msg;
|
|
}
|
|
|
|
int receiveMailNonBlocking(MBX * mailbox) {
|
|
int msg = 0;
|
|
int mbxStatus = rt_mbx_receive_if(mailbox, &msg, sizeof(int));
|
|
if(mbxStatus == EINVAL) end(true);
|
|
return msg;
|
|
}
|
|
|
|
void sleepStuff(void) {
|
|
rt_sleep(1000 * nano2count(1000000));
|
|
}
|
|
|
|
static void turntable(long data) {
|
|
rt_printk("started turntable task\n");
|
|
disable(ACTOR_TURNTABLE);
|
|
|
|
while (1) {
|
|
receiveMail(&turntable_status_mbx);
|
|
receiveMail(&turntable_status_mbx);
|
|
receiveMail(&turntable_status_mbx);
|
|
int partOnTable = readData(SENSOR_PART_TURNTABLE);
|
|
int msg1 = receiveMailNonBlocking(&turntable_data_mbx);
|
|
int msg2 = receiveMailNonBlocking(&turntable_data_mbx);
|
|
if(partOnTable || msg1 || msg2) {
|
|
enable(ACTOR_TURNTABLE);
|
|
sleepStuff();
|
|
disable(ACTOR_TURNTABLE);
|
|
sleepStuff();
|
|
}
|
|
sendMail(&tester_status_mbx, 1);
|
|
sendMail(&drill_status_mbx, 1);
|
|
sendMail(&output_status_mbx, 1);
|
|
}
|
|
}
|
|
|
|
static void tester(long data) {
|
|
rt_printk("started tester task\n");
|
|
|
|
disable(ACTOR_TESTER);
|
|
sendMail(&turntable_status_mbx, 1);
|
|
while (1) {
|
|
receiveMail(&tester_status_mbx);
|
|
int partOnTable = readData(SENSOR_PART_TESTER);
|
|
if(partOnTable) {
|
|
sendMailNonBlocking(&turntable_data_mbx, 1);
|
|
enable(ACTOR_TESTER);
|
|
sleepStuff();
|
|
int partNormal = readData(SENSOR_PART_TEST);
|
|
if(partNormal) {
|
|
sendMailNonBlocking(&drill_data_mbx, 1);
|
|
}else {
|
|
sendMailNonBlocking(&drill_data_mbx, 0);
|
|
}
|
|
disable(ACTOR_TESTER);
|
|
}
|
|
sendMail(&turntable_status_mbx, 1);
|
|
}
|
|
}
|
|
|
|
static void drill(long data) {
|
|
disable(ACTOR_DRILL);
|
|
disable(ACTOR_PART_HOLD);
|
|
disable(ACTOR_DRILL_DOWN);
|
|
|
|
enable(ACTOR_DRILL_UP);
|
|
if (!readData(SENSOR_DRILL_UP)) {
|
|
do {
|
|
} while (readData(SENSOR_DRILL_UP) == 0);
|
|
}
|
|
disable(ACTOR_DRILL_UP);
|
|
sendMail(&turntable_status_mbx, 1);
|
|
|
|
while (1) {
|
|
receiveMail(&drill_status_mbx);
|
|
int partOnTable = readData(SENSOR_PART_DRILL);
|
|
int msg = receiveMailNonBlocking(&drill_data_mbx);
|
|
if(partOnTable) {
|
|
sendMailNonBlocking(&turntable_data_mbx, 1);
|
|
int msg = receiveMailNonBlocking(&drill_data_mbx);
|
|
if(msg) {
|
|
enable(ACTOR_PART_HOLD);
|
|
enable(ACTOR_DRILL);
|
|
enable(ACTOR_DRILL_DOWN);
|
|
if (!readData(SENSOR_DRILL_DOWN)) {
|
|
do {
|
|
} while (readData(SENSOR_DRILL_DOWN) == 0);
|
|
}
|
|
sleepStuff();
|
|
disable(ACTOR_DRILL);
|
|
disable(ACTOR_DRILL_DOWN);
|
|
enable(ACTOR_DRILL_UP);
|
|
if (!readData(SENSOR_DRILL_UP)) {
|
|
do {
|
|
} while (readData(SENSOR_DRILL_UP) == 0);
|
|
}
|
|
disable(ACTOR_DRILL_UP);
|
|
disable(ACTOR_PART_HOLD);
|
|
}
|
|
sendMailNonBlocking(&output_data_mbx, 1);
|
|
}
|
|
|
|
|
|
sendMail(&turntable_status_mbx, 1);
|
|
}
|
|
}
|
|
|
|
static void output(long data) {
|
|
disable(ACTOR_EXIT);
|
|
sendMail(&turntable_status_mbx, 1);
|
|
while (1) {
|
|
receiveMail(&output_status_mbx);
|
|
int msg = receiveMailNonBlocking(&output_data_mbx);
|
|
|
|
if(msg) {
|
|
enable(ACTOR_EXIT);
|
|
sleepStuff();
|
|
disable(ACTOR_EXIT);
|
|
}
|
|
sendMail(&turntable_status_mbx, 1);
|
|
}
|
|
}
|
|
|
|
static int __init
|
|
example_init(void) {
|
|
rt_set_oneshot_mode();
|
|
start_rt_timer(0);
|
|
modbus_init();
|
|
|
|
rt_printk("init: started\n");
|
|
if ((connection = rt_modbus_connect(node)) == -1) {
|
|
rt_printk("init: could not connect to %s\n", node);
|
|
return -1;
|
|
}
|
|
|
|
rt_sem_init(&semaphore, 1);
|
|
|
|
if (rt_task_init(&turntable_task, turntable, 0, 1024, 0, 0, NULL)) {
|
|
rt_printk("turntable: cannot initialize task\n");
|
|
end(true);
|
|
}
|
|
if (rt_task_init(&drill_task, drill, 0, 1024, 0, 0, NULL)) {
|
|
rt_printk("drill: cannot initialize task\n");
|
|
end(true);
|
|
}
|
|
if (rt_task_init(&output_task, output, 0, 1024, 0, 0, NULL)) {
|
|
rt_printk("output: cannot initialize task\n");
|
|
end(true);
|
|
}
|
|
if (rt_task_init(&tester_task, tester, 0, 1024, 0, 0, NULL)) {
|
|
rt_printk("tester: cannot initialize task\n");
|
|
end(true);
|
|
}
|
|
|
|
if (rt_mbx_init(&turntable_status_mbx, sizeof(int))) {
|
|
end(true);
|
|
}
|
|
if (rt_mbx_init(&tester_status_mbx, sizeof(int))) {
|
|
end(true);
|
|
}
|
|
if (rt_mbx_init(&drill_status_mbx, sizeof(int))) {
|
|
end(true);
|
|
}
|
|
if (rt_mbx_init(&output_status_mbx, sizeof(int))) {
|
|
end(true);
|
|
}
|
|
if (rt_mbx_init(&output_data_mbx, sizeof(int))) {
|
|
end(true);
|
|
}
|
|
if (rt_mbx_init(&drill_data_mbx, sizeof(int))) {
|
|
end(true);
|
|
}
|
|
if (rt_mbx_init(&turntable_data_mbx, sizeof(int))) {
|
|
end(true);
|
|
}
|
|
|
|
rt_task_resume(&turntable_task);
|
|
rt_task_resume(&drill_task);
|
|
rt_task_resume(&output_task);
|
|
rt_task_resume(&tester_task);
|
|
rt_printk("loaded module Bearbeiten2\n");
|
|
return (0);
|
|
}
|
|
|
|
static void __exit
|
|
example_exit(void) {
|
|
end(false);
|
|
rt_printk("module Bearbeiten2 unloaded\n");
|
|
}
|
|
|
|
module_exit(example_exit)
|
|
module_init(example_init)
|