first commit

CMakeLists.txt

@@ -0,0 +1,76 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(modechanger)
+set(CMAKE_VERBOSE_MAKEFILE ON)
+
+## Make top-level project name available in all nested projects.
+set(MW_TOP_LEVEL_PROJECT_NAME ${PROJECT_NAME})
+
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+geometry_msgs
+roscpp
+std_msgs
+)
+
+add_definitions(
+-DMODEL=modeChanger -DNUMST=1 -DNCSTATES=0 -DHAVESTDIO -DMODEL_HAS_DYNAMICALLY_LOADED_SFCNS=0 -DCLASSIC_INTERFACE=0 -DALLOCATIONFCN=0 -DTID01EQ=0 -DONESTEPFCN=1 -DTERMFCN=1 -DMULTI_INSTANCE_CODE=0 -DINTEGER_CODE=0 -DMT=0 -DROS_PROJECT -DMW_SCHED_OTHER -DSTACK_SIZE=64 -D__MW_TARGET_USE_HARDWARE_RESOURCES_H__ -DRT=RT )
+
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+   CATKIN_DEPENDS geometry_msgs roscpp std_msgs 
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(
+  include
+  ${PROJECT_SOURCE_DIR}
+  ${Boost_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Get list of .c files in project source directory
+file(GLOB ${PROJECT_NAME}_C_SOURCES RELATIVE ${PROJECT_SOURCE_DIR} ${PROJECT_SOURCE_DIR}/*.c)
+## Get list of .cpp files in project source directory
+file(GLOB ${PROJECT_NAME}_CPP_SOURCES RELATIVE ${PROJECT_SOURCE_DIR} ${PROJECT_SOURCE_DIR}/*.cpp)
+
+## Declare executable
+add_executable(${PROJECT_NAME}_node
+   ert_main.cpp
+   modeChanger.cpp
+   modeChanger_data.cpp
+   linuxinitialize.cpp
+   slros_busmsg_conversion.cpp
+   slros_initialize.cpp
+   slros_generic_param.cpp
+   ${${PROJECT_NAME}_C_SOURCES}
+   ${${PROJECT_NAME}_CPP_SOURCES}
+)
+
+## Add cmake target dependencies of the executable/library
+## as an example, message headers may need to be generated before nodes
+add_dependencies(${PROJECT_NAME}_node ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}_node
+   ${catkin_LIBRARIES}
+   ${Boost_LIBRARIES}
+   rt
+)
+
+set_target_properties(${PROJECT_NAME}_node PROPERTIES COMPILE_FLAGS " ")
+SET( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fpermissive" )
+SET( CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -include stdbool.h" )

MW_custom_RTOS_header.h

@@ -0,0 +1,25 @@
+#ifndef _MW_CUSTOM_RTOS_HEADER_H_
+#define _MW_CUSTOM_RTOS_HEADER_H_
+#define MW_BASERATE_PRIORITY           40
+#define MW_BASERATE_PERIOD             0.01
+#define MW_NUMBER_SUBRATES             0
+#define MW_NUMBER_APERIODIC_TASKS      0
+#define MW_IS_CONCURRENT               0
+#define MW_NUMBER_TIMER_DRIVEN_TASKS   0
+
+extern void exitFcn(int sig);
+extern void *terminateTask(void *arg);
+extern void *baseRateTask(void *arg);
+extern void *subrateTask(void *arg);
+extern pthread_t schedulerThread;
+extern pthread_t baseRateThread;
+extern pthread_t subRateThread[];
+extern sem_t stopSem;
+extern sem_t baserateTaskSem;
+extern sem_t subrateTaskSem[];
+extern int taskId[];
+extern int subratePriority[];
+
+#endif
+
+#define MW_MAX_TASKNAME                16

ert_main.cpp

@@ -0,0 +1,94 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include "modeChanger.h"
+#include "modeChanger_private.h"
+#include "rtwtypes.h"
+#include "limits.h"
+#include "linuxinitialize.h"
+#define UNUSED(x)                      x = x
+#define NAMELEN                        16
+
+/* Function prototype declaration*/
+void exitFcn(int sig);
+void *terminateTask(void *arg);
+void *baseRateTask(void *arg);
+void *subrateTask(void *arg);
+volatile boolean_T stopRequested = false;
+volatile boolean_T runModel = true;
+sem_t stopSem;
+sem_t baserateTaskSem;
+pthread_t schedulerThread;
+pthread_t baseRateThread;
+void *threadJoinStatus;
+int terminatingmodel = 0;
+void *baseRateTask(void *arg)
+{
+  runModel = (rtmGetErrorStatus(modeChanger_M) == (NULL));
+  while (runModel) {
+    sem_wait(&baserateTaskSem);
+    modeChanger_step();
+
+    /* Get model outputs here */
+    stopRequested = !((rtmGetErrorStatus(modeChanger_M) == (NULL)));
+    runModel = !stopRequested;
+  }
+
+  runModel = 0;
+  terminateTask(arg);
+  pthread_exit((void *)0);
+  return NULL;
+}
+
+void exitFcn(int sig)
+{
+  UNUSED(sig);
+  rtmSetErrorStatus(modeChanger_M, "stopping the model");
+}
+
+void *terminateTask(void *arg)
+{
+  UNUSED(arg);
+  terminatingmodel = 1;
+
+  {
+    runModel = 0;
+  }
+
+  /* Disable rt_OneStep() here */
+
+  /* Terminate model */
+  modeChanger_terminate();
+  sem_post(&stopSem);
+  return NULL;
+}
+
+int main(int argc, char **argv)
+{
+  UNUSED(argc);
+  UNUSED(argv);
+  void slros_node_init(int argc, char** argv);
+  slros_node_init(argc, argv);
+  rtmSetErrorStatus(modeChanger_M, 0);
+
+  /* Initialize model */
+  modeChanger_initialize();
+
+  /* Call RTOS Initialization function */
+  myRTOSInit(0.01, 0);
+
+  /* Wait for stop semaphore */
+  sem_wait(&stopSem);
+
+#if (MW_NUMBER_TIMER_DRIVEN_TASKS > 0)
+
+  {
+    int i;
+    for (i=0; i < MW_NUMBER_TIMER_DRIVEN_TASKS; i++) {
+      CHECK_STATUS(sem_destroy(&timerTaskSem[i]), 0, "sem_destroy");
+    }
+  }
+
+#endif
+
+  return 0;
+}