From 9a9bdf5dd29423f3080f66b423c332ff84715294 Mon Sep 17 00:00:00 2001
From: Helge Brueger <noreply@fortiss.org>
Date: Tue, 6 Jun 2017 19:08:36 +0000
Subject: [PATCH] Added untested pwm code and fixed some bugs refs 7890
---
.../trunk/META-INF/MANIFEST.MF | 3 -
.../trunk/lib/inc/PWM.h | 36 ++++++++
.../trunk/lib/inc/libmaestro.h | 12 +++
.../trunk/lib/src/Gamepad.c | 3 +
.../trunk/lib/src/PWM.c | 83 +++++++++++++++++++
.../trunk/lib/src/libmaestro.c | 36 ++++++++
.../trunk/lib/src/main.c | 5 +-
.../executable/PWMActuatorExecutable.java | 14 ++--
.../executable/RaspberryPIExecutable.java | 19 +----
9 files changed, 183 insertions(+), 28 deletions(-)
create mode 100644 org.fortiss.af3.platform.raspberry/trunk/lib/inc/libmaestro.h
create mode 100644 org.fortiss.af3.platform.raspberry/trunk/lib/src/libmaestro.c
diff --git a/org.fortiss.af3.platform.raspberry/trunk/META-INF/MANIFEST.MF b/org.fortiss.af3.platform.raspberry/trunk/META-INF/MANIFEST.MF
index 9faccfe4..0b3651d7 100644
--- a/org.fortiss.af3.platform.raspberry/trunk/META-INF/MANIFEST.MF
+++ b/org.fortiss.af3.platform.raspberry/trunk/META-INF/MANIFEST.MF
@@ -8,9 +8,6 @@ Bundle-Vendor: fortiss GmbH
Bundle-Localization: plugin
Bundle-RequiredExecutionEnvironment: JavaSE-1.8
Export-Package: org.fortiss.af3.platform.raspberry.model,
- org.fortiss.af3.platform.raspberry.model.annotation,
- org.fortiss.af3.platform.raspberry.model.annotation.impl,
- org.fortiss.af3.platform.raspberry.model.annotation.util,
org.fortiss.af3.platform.raspberry.model.gamepad,
org.fortiss.af3.platform.raspberry.model.gamepad.impl,
org.fortiss.af3.platform.raspberry.model.gamepad.util,
diff --git a/org.fortiss.af3.platform.raspberry/trunk/lib/inc/PWM.h b/org.fortiss.af3.platform.raspberry/trunk/lib/inc/PWM.h
index e69de29b..6519009a 100644
--- a/org.fortiss.af3.platform.raspberry/trunk/lib/inc/PWM.h
+++ b/org.fortiss.af3.platform.raspberry/trunk/lib/inc/PWM.h
@@ -0,0 +1,36 @@
+#ifndef __PWM_H
+#define __PWM_H
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <termios.h>
+#include <fcntl.h>
+#include <sys/stat.h>
+
+#include "inc-gen/data_dictionary.h"
+#include "inc/libmaestro.h"
+
+// macros
+#define PWM_ADJUST_VALUE(x) (6000 + 4 * x / 66)
+
+// types
+typedef unsigned char PWM_CHANNEL_T;
+
+// constants
+extern const int PWM_SLEEP_TIME;
+extern const char* PWM_DEVICE_NAME;
+extern const size_t PWM_MAX_NO_CHANNELS;
+
+// worker
+void* pwm_worker(void* pt_args);
+
+// init, term
+void pwm_init(PWM_CHANNEL_T channel);
+void pwm_term(PWM_CHANNEL_T channel);
+
+// writers
+void pwm_write(PWM_CHANNEL_T channel, GEN_TYPE_int value);
+void pwm_set_noval(PWM_CHANNEL_T channel);
+
+#endif // __PWM_H
diff --git a/org.fortiss.af3.platform.raspberry/trunk/lib/inc/libmaestro.h b/org.fortiss.af3.platform.raspberry/trunk/lib/inc/libmaestro.h
new file mode 100644
index 00000000..22f83e05
--- /dev/null
+++ b/org.fortiss.af3.platform.raspberry/trunk/lib/inc/libmaestro.h
@@ -0,0 +1,12 @@
+#ifndef __LIBMAESTRO_H
+#define __LIBMAESTRO_H
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <termios.h>
+
+int maestroRead(int fd, unsigned char channel);
+int maestroWrite(int fd, unsigned char channel, unsigned short target);
+
+#endif // __LIBMAESTRO_H
diff --git a/org.fortiss.af3.platform.raspberry/trunk/lib/src/Gamepad.c b/org.fortiss.af3.platform.raspberry/trunk/lib/src/Gamepad.c
index 59f79911..a97edf9e 100644
--- a/org.fortiss.af3.platform.raspberry/trunk/lib/src/Gamepad.c
+++ b/org.fortiss.af3.platform.raspberry/trunk/lib/src/Gamepad.c
@@ -56,6 +56,8 @@ void* gp_worker(void* pt_args) {
}
}
+ close(fd);
+
return NULL;
}
@@ -67,6 +69,7 @@ void gp_init() {
// important, since all btns are accessing the same method
if(gp_initialized) return;
+ gp_initialized = true;
// TODO initialize...
}
diff --git a/org.fortiss.af3.platform.raspberry/trunk/lib/src/PWM.c b/org.fortiss.af3.platform.raspberry/trunk/lib/src/PWM.c
index e69de29b..ea6ace0f 100644
--- a/org.fortiss.af3.platform.raspberry/trunk/lib/src/PWM.c
+++ b/org.fortiss.af3.platform.raspberry/trunk/lib/src/PWM.c
@@ -0,0 +1,83 @@
+#include "inc/PWM.h"
+
+// constants
+const int PWM_SLEEP_TIME = 50000;
+const char* PWM_DEVICE_NAME = "/dev/ttyACM0";
+const size_t PWM_MAX_NO_CHANNELS = 4;
+
+// worker
+GEN_TYPE_boolean pwm_is_running = true;
+
+GEN_TYPE_int* pwm_channel_values;
+GEN_TYPE_boolean* pwm_channel_enabled;
+
+void pwm_send(int fd) {
+ size_t i;
+
+ for(i = 0; i < PWM_MAX_NO_CHANNELS; i++) {
+ if(pwm_channel_enabled[i]) {
+ maestroWrite(fd, i, pwm_channel_values[i]);
+ }
+ }
+}
+
+void* pwm_worker(void* pt_args) {
+ int fd = open(PWM_DEVICE_NAME, O_RDWR | O_NOCTTY);
+
+ if (fd == -1) {
+ perror("Error while connecting to pwm device");
+ return NULL;
+ }
+
+ struct termios options;
+ tcgetattr(fd, &options);
+ options.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
+ options.c_oflag &= ~(ONLCR | OCRNL);
+ tcsetattr(fd, TCSANOW, &options);
+
+ while(pwm_is_running) {
+ pwm_send(fd);
+ usleep(PWM_SLEEP_TIME);
+ }
+
+ close(fd);
+
+ return NULL;
+}
+
+// init, term
+GEN_TYPE_boolean pwm_initialized = false;
+GEN_TYPE_boolean pwm_terminated = false;
+
+void pwm_init(PWM_CHANNEL_T channel) {
+ // important, since all btns are accessing the same method
+ if(!pwm_initialized) {
+ pwm_channel_values = calloc(sizeof(GEN_TYPE_int), PWM_MAX_NO_CHANNELS);
+ pwm_channel_enabled = calloc(sizeof(GEN_TYPE_boolean), PWM_MAX_NO_CHANNELS);
+
+ pwm_initialized = true;
+ }
+
+ pwm_channel_enabled[channel] = true;
+}
+
+void pwm_term(PWM_CHANNEL_T channel) {
+ // important, since all pwms are accessing the same method
+ if(!pwm_terminated) return;
+
+ free(pwm_channel_values);
+ free(pwm_channel_enabled);
+
+ pwm_is_running = false;
+ pwm_terminated = true;
+}
+
+// writers
+void pwm_write(PWM_CHANNEL_T channel, GEN_TYPE_int value) {
+ pwm_channel_values[channel] = PWM_ADJUST_VALUE(value);
+}
+
+void pwm_set_noval(PWM_CHANNEL_T channel) {
+ // do nothing, not used
+}
+
diff --git a/org.fortiss.af3.platform.raspberry/trunk/lib/src/libmaestro.c b/org.fortiss.af3.platform.raspberry/trunk/lib/src/libmaestro.c
new file mode 100644
index 00000000..356d5692
--- /dev/null
+++ b/org.fortiss.af3.platform.raspberry/trunk/lib/src/libmaestro.c
@@ -0,0 +1,36 @@
+#include "inc/libmaestro.h"
+
+// read from channel given a file descriptor
+int maestroRead(int fd, unsigned char channel) {
+ unsigned char command[] = {0x90, channel};
+
+ if(write(fd, command, sizeof(command)) == -1)
+ {
+ perror("error writing");
+ return -1;
+ }
+
+ unsigned char response[2];
+
+ if(read(fd,response,2) != 2)
+ {
+ perror("Error reading from maestro");
+ return -1;
+ }
+
+ return response[0] + 256*response[1];
+}
+
+// write target to a channel given a file descriptor
+// the units of 'target' are quarter-microseconds
+int maestroWrite(int fd, unsigned char channel, unsigned short target) {
+ unsigned char command[] = {0x84, channel, (target & 0x7F), (target >> 7 & 0x7F)};
+
+ if (write(fd, command, sizeof(command)) == -1)
+ {
+ perror("Error writing to maestro");
+ return -1;
+ }
+
+ return 0;
+}
diff --git a/org.fortiss.af3.platform.raspberry/trunk/lib/src/main.c b/org.fortiss.af3.platform.raspberry/trunk/lib/src/main.c
index 45c91f7e..f927d2eb 100644
--- a/org.fortiss.af3.platform.raspberry/trunk/lib/src/main.c
+++ b/org.fortiss.af3.platform.raspberry/trunk/lib/src/main.c
@@ -8,6 +8,7 @@
#include "inc-gen/system.h"
#include "inc/Gamepad.h"
+#include "inc/PWM.h"
int main(int argc, char* argv[]) {
// start a gamepad thread
@@ -15,8 +16,8 @@ int main(int argc, char* argv[]) {
pthread_create(&pt_gamepad, NULL, gp_worker, NULL);
// start a rover thread
- //pthread_t pt_rover;
- //pthread_create(&pt_rover, NULL, rover_main, NULL);
+ pthread_t pt_pwm;
+ pthread_create(&pt_pwm, NULL, pwm_worker, NULL);
// run endlessly
while(1) {
diff --git a/org.fortiss.af3.platform.raspberry/trunk/src/org/fortiss/af3/platform/raspberry/generator/executable/PWMActuatorExecutable.java b/org.fortiss.af3.platform.raspberry/trunk/src/org/fortiss/af3/platform/raspberry/generator/executable/PWMActuatorExecutable.java
index 6ae59325..34c1cb46 100644
--- a/org.fortiss.af3.platform.raspberry/trunk/src/org/fortiss/af3/platform/raspberry/generator/executable/PWMActuatorExecutable.java
+++ b/org.fortiss.af3.platform.raspberry/trunk/src/org/fortiss/af3/platform/raspberry/generator/executable/PWMActuatorExecutable.java
@@ -17,10 +17,12 @@ $Id: codetemplates.xml 1 2011-01-01 00:00:01Z hoelzl $
+--------------------------------------------------------------------------*/
package org.fortiss.af3.platform.raspberry.generator.executable;
+import static org.fortiss.af3.expression.utils.ExpressionModelElementFactory.funcCall;
+import static org.fortiss.af3.expression.utils.ExpressionModelElementFactory.intConst;
+
import org.fortiss.af3.component.model.OutputPort;
import org.fortiss.af3.expression.model.terms.IExpressionTerm;
import org.fortiss.af3.platform.generic.generator.executable.GenericTransmitterExecutable;
-import org.fortiss.af3.platform.model.generic.GenericReceiver;
import org.fortiss.af3.platform.model.generic.GenericTransmitter;
import org.fortiss.af3.platform.raspberry.model.ActuatorPWM;
import org.fortiss.af3.platform.raspberry.model.annotation.Channel;
@@ -44,28 +46,28 @@ public class PWMActuatorExecutable extends GenericTransmitterExecutable {
/** {@inheritDoc} */
@Override
public IExpressionTerm getInitialization() {
- return super.getInitialization();
+ return funcCall("pwm_init", intConst(getPWMChannelFromModelElement(modelElement)));
}
/** {@inheritDoc} */
@Override
public IExpressionTerm getNoValWriteAccessor(OutputPort logicalSignal) {
- return super.getNoValWriteAccessor(logicalSignal);
+ return funcCall("pwm_set_noval", intConst(getPWMChannelFromModelElement(modelElement)));
}
/** {@inheritDoc} */
@Override
public IExpressionTerm getTermination() {
- return super.getTermination();
+ return funcCall("pwm_term", intConst(getPWMChannelFromModelElement(modelElement)));
}
/** {@inheritDoc} */
@Override
public IExpressionTerm getValueWriteAccessor(OutputPort logicalSignal, IExpressionTerm value) {
- return super.getValueWriteAccessor(logicalSignal, value);
+ return funcCall("pwm_write", intConst(getPWMChannelFromModelElement(modelElement)), value);
}
- private int getPWMChannelFromModelElement(GenericReceiver modelElement) {
+ private int getPWMChannelFromModelElement(GenericTransmitter modelElement) {
return AnnotationUtils.getAnnotation(modelElement, Channel.class).getChannel();
}
}
diff --git a/org.fortiss.af3.platform.raspberry/trunk/src/org/fortiss/af3/platform/raspberry/generator/executable/RaspberryPIExecutable.java b/org.fortiss.af3.platform.raspberry/trunk/src/org/fortiss/af3/platform/raspberry/generator/executable/RaspberryPIExecutable.java
index 6bd049f7..caeeca62 100644
--- a/org.fortiss.af3.platform.raspberry/trunk/src/org/fortiss/af3/platform/raspberry/generator/executable/RaspberryPIExecutable.java
+++ b/org.fortiss.af3.platform.raspberry/trunk/src/org/fortiss/af3/platform/raspberry/generator/executable/RaspberryPIExecutable.java
@@ -79,21 +79,8 @@ public class RaspberryPIExecutable extends GenericExecutionUnitExecutable {
super.createExecutionUnitSourcePackage(name, deployedComponents, deployedPorts,
context);
- // TODO: doesn't work since not recognized by configure script
-
- // add src-lib/inc-lib sub packages (= folders in deployment)
- // CSourcePackage sub =
- // AF3GeneratorCommonLanguagesCFactory.eINSTANCE.createCSourcePackage();
- // sub.setBaseLocation(SRC_LIB_SUB_PACKAGE_NAME);
- // pkg.getSubPackages().add(sub);
-
- // sub = AF3GeneratorCommonLanguagesCFactory.eINSTANCE.createCSourcePackage();
- // sub.setBaseLocation(INC_LIB_SUB_PACKAGE_NAME);
- // pkg.getSubPackages().add(sub);
-
// get system.h to add references to self-defined inc files
SourceUnit system = pkg.getSrcGenPackage().findSourceUnitByName("system.c");
- // pkg.getIncGenPackage().findSourceUnitByName("system.h");
// Add source/header files here to be copied into the deployment directory
try {
@@ -116,13 +103,11 @@ public class RaspberryPIExecutable extends GenericExecutionUnitExecutable {
/** LIBRARIES **/
- // TODO: will fail during a build since not defined in Makefile
-
// add UART lib
- // addFile(pkg, "inc-lib/uart.h");
- // addFile(pkg, "src-lib/uart.c");
addFile(pkg, "inc/libuart.h");
addFile(pkg, "src/libuart.c");
+ addFile(pkg, "inc/libmaestro.h");
+ addFile(pkg, "src/libmaestro.c");
/** FIXES AND PATCHES **/
--
GitLab