From 5ac073640c1dd958c160e6f69521c3901a0ffcd4 Mon Sep 17 00:00:00 2001 From: Anders Blomdell <anders.blomdell@control.lth.se> Date: Tue, 5 Mar 2019 10:55:20 +0100 Subject: [PATCH] Added simulink support --- Makefile | 9 +- adaptors/matlab/Makefile | 20 +++ adaptors/matlab/analogin.c | 170 ++++++++++++++++++++++++++ adaptors/matlab/analogout.c | 171 ++++++++++++++++++++++++++ adaptors/matlab/digitalin.c | 173 ++++++++++++++++++++++++++ adaptors/matlab/digitalout.c | 171 ++++++++++++++++++++++++++ adaptors/matlab/encoderin.c | 172 ++++++++++++++++++++++++++ adaptors/matlab/moberg4simulink.c | 194 ++++++++++++++++++++++++++++++ adaptors/matlab/moberg4simulink.h | 31 +++++ moberg.c | 110 ++++++++++++++++- moberg.h | 32 +++++ plugins/comedi/Makefile | 3 +- plugins/serial2002/Makefile | 2 +- test/Makefile | 9 +- 14 files changed, 1255 insertions(+), 12 deletions(-) create mode 100644 adaptors/matlab/Makefile create mode 100644 adaptors/matlab/analogin.c create mode 100644 adaptors/matlab/analogout.c create mode 100644 adaptors/matlab/digitalin.c create mode 100644 adaptors/matlab/digitalout.c create mode 100644 adaptors/matlab/encoderin.c create mode 100644 adaptors/matlab/moberg4simulink.c create mode 100644 adaptors/matlab/moberg4simulink.h diff --git a/Makefile b/Makefile index 1b8a069..32b5f3c 100644 --- a/Makefile +++ b/Makefile @@ -2,11 +2,12 @@ LIBRARIES=libmoberg.so CCFLAGS+=-Wall -Werror -I$(shell pwd) -g LDFLAGS+=-L$(shell pwd)/build/ -lmoberg PLUGINS:=$(wildcard plugins/*) +ADAPTORS:=$(wildcard adaptors/*) export CCFLAGS LDFLAGS LDFLAGS_parse_config=-ldl #-export-dynamic -all: $(LIBRARIES:%=build/%) $(PLUGINS) +all: $(LIBRARIES:%=build/%) $(PLUGINS) $(ADAPTORS) echo $(PLUGINS) echo $(CCFLAGS) @@ -27,8 +28,8 @@ build/lib/%.o: %.c Makefile | build/lib $(CC) $(CCFLAGS) -c -fPIC -o $@ $< -.PHONY: $(PLUGINS) -$(PLUGINS): +.PHONY: $(PLUGINS) $(ADAPTORS) +$(ADAPTORS) $(PLUGINS): $(MAKE) -C $@ @@ -37,7 +38,7 @@ test: all $(MAKE) -C test test clean: - find build -type f -delete + rm -f build/*.so build/*.mex* rm -f *~ make -C test clean diff --git a/adaptors/matlab/Makefile b/adaptors/matlab/Makefile new file mode 100644 index 0000000..c4e6c3d --- /dev/null +++ b/adaptors/matlab/Makefile @@ -0,0 +1,20 @@ +LIBRARIES=libmoberg4simulink.so +SFUNC=analogin analogout digitalin digitalout encoderin +MATLAB_VERSION= +MEX=MATLAB_VERSION=$(MATLAB_VERSION) mex +MEX_SUFFIX=$(shell $(MEX) -v -n analogin.c \ + | sed -e 's/^.*LDEXTENSION.*[.]\(mex.*\)/\1/p;d') + +CCFLAGS+=-Wall -Werror -I. -I../.. -g + +all: $(LIBRARIES:%=../../build/%) $(SFUNC:%=../../build/%.$(MEX_SUFFIX)) + echo $(SUFFIX) + +../../build/libmoberg4simulink.so: moberg4simulink.c Makefile + $(CC) -o $@ $(CCFLAGS) -L../../build -shared -fPIC -lmoberg $< + +../../build/libmoberg4simulink.so: ../../moberg.h + +../../build/%.$(MEX_SUFFIX): %.c Makefile + $(MEX) CFLAGS="$(CCFLAGS) -fPIC" \ + -outdir ../../build -L../../build -lmoberg4simulink $< diff --git a/adaptors/matlab/analogin.c b/adaptors/matlab/analogin.c new file mode 100644 index 0000000..6c8d6c5 --- /dev/null +++ b/adaptors/matlab/analogin.c @@ -0,0 +1,170 @@ +/* + analogin.c, + a MEX file for reading analog input via Moberg + + Copyright (C) 2019 Anders Blomdell <anders.blomdell@control.lth.se> + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see <http://www.gnu.org/licenses/>. + +*/ + +#define S_FUNCTION_LEVEL 2 +#define S_FUNCTION_NAME analogin + +#include <uchar.h> /* for CHAR16_T typedef in tmwtypes.h */ +#include "simstruc.h" +#include <moberg4simulink.h> + +/* + Usage of work vectors: + + PWork: 0 moberg_analog_in pointer[0] + 1 moberg_analog_in pointer[1] + ... + + */ + +#define MDL_CHECK_PARAMETERS +static void mdlCheckParameters(SimStruct *S) +{ + /* 1st parameter: sampling interval */ + { + if (!mxIsDouble(ssGetSFcnParam(S,0)) || + mxGetNumberOfElements(ssGetSFcnParam(S,0)) != 1) { + ssSetErrorStatus(S, "sampling time must be a scalar"); + return; + } + } + + /* 2nd parameter: input channels */ + { + int number_of_dims = mxGetNumberOfDimensions(ssGetSFcnParam(S,1)); + + if (!mxIsDouble(ssGetSFcnParam(S,1)) || + number_of_dims != 2 || mxGetM(ssGetSFcnParam(S,1)) != 1) { + ssSetErrorStatus(S, "input channels must be a scalar or a vector"); + return; + } + } +} + +static void mdlInitializeSizes(SimStruct *S) +{ + int channelCount; + + ssSetNumSFcnParams(S, 2); + if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) { + mdlCheckParameters(S); + if (ssGetErrorStatus(S) != NULL) { return; } + } else { + return; + } + + channelCount = mxGetN(ssGetSFcnParam(S,1)); + + ssSetNumContStates(S, 0); + ssSetNumDiscStates(S, 0); + + if (!ssSetNumInputPorts(S, 1)) { return; } + ssSetInputPortWidth(S, 0, 1); + ssSetInputPortDirectFeedThrough(S, 0, 1); + + if (!ssSetNumOutputPorts(S, 2)) { return; } + ssSetOutputPortWidth(S, 0, 1); + ssSetOutputPortWidth(S, 1, channelCount); + + ssSetNumSampleTimes(S, 1); + ssSetNumPWork(S, channelCount); /* 0: moberg_analog_in pointer[0] + 1: moberg_analog_in pointer[1] + ... + */ + ssSetNumModes(S, 0); + ssSetNumNonsampledZCs(S, 0); + + ssSetOptions(S, 0); +} + +static void mdlInitializeSampleTimes(SimStruct *S) +{ + ssSetSampleTime(S, 0, mxGetScalar(ssGetSFcnParam(S, 0))); + ssSetOffsetTime(S, 0, 0.0); +} + +#define MDL_INITIALIZE_CONDITIONS +static void mdlInitializeConditions(SimStruct *S) +{ + void **pwork = ssGetPWork(S); + double *channel = mxGetPr(ssGetSFcnParam(S,1)); + int channel_count = mxGetN(ssGetSFcnParam(S,1)); + int i; + + for (i = 0 ; i < channel_count ; i++) { + pwork[i] = moberg4simulink_analog_in_open(channel[i]); + if (! pwork[i]) { + static char error[256]; + sprintf(error, "Failed to open analogin #%d", (int)channel[i]); + ssSetErrorStatus(S, error); + } + } +} + +static void mdlOutputs(SimStruct *S, int_T tid) +{ + void **pwork = ssGetPWork(S); + + { + /* Propagate the dummy sorting signal */ + InputRealPtrsType up = ssGetInputPortRealSignalPtrs(S,0); + real_T *y = ssGetOutputPortRealSignal(S, 0); + y[0] = *up[0]+1; + } + { + int i; + real_T *y = ssGetOutputPortRealSignal(S, 1); + + for (i = 0 ; i < ssGetNumPWork(S) ; i++) { + struct moberg_analog_in *ain = (struct moberg_analog_in*)pwork[i]; + ain->read(ain->context, &y[i]); + } + } +} + +static void mdlTerminate(SimStruct *S) +{ + int i; + void **pwork = ssGetPWork(S); + double *channel = mxGetPr(ssGetSFcnParam(S,1)); + int channel_count = mxGetN(ssGetSFcnParam(S,1)); + + for (i = 0 ; i < channel_count ; i++) { + if (pwork[i]) { + struct moberg_analog_in *ain = (struct moberg_analog_in*)pwork[i]; + moberg4simulink_analog_in_close(channel[i], ain); + } + } +} + +/*======================================================* + * See sfuntmpl.doc for the optional S-function methods * + *======================================================*/ + +/*=============================* + * Required S-function trailer * + *=============================*/ + +#ifdef MATLAB_MEX_FILE /* Is this file being compiled as a MEX-file? */ +#include "simulink.c" /* MEX-file interface mechanism */ +#else +#include "cg_sfun.h" /* Code generation registration function */ +#endif diff --git a/adaptors/matlab/analogout.c b/adaptors/matlab/analogout.c new file mode 100644 index 0000000..ba1b091 --- /dev/null +++ b/adaptors/matlab/analogout.c @@ -0,0 +1,171 @@ +/* + analogout.c, + a MEX file for writing analog output via Moberg. + + Copyright (C) 2019 Anders Blomdell <anders.blomdell@control.lth.se> + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see <http://www.gnu.org/licenses/>. + +*/ + +#define S_FUNCTION_LEVEL 2 +#define S_FUNCTION_NAME analogout + +#include <uchar.h> /* for CHAR16_T typedef in tmwtypes.h */ +#include "simstruc.h" +#include <moberg4simulink.h> + +/* + Usage of work vectors: + + PWork: 0 moberg_analog_out pointer[0] + 1 moberg_analog_out pointer[1] + ... + + */ + +#define MDL_CHECK_PARAMETERS +static void mdlCheckParameters(SimStruct *S) +{ + /* 1st parameter: sampling interval */ + { + if (!mxIsDouble(ssGetSFcnParam(S,0)) || + mxGetNumberOfElements(ssGetSFcnParam(S,0)) != 1) { + ssSetErrorStatus(S, "sampling time must be a scalar"); + return; + } + } + + /* 2nd parameter: input channels */ + { + int number_of_dims = mxGetNumberOfDimensions(ssGetSFcnParam(S,1)); + + if (!mxIsDouble(ssGetSFcnParam(S,1)) || + number_of_dims != 2 || mxGetM(ssGetSFcnParam(S,1)) != 1) { + ssSetErrorStatus(S, "output channels must be a scalar or a vector"); + return; + } + } +} + +static void mdlInitializeSizes(SimStruct *S) +{ + int channelCount; + + ssSetNumSFcnParams(S, 2); + if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) { + mdlCheckParameters(S); + if (ssGetErrorStatus(S) != NULL) { return; } + } else { + return; + } + + channelCount = mxGetN(ssGetSFcnParam(S,1)); + + ssSetNumContStates(S, 0); + ssSetNumDiscStates(S, 0); + + if (!ssSetNumInputPorts(S, 2)) return; + ssSetInputPortWidth(S, 0, 1); + ssSetInputPortDirectFeedThrough(S, 0, 1); + ssSetInputPortWidth(S, 1, channelCount); + ssSetInputPortDirectFeedThrough(S, 1, 1); + + if (!ssSetNumOutputPorts(S, 1)) return; + ssSetOutputPortWidth(S, 0, 1); + + ssSetNumSampleTimes(S, 1); + ssSetNumPWork(S, channelCount); /* 0: moberg_analog_out pointer[0] + 1: moberg_analog_out pointer[1] + ... + */ + ssSetNumModes(S, 0); + ssSetNumNonsampledZCs(S, 0); + + ssSetOptions(S, 0); +} + +static void mdlInitializeSampleTimes(SimStruct *S) +{ + ssSetSampleTime(S, 0, mxGetScalar(ssGetSFcnParam(S, 0))); + ssSetOffsetTime(S, 0, 0.0); +} + +#define MDL_INITIALIZE_CONDITIONS +static void mdlInitializeConditions(SimStruct *S) +{ + void **pwork = ssGetPWork(S); + double *channel = mxGetPr(ssGetSFcnParam(S,1)); + int channel_count = mxGetN(ssGetSFcnParam(S,1)); + int i; + + for (i = 0 ; i < channel_count ; i++) { + pwork[i] = moberg4simulink_analog_out_open(channel[i]); + if (! pwork[i]) { + static char error[256]; + sprintf(error, "Failed to open analogout #%d", (int)channel[i]); + ssSetErrorStatus(S, error); + } + } +} + +static void mdlOutputs(SimStruct *S, int_T tid) +{ + void **pwork = ssGetPWork(S); + + { + /* Propagate the dummy sorting signal */ + InputRealPtrsType up = ssGetInputPortRealSignalPtrs(S,0); + real_T *y = ssGetOutputPortRealSignal(S, 0); + y[0] = *up[0]+1; + } + { + InputRealPtrsType up = ssGetInputPortRealSignalPtrs(S,1); + int i; + + for (i = 0 ; i < ssGetNumPWork(S) ; i++) { + struct moberg_analog_out *aout = (struct moberg_analog_out*)pwork[i]; + aout->write(aout->context, *up[i]); + } + } +} + +static void mdlTerminate(SimStruct *S) +{ + int i; + void **pwork = ssGetPWork(S); + double *channel = mxGetPr(ssGetSFcnParam(S,1)); + int channel_count = mxGetN(ssGetSFcnParam(S,1)); + + for (i = 0 ; i < channel_count ; i++) { + if (pwork[i]) { + struct moberg_analog_out *aout = (struct moberg_analog_out*)pwork[i]; + moberg4simulink_analog_out_close(channel[i], aout); + } + } +} + +/*======================================================* + * See sfuntmpl.doc for the optional S-function methods * + *======================================================*/ + +/*=============================* + * Required S-function trailer * + *=============================*/ + +#ifdef MATLAB_MEX_FILE /* Is this file being compiled as a MEX-file? */ +#include "simulink.c" /* MEX-file interface mechanism */ +#else +#include "cg_sfun.h" /* Code generation registration function */ +#endif diff --git a/adaptors/matlab/digitalin.c b/adaptors/matlab/digitalin.c new file mode 100644 index 0000000..55674db --- /dev/null +++ b/adaptors/matlab/digitalin.c @@ -0,0 +1,173 @@ +/* + digitalin.c, + a MEX file for reading digital input via Moberg + + Copyright (C) 2019 Anders Blomdell <anders.blomdell@control.lth.se> + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see <http://www.gnu.org/licenses/>. + +*/ + +#define S_FUNCTION_LEVEL 2 +#define S_FUNCTION_NAME digitalin + +#include <uchar.h> /* for CHAR16_T typedef in tmwtypes.h */ +#include "simstruc.h" +#include <moberg4simulink.h> + +/* + Usage of work vectors: + + PWork: 0 moberg_digital_in pointer[0] + 1 moberg_digital_in pointer[1] + ... + + + */ + +#define MDL_CHECK_PARAMETERS +static void mdlCheckParameters(SimStruct *S) +{ + /* 1st parameter: sampling interval */ + { + if (!mxIsDouble(ssGetSFcnParam(S,0)) || + mxGetNumberOfElements(ssGetSFcnParam(S,0)) != 1) { + ssSetErrorStatus(S, "sampling time must be a scalar"); + return; + } + } + + /* 2nd parameter: input channels */ + { + int number_of_dims = mxGetNumberOfDimensions(ssGetSFcnParam(S,1)); + + if (!mxIsDouble(ssGetSFcnParam(S,1)) || + number_of_dims != 2 || mxGetM(ssGetSFcnParam(S,1)) != 1) { + ssSetErrorStatus(S, "input channels must be a scalar or a vector"); + return; + } + } +} + +static void mdlInitializeSizes(SimStruct *S) +{ + int channelCount; + + ssSetNumSFcnParams(S, 2); + if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) { + mdlCheckParameters(S); + if (ssGetErrorStatus(S) != NULL) { return; } + } else { + return; + } + + channelCount = mxGetN(ssGetSFcnParam(S,1)); + + ssSetNumContStates(S, 0); + ssSetNumDiscStates(S, 0); + + if (!ssSetNumInputPorts(S, 1)) { return; } + ssSetInputPortWidth(S, 0, 1); + ssSetInputPortDirectFeedThrough(S, 0, 1); + + if (!ssSetNumOutputPorts(S, 2)) { return; } + ssSetOutputPortWidth(S, 0, 1); + ssSetOutputPortWidth(S, 1, channelCount); + + ssSetNumSampleTimes(S, 1); + ssSetNumPWork(S, channelCount); /* 0: moberg_digital_in pointer[0] + 1: moberg_digital_in pointer[1] + ... + */ + ssSetNumModes(S, 0); + ssSetNumNonsampledZCs(S, 0); + + ssSetOptions(S, 0); +} + +static void mdlInitializeSampleTimes(SimStruct *S) +{ + ssSetSampleTime(S, 0, mxGetScalar(ssGetSFcnParam(S, 0))); + ssSetOffsetTime(S, 0, 0.0); +} + +#define MDL_INITIALIZE_CONDITIONS +static void mdlInitializeConditions(SimStruct *S) +{ + void **pwork = ssGetPWork(S); + double *channel = mxGetPr(ssGetSFcnParam(S,1)); + int channel_count = mxGetN(ssGetSFcnParam(S,1)); + int i; + + for (i = 0 ; i < channel_count ; i++) { + pwork[i] = moberg4simulink_digital_in_open(channel[i]); + if (! pwork[i]) { + static char error[256]; + sprintf(error, "Failed to open digitalin #%d", (int)channel[i]); + ssSetErrorStatus(S, error); + } + } +} + +static void mdlOutputs(SimStruct *S, int_T tid) +{ + void **pwork = ssGetPWork(S); + + { + /* Propagate the dummy sorting signal */ + InputRealPtrsType up = ssGetInputPortRealSignalPtrs(S,0); + real_T *y = ssGetOutputPortRealSignal(S, 0); + y[0] = *up[0]+1; + } + { + int i; + real_T *y = ssGetOutputPortRealSignal(S, 1); + + for (i = 0 ; i < ssGetNumPWork(S) ; i++) { + struct moberg_digital_in *din = (struct moberg_digital_in*)pwork[i]; + int value; + din->read(din->context, &value); + y[i] = value; + } + } +} + +static void mdlTerminate(SimStruct *S) +{ + int i; + void **pwork = ssGetPWork(S); + double *channel = mxGetPr(ssGetSFcnParam(S,1)); + int channel_count = mxGetN(ssGetSFcnParam(S,1)); + + for (i = 0 ; i < channel_count ; i++) { + if (pwork[i]) { + struct moberg_digital_in *ain = (struct moberg_digital_in*)pwork[i]; + moberg4simulink_digital_in_close(channel[i], ain); + } + } +} + +/*======================================================* + * See sfuntmpl.doc for the optional S-function methods * + *======================================================*/ + +/*=============================* + * Required S-function trailer * + *=============================*/ + +#ifdef MATLAB_MEX_FILE /* Is this file being compiled as a MEX-file? */ +#include "simulink.c" /* MEX-file interface mechanism */ +#else +#include "cg_sfun.h" /* Code generation registration function */ +#endif diff --git a/adaptors/matlab/digitalout.c b/adaptors/matlab/digitalout.c new file mode 100644 index 0000000..801573b --- /dev/null +++ b/adaptors/matlab/digitalout.c @@ -0,0 +1,171 @@ +/* + digitalout.c, + a MEX file for writing digital output via Moberg. + + Copyright (C) 2019 Anders Blomdell <anders.blomdell@control.lth.se> + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see <http://www.gnu.org/licenses/>. + +*/ + +#define S_FUNCTION_LEVEL 2 +#define S_FUNCTION_NAME digitalout + +#include <uchar.h> /* for CHAR16_T typedef in tmwtypes.h */ +#include "simstruc.h" +#include <moberg4simulink.h> + +/* + Usage of work vectors: + + PWork: 0 moberg_digital_out pointer[0] + 1 moberg_digital_out pointer[1] + ... + + */ + +#define MDL_CHECK_PARAMETERS +static void mdlCheckParameters(SimStruct *S) +{ + /* 1st parameter: sampling interval */ + { + if (!mxIsDouble(ssGetSFcnParam(S,0)) || + mxGetNumberOfElements(ssGetSFcnParam(S,0)) != 1) { + ssSetErrorStatus(S, "sampling time must be a scalar"); + return; + } + } + + /* 2nd parameter: input channels */ + { + int number_of_dims = mxGetNumberOfDimensions(ssGetSFcnParam(S,1)); + + if (!mxIsDouble(ssGetSFcnParam(S,1)) || + number_of_dims != 2 || mxGetM(ssGetSFcnParam(S,1)) != 1) { + ssSetErrorStatus(S, "output channels must be a scalar or a vector"); + return; + } + } +} + +static void mdlInitializeSizes(SimStruct *S) +{ + int channelCount; + + ssSetNumSFcnParams(S, 2); + if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) { + mdlCheckParameters(S); + if (ssGetErrorStatus(S) != NULL) { return; } + } else { + return; + } + + channelCount = mxGetN(ssGetSFcnParam(S,1)); + + ssSetNumContStates(S, 0); + ssSetNumDiscStates(S, 0); + + if (!ssSetNumInputPorts(S, 2)) return; + ssSetInputPortWidth(S, 0, 1); + ssSetInputPortDirectFeedThrough(S, 0, 1); + ssSetInputPortWidth(S, 1, channelCount); + ssSetInputPortDirectFeedThrough(S, 1, 1); + + if (!ssSetNumOutputPorts(S, 1)) return; + ssSetOutputPortWidth(S, 0, 1); + + ssSetNumSampleTimes(S, 1); + ssSetNumPWork(S, channelCount); /* 0: moberg_digital_out pointer[0] + 1: moberg_digital_out pointer[1] + ... + */ + ssSetNumModes(S, 0); + ssSetNumNonsampledZCs(S, 0); + + ssSetOptions(S, 0); +} + +static void mdlInitializeSampleTimes(SimStruct *S) +{ + ssSetSampleTime(S, 0, mxGetScalar(ssGetSFcnParam(S, 0))); + ssSetOffsetTime(S, 0, 0.0); +} + +#define MDL_INITIALIZE_CONDITIONS +static void mdlInitializeConditions(SimStruct *S) +{ + void **pwork = ssGetPWork(S); + double *channel = mxGetPr(ssGetSFcnParam(S,1)); + int channel_count = mxGetN(ssGetSFcnParam(S,1)); + int i; + + for (i = 0 ; i < channel_count ; i++) { + pwork[i] = moberg4simulink_digital_out_open(channel[i]); + if (! pwork[i]) { + static char error[256]; + sprintf(error, "Failed to open digitalout #%d", (int)channel[i]); + ssSetErrorStatus(S, error); + } + } +} + +static void mdlOutputs(SimStruct *S, int_T tid) +{ + void **pwork = ssGetPWork(S); + + { + /* Propagate the dummy sorting signal */ + InputRealPtrsType up = ssGetInputPortRealSignalPtrs(S,0); + real_T *y = ssGetOutputPortRealSignal(S, 0); + y[0] = *up[0]+1; + } + { + InputRealPtrsType up = ssGetInputPortRealSignalPtrs(S,1); + int i; + + for (i = 0 ; i < ssGetNumPWork(S) ; i++) { + struct moberg_digital_out *dout = (struct moberg_digital_out*)pwork[i]; + dout->write(dout->context, *up[i]); + } + } +} + +static void mdlTerminate(SimStruct *S) +{ + int i; + void **pwork = ssGetPWork(S); + double *channel = mxGetPr(ssGetSFcnParam(S,1)); + int channel_count = mxGetN(ssGetSFcnParam(S,1)); + + for (i = 0 ; i < channel_count ; i++) { + if (pwork[i]) { + struct moberg_digital_out *dout = (struct moberg_digital_out*)pwork[i]; + moberg4simulink_digital_out_close(channel[i], dout); + } + } +} + +/*======================================================* + * See sfuntmpl.doc for the optional S-function methods * + *======================================================*/ + +/*=============================* + * Required S-function trailer * + *=============================*/ + +#ifdef MATLAB_MEX_FILE /* Is this file being compiled as a MEX-file? */ +#include "simulink.c" /* MEX-file interface mechanism */ +#else +#include "cg_sfun.h" /* Code generation registration function */ +#endif diff --git a/adaptors/matlab/encoderin.c b/adaptors/matlab/encoderin.c new file mode 100644 index 0000000..9a7be54 --- /dev/null +++ b/adaptors/matlab/encoderin.c @@ -0,0 +1,172 @@ +/* + encoderin.c, + a MEX file for reading encoder input via Moberg + + Copyright (C) 2019 Anders Blomdell <anders.blomdell@control.lth.se> + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see <http://www.gnu.org/licenses/>. + +*/ + +#define S_FUNCTION_LEVEL 2 +#define S_FUNCTION_NAME encoderin + +#include <uchar.h> /* for CHAR16_T typedef in tmwtypes.h */ +#include "simstruc.h" +#include <moberg4simulink.h> + +/* + Usage of work vectors: + + PWork: 0 moberg_encoder_in pointer[0] + 1 moberg_encoder_in pointer[1] + ... + + */ + +#define MDL_CHECK_PARAMETERS +static void mdlCheckParameters(SimStruct *S) +{ + /* 1st parameter: sampling interval */ + { + if (!mxIsDouble(ssGetSFcnParam(S,0)) || + mxGetNumberOfElements(ssGetSFcnParam(S,0)) != 1) { + ssSetErrorStatus(S, "sampling time must be a scalar"); + return; + } + } + + /* 2nd parameter: input channels */ + { + int number_of_dims = mxGetNumberOfDimensions(ssGetSFcnParam(S,1)); + + if (!mxIsDouble(ssGetSFcnParam(S,1)) || + number_of_dims != 2 || mxGetM(ssGetSFcnParam(S,1)) != 1) { + ssSetErrorStatus(S, "input channels must be a scalar or a vector"); + return; + } + } +} + +static void mdlInitializeSizes(SimStruct *S) +{ + int channelCount; + + ssSetNumSFcnParams(S, 2); + if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) { + mdlCheckParameters(S); + if (ssGetErrorStatus(S) != NULL) { return; } + } else { + return; + } + + channelCount = mxGetN(ssGetSFcnParam(S,1)); + + ssSetNumContStates(S, 0); + ssSetNumDiscStates(S, 0); + + if (!ssSetNumInputPorts(S, 1)) { return; } + ssSetInputPortWidth(S, 0, 1); + ssSetInputPortDirectFeedThrough(S, 0, 1); + + if (!ssSetNumOutputPorts(S, 2)) { return; } + ssSetOutputPortWidth(S, 0, 1); + ssSetOutputPortWidth(S, 1, channelCount); + + ssSetNumSampleTimes(S, 1); + ssSetNumPWork(S, channelCount); /* 0: moberg_encoder_in pointer[0] + 1: moberg_encoder_in pointer[1] + ... + */ + ssSetNumModes(S, 0); + ssSetNumNonsampledZCs(S, 0); + + ssSetOptions(S, 0); +} + +static void mdlInitializeSampleTimes(SimStruct *S) +{ + ssSetSampleTime(S, 0, mxGetScalar(ssGetSFcnParam(S, 0))); + ssSetOffsetTime(S, 0, 0.0); +} + +#define MDL_INITIALIZE_CONDITIONS +static void mdlInitializeConditions(SimStruct *S) +{ + void **pwork = ssGetPWork(S); + double *channel = mxGetPr(ssGetSFcnParam(S,1)); + int channel_count = mxGetN(ssGetSFcnParam(S,1)); + int i; + + for (i = 0 ; i < channel_count ; i++) { + pwork[i] = moberg4simulink_encoder_in_open(channel[i]); + if (! pwork[i]) { + static char error[256]; + sprintf(error, "Failed to open encoderin #%d", (int)channel[i]); + ssSetErrorStatus(S, error); + } + } +} + +static void mdlOutputs(SimStruct *S, int_T tid) +{ + void **pwork = ssGetPWork(S); + + { + /* Propagate the dummy sorting signal */ + InputRealPtrsType up = ssGetInputPortRealSignalPtrs(S,0); + real_T *y = ssGetOutputPortRealSignal(S, 0); + y[0] = *up[0]+1; + } + { + int i; + real_T *y = ssGetOutputPortRealSignal(S, 1); + + for (i = 0 ; i < ssGetNumPWork(S) ; i++) { + struct moberg_encoder_in *ein = (struct moberg_encoder_in*)pwork[i]; + long value; + ein->read(ein->context, &value); + y[i] = value; + } + } +} + +static void mdlTerminate(SimStruct *S) +{ + int i; + void **pwork = ssGetPWork(S); + double *channel = mxGetPr(ssGetSFcnParam(S,1)); + int channel_count = mxGetN(ssGetSFcnParam(S,1)); + + for (i = 0 ; i < channel_count ; i++) { + if (pwork[i]) { + struct moberg_encoder_in *ein = (struct moberg_encoder_in*)pwork[i]; + moberg4simulink_encoder_in_close(channel[i], ein); + } + } +} + +/*======================================================* + * See sfuntmpl.doc for the optional S-function methods * + *======================================================*/ + +/*=============================* + * Required S-function trailer * + *=============================*/ + +#ifdef MATLAB_MEX_FILE /* Is this file being compiled as a MEX-file? */ +#include "simulink.c" /* MEX-file interface mechanism */ +#else +#include "cg_sfun.h" /* Code generation registration function */ +#endif diff --git a/adaptors/matlab/moberg4simulink.c b/adaptors/matlab/moberg4simulink.c new file mode 100644 index 0000000..736edd0 --- /dev/null +++ b/adaptors/matlab/moberg4simulink.c @@ -0,0 +1,194 @@ +#include <stdio.h> +#include <stdlib.h> +#include <stddef.h> +#include <moberg.h> +#include <moberg4simulink.h> + +static struct channel { + struct channel *next; + struct channel *prev; + union { + struct moberg_analog_in analog_in; + struct moberg_analog_out analog_out; + struct moberg_digital_in digital_in; + struct moberg_digital_out digital_out; + struct moberg_encoder_in encoder_in; + }; +} analog_in_list={.next=&analog_in_list, + .prev=&analog_in_list}, + analog_out_list={.next=&analog_out_list, + .prev=&analog_out_list}, + digital_in_list={.next=&digital_in_list, + .prev=&digital_in_list}, + digital_out_list={.next=&digital_out_list, + .prev=&digital_out_list}, + encoder_in_list={.next=&encoder_in_list, + .prev=&encoder_in_list}; + +struct { + int count; + struct moberg *moberg; +} g_moberg = { 0, NULL }; + +static int up() +{ + if (g_moberg.count <= 0) { + g_moberg.moberg = moberg_new(NULL); + } + g_moberg.count++; + return 0; +} + +static int down() +{ + g_moberg.count--; + if (g_moberg.count <= 0) { + moberg_free(g_moberg.moberg); + g_moberg.moberg = NULL; + } + return 0; +} + +void list_insert(struct channel *list, + struct channel *element) +{ + element->next = list; + element->prev = list->prev; + element->prev->next = element; + element->next->prev = element; +} + +void list_remove(struct channel *element) +{ + element->prev->next = element->next; + element->next->prev = element->prev; +} + +struct moberg_analog_in *moberg4simulink_analog_in_open(int index) +{ + up(); + struct channel *result = malloc(sizeof(*result)); + if (result && moberg_analog_in_open(g_moberg.moberg, index, + &result->analog_in)) { + list_insert(&analog_in_list, result); + return &result->analog_in; + } else { + down(); + return NULL; + } +} + +void moberg4simulink_analog_in_close(int index, + struct moberg_analog_in *analog_in) +{ + struct channel *channel = + (void*)analog_in - offsetof(struct channel, analog_in); + moberg_analog_in_close(g_moberg.moberg, index, channel->analog_in); + list_remove(channel); + free(channel); + down(); +} + +struct moberg_analog_out *moberg4simulink_analog_out_open(int index) + { + up(); + struct channel *result = malloc(sizeof(*result)); + if (result && moberg_analog_out_open(g_moberg.moberg, index, + &result->analog_out)) { + list_insert(&analog_out_list, result); + return &result->analog_out; + } else { + down(); + return NULL; + } +} + +void moberg4simulink_analog_out_close(int index, + struct moberg_analog_out *analog_out) +{ + struct channel *channel = + (void*)analog_out - offsetof(struct channel, analog_out); + moberg_analog_out_close(g_moberg.moberg, index, channel->analog_out); + list_remove(channel); + free(channel); + down(); +} + +struct moberg_digital_in *moberg4simulink_digital_in_open(int index) +{ + up(); + struct channel *result = malloc(sizeof(*result)); + if (result && moberg_digital_in_open(g_moberg.moberg, index, + &result->digital_in)) { + list_insert(&digital_in_list, result); + return &result->digital_in; + } else { + down(); + return NULL; + } +} + + +void moberg4simulink_digital_in_close(int index, + struct moberg_digital_in *digital_in) +{ + struct channel *channel = + (void*)digital_in - offsetof(struct channel, digital_in); + moberg_digital_in_close(g_moberg.moberg, index, channel->digital_in); + list_remove(channel); + free(channel); + down(); +} + +struct moberg_digital_out *moberg4simulink_digital_out_open(int index) +{ + up(); + struct channel *result = malloc(sizeof(*result)); + if (result && moberg_digital_out_open(g_moberg.moberg, index, + &result->digital_out)) { + list_insert(&digital_out_list, result); + return &result->digital_out; + } else { + down(); + return NULL; + } +} + + +void moberg4simulink_digital_out_close(int index, + struct moberg_digital_out *digital_out) +{ + struct channel *channel = + (void*)digital_out - offsetof(struct channel, digital_out); + moberg_digital_out_close(g_moberg.moberg, index, channel->digital_out); + list_remove(channel); + free(channel); + down(); +} + +struct moberg_encoder_in *moberg4simulink_encoder_in_open(int index) +{ + up(); + struct channel *result = malloc(sizeof(*result)); + if (result && moberg_encoder_in_open(g_moberg.moberg, index, + &result->encoder_in)) { + list_insert(&encoder_in_list, result); + return &result->encoder_in; + } else { + down(); + return NULL; + } +} + + +void moberg4simulink_encoder_in_close(int index, + struct moberg_encoder_in *encoder_in) +{ + struct channel *channel = + (void*)encoder_in - offsetof(struct channel, encoder_in); + moberg_encoder_in_close(g_moberg.moberg, index, channel->encoder_in); + list_remove(channel); + free(channel); + down(); +} + diff --git a/adaptors/matlab/moberg4simulink.h b/adaptors/matlab/moberg4simulink.h new file mode 100644 index 0000000..2b26a67 --- /dev/null +++ b/adaptors/matlab/moberg4simulink.h @@ -0,0 +1,31 @@ +#ifndef __MOBERG4SIMULINK_H__ +#define __MOBERG4SIMULINK_H__ + +#include <moberg.h> + +struct moberg_analog_in *moberg4simulink_analog_in_open(int index); + +void moberg4simulink_analog_in_close(int index, + struct moberg_analog_in *analog_in); + +struct moberg_analog_out *moberg4simulink_analog_out_open(int index); + +void moberg4simulink_analog_out_close(int index, + struct moberg_analog_out *analog_out); + +struct moberg_digital_in *moberg4simulink_digital_in_open(int index); + +void moberg4simulink_digital_in_close(int index, + struct moberg_digital_in *digital_in); + +struct moberg_digital_out *moberg4simulink_digital_out_open(int index); + +void moberg4simulink_digital_out_close(int index, + struct moberg_digital_out *digital_out); + +struct moberg_encoder_in *moberg4simulink_encoder_in_open(int index); + +void moberg4simulink_encoder_in_close(int index, + struct moberg_encoder_in *encoder_in); + +#endif diff --git a/moberg.c b/moberg.c index e302fce..a0b6e37 100644 --- a/moberg.c +++ b/moberg.c @@ -212,8 +212,12 @@ static int install_config(struct moberg *moberg) .context=moberg, .channel=install_channel }; - return moberg_config_install_channels(moberg->config, &install); - /* TODO cleanup unused devices...*/ + if (! moberg->config) { + fprintf(stderr, "No moberg configuration found\n"); + return 0; + } else { + return moberg_config_install_channels(moberg->config, &install); + } } struct moberg *moberg_new( @@ -310,7 +314,109 @@ int moberg_analog_in_close(struct moberg *moberg, return 1; } +int moberg_analog_out_open(struct moberg *moberg, + int index, + struct moberg_analog_out *analog_out) +{ + struct moberg_channel *channel = NULL; + channel_list_get(&moberg->analog_out, index, &channel); + if (channel) { + channel->open(channel); + *analog_out = channel->action.analog_out; + return 1; + } + return 0; +} + +int moberg_analog_out_close(struct moberg *moberg, + int index, + struct moberg_analog_out analog_out) +{ + struct moberg_channel *channel = NULL; + channel_list_get(&moberg->analog_out, index, &channel); + if (channel && channel->action.analog_out.context == analog_out.context) { + channel->close(channel); + } + return 1; +} + +int moberg_digital_in_open(struct moberg *moberg, + int index, + struct moberg_digital_in *digital_in) +{ + struct moberg_channel *channel = NULL; + channel_list_get(&moberg->digital_in, index, &channel); + if (channel) { + channel->open(channel); + *digital_in = channel->action.digital_in; + return 1; + } + return 0; +} + +int moberg_digital_in_close(struct moberg *moberg, + int index, + struct moberg_digital_in digital_in) +{ + struct moberg_channel *channel = NULL; + channel_list_get(&moberg->digital_in, index, &channel); + if (channel && channel->action.digital_in.context == digital_in.context) { + channel->close(channel); + } + return 1; +} + +int moberg_digital_out_open(struct moberg *moberg, + int index, + struct moberg_digital_out *digital_out) +{ + struct moberg_channel *channel = NULL; + channel_list_get(&moberg->digital_out, index, &channel); + if (channel) { + channel->open(channel); + *digital_out = channel->action.digital_out; + return 1; + } + return 0; +} + +int moberg_digital_out_close(struct moberg *moberg, + int index, + struct moberg_digital_out digital_out) +{ + struct moberg_channel *channel = NULL; + channel_list_get(&moberg->digital_out, index, &channel); + if (channel && channel->action.digital_out.context == digital_out.context) { + channel->close(channel); + } + return 1; +} + +int moberg_encoder_in_open(struct moberg *moberg, + int index, + struct moberg_encoder_in *encoder_in) +{ + struct moberg_channel *channel = NULL; + channel_list_get(&moberg->encoder_in, index, &channel); + if (channel) { + channel->open(channel); + *encoder_in = channel->action.encoder_in; + return 1; + } + return 0; +} +int moberg_encoder_in_close(struct moberg *moberg, + int index, + struct moberg_encoder_in encoder_in) +{ + struct moberg_channel *channel = NULL; + channel_list_get(&moberg->encoder_in, index, &channel); + if (channel && channel->action.encoder_in.context == encoder_in.context) { + channel->close(channel); + } + return 1; +} /* System init functionality (systemd/init/...) */ diff --git a/moberg.h b/moberg.h index 1ffa2c3..ffe5410 100644 --- a/moberg.h +++ b/moberg.h @@ -47,6 +47,38 @@ int moberg_analog_in_close(struct moberg *moberg, int index, struct moberg_analog_in analog_in); +int moberg_analog_out_open(struct moberg *moberg, + int index, + struct moberg_analog_out *analog_out); + +int moberg_analog_out_close(struct moberg *moberg, + int index, + struct moberg_analog_out analog_out); + +int moberg_digital_in_open(struct moberg *moberg, + int index, + struct moberg_digital_in *digital_in); + +int moberg_digital_in_close(struct moberg *moberg, + int index, + struct moberg_digital_in digital_in); + +int moberg_digital_out_open(struct moberg *moberg, + int index, + struct moberg_digital_out *digital_out); + +int moberg_digital_out_close(struct moberg *moberg, + int index, + struct moberg_digital_out digital_out); + +int moberg_encoder_in_open(struct moberg *moberg, + int index, + struct moberg_encoder_in *encoder_in); + +int moberg_encoder_in_close(struct moberg *moberg, + int index, + struct moberg_encoder_in encoder_in); + /* System init functionality (systemd/init/...) */ int moberg_start( diff --git a/plugins/comedi/Makefile b/plugins/comedi/Makefile index 543d534..5aa40f2 100644 --- a/plugins/comedi/Makefile +++ b/plugins/comedi/Makefile @@ -1,10 +1,9 @@ LIBRARIES=libmoberg_comedi.so CCFLAGS+=-Wall -Werror -I../.. -g -MODULES=$(wildcard modules/*) all: $(LIBRARIES:%=../../build/%) ../../build/libmoberg_comedi.so: comedi.c Makefile - $(CC) -o $@ $(CCFLAGS) -shared -fPIC -lcomedi $< + $(CC) -o $@ $(CCFLAGS) -shared -fPIC -lcomedi -L../../build -lmoberg $< ../../build/libmoberg_comedi.so: ../../moberg_module.h diff --git a/plugins/serial2002/Makefile b/plugins/serial2002/Makefile index e6b34fb..9d13537 100644 --- a/plugins/serial2002/Makefile +++ b/plugins/serial2002/Makefile @@ -6,6 +6,6 @@ MODULES=$(wildcard modules/*) all: $(LIBRARIES:%=../../build/%) ../../build/libmoberg_serial2002.so: serial2002.c Makefile - $(CC) -o $@ $(CCFLAGS) -shared -fPIC $< + $(CC) -o $@ $(CCFLAGS) -shared -fPIC -L../../build -lmoberg $< ../../build/libmoberg_serial2002.so: ../../moberg_module.h diff --git a/test/Makefile b/test/Makefile index 1515606..1c0ca42 100644 --- a/test/Makefile +++ b/test/Makefile @@ -1,6 +1,9 @@ -TEST = test_start_stop test_io - +TEST = test_start_stop test_io test_moberg4simulink +CCFLAGS += -Wall -Werror -I$(shell pwd) -g +LDFLAGS += -L$(shell pwd)/build/ -lmoberg ENV_TEST = LD_LIBRARY_PATH=../build HOME=. +LDFLAGS_test_moberg4simulink = -lmoberg4simulink +CCFLAGS_test_moberg4simulink = -I../adaptors/matlab -Wall -Werror -I$(shell pwd) -g all: @@ -14,7 +17,7 @@ run_%: build/% .PRECIOUS: build/% build/%: %.c | build - $(CC) $(CCFLAGS) $(LDFLAGS) -o $@ $< + $(CC) $(CCFLAGS) $(CCFLAGS_$*) $(LDFLAGS) $(LDFLAGS_$*) -o $@ $< build: mkdir build -- GitLab