Annotated rtmain.c


/*
  COPYRIGHT (C) 2003  Lorenzo Dozio (dozio@aero.polimi.it)
  Paolo Mantegazza (mantegazza@aero.polimi.it)
  Roberto Bucher (roberto.bucher@supsi.ch)
  Daniele Gasperini (daniele.gasperini@elet.polimi.it)

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2 of the License, or (at your option) any later version.

  This library 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
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General Public
  License along with this library; if not, write to the Free Software
  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.

  Portions copyright 2008 Associated Universities Incorporated.

*/

#define _XOPEN_SOURCE   600

#include <float.h>
#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>
#include <signal.h>
#include <string.h>
#include <sys/mman.h>
#include <sys/io.h>
#include <getopt.h>
#include <pthread.h>
#include <math.h>
#include <semaphore.h>

#include "tmwtypes.h"
#include "rtmodel.h"
#include "rt_sim.h"
#include "rt_nonfinite.h"
#include "mdl_info.h"
#include "bio_sig.h"
#include "simstruc.h"

#include <rtai_netrpc.h>
#include <rtai_msg.h>
#include <rtai_mbx.h>
#include <rtai_fifos.h>

#define EXPAND_CONCAT(name1,name2)   name1 ## name2
#define CONCAT(name1,name2)      EXPAND_CONCAT(name1,name2)
#define RT_MODEL         CONCAT(MODEL,_rtModel)

#if !defined(MAIN_NAME)
#define MAIN_NAME main
#endif

#if TID01EQ == 1
#define FIRST_TID 1
#else
#define FIRST_TID 0
#endif
#ifndef RT
# error "must define RT"
#endif
#ifndef MODEL
# error "must define MODEL"
#endif
#ifndef NUMST
# error "must define number of sample times, NUMST"
#endif
#ifndef NCSTATES
# error "must define NCSTATES"
#endif

extern void MdlInitializeSizes(void);
extern void MdlInitializeSampleTimes(void);
extern void MdlStart(void);
extern void MdlOutputs(int_T tid);
extern void MdlUpdate(int_T tid);
extern void MdlTerminate(void);
#define INITIALIZE_SIZES(M)      MdlInitializeSizes()
#define INITIALIZE_SAMPLE_TIMES(M)   MdlInitializeSampleTimes()
#define START(M)         MdlStart()
#define OUTPUTS(M,tid)         MdlOutputs(tid)
#define UPDATED(M,tid)         MdlUpdate(tid)
#define TERMINATE(M)         MdlTerminate()
#if NCSTATES > 0
extern void rt_ODECreateIntegrationData(RTWSolverInfo *si);
extern void rt_ODEUpdateContinuousStates(RTWSolverInfo *si);
#else
#define rt_ODECreateIntegrationData(si)      rtsiSetSolverName(si,"FixedStepDiscrete")
#define rt_ODEUpdateContinuousStates(si)   rtsiSetT(si,rtsiGetSolverStopTime(si))
#endif

extern RT_MODEL *MODEL(void);
static RT_MODEL *rtM;

#define RTAILAB_VERSION         "3.4.7"
#define MAX_NTARGETS      1000
#define MAX_NAMES_SIZE      256
#define RUN_FOREVER      -1.0
#define POLL_PERIOD      100000000
#define CONNECT_TO_TARGET   0
#define DISCONNECT_TO_TARGET   1
#define START_TARGET      2
#define STOP_TARGET      3
#define UPDATE_PARAM      4

#define MAX_DATA_SIZE      30

#define rt_MainTaskPriority      97
#define rt_HostInterfaceTaskPriority   96

typedef struct rtTargetParamInfo
{
    char modelName[MAX_NAMES_SIZE];
    char blockName[MAX_NAMES_SIZE];
    char paramName[MAX_NAMES_SIZE];
    unsigned int nRows;
    unsigned int nCols;
    unsigned int dataType;
    unsigned int dataClass;
    double dataValue[MAX_DATA_SIZE];
} rtTargetParamInfo;

static sem_t err_sem;
static RT_TASK *rt_MainTask;

static pthread_t rt_HostInterfaceThread;
static RT_TASK *rt_HostInterfaceTask;

static pthread_t rt_BaseRateThread;
static RT_TASK *rt_BaseRateTask;

#ifdef MULTITASKING
static pthread_t *rt_SubRateThreads;
static RT_TASK **rt_SubRateTasks;
#endif

char *HostInterfaceTaskName     = "IFTASK";
char *TargetScopeMbxID           = "RTS";
char *TargetALogMbxID           = "RAL";
char *TargetLogMbxID            = "RTL";
char *TargetLedMbxID            = "RTE";
char *TargetMeterMbxID           = "RTM";
char *TargetSynchronoscopeMbxID = "RTY";

static volatile int Verbose       = 0;
static volatile int UseHRT        = 1;
static volatile int WaitToStart   = 0;
static volatile int IsRunning     = 0;
static volatile int InternalTimer = 1;
static volatile int ClockTick     = 1;
static volatile int CpuMap     = 0xF;
static volatile int StackInc     = 30000;
static volatile int endBaseRate   = 0;
static volatile int endInterface  = 0;
static float FinalTime            = RUN_FOREVER;
static RTIME rt_BaseRateTick;
#ifdef MULTITASKING
static volatile int endSubRate    = 0;
#endif

volatile int endex=0;
volatile int base_rate_loop_counter=0;

#ifdef TASKDURATION
RTIME RTTSKinit=0, RTTSKend=0;
#endif

#define MAX_RTAI_SCOPES   1000
#define MAX_RTAI_LOGS   1000
#define MAX_RTAI_LEDS   1000
#define MAX_RTAI_METERS   1000
#define MAX_RTAI_SYNCHS   1000
SimStruct *rtaiScope[MAX_RTAI_SCOPES];
SimStruct *rtaiALog[MAX_RTAI_LOGS];
SimStruct *rtaiLog[MAX_RTAI_LOGS];
SimStruct *rtaiLed[MAX_RTAI_LEDS];
SimStruct *rtaiMeter[MAX_RTAI_METERS];
SimStruct *rtaiSynchronoscope[MAX_RTAI_SYNCHS];

#define MAX_COMEDI_DEVICES        10

void *ComediDev[MAX_COMEDI_DEVICES];
int ComediDev_InUse[MAX_COMEDI_DEVICES] = {0};
int ComediDev_AIInUse[MAX_COMEDI_DEVICES] = {0};
int ComediDev_AOInUse[MAX_COMEDI_DEVICES] = {0};
int ComediDev_DIOInUse[MAX_COMEDI_DEVICES] = {0};

static void (*WaitTimingEvent)(unsigned long);
static void (*SendTimingEvent)(unsigned long);
static unsigned long TimingEventArg;
static void DummyWait(void) { }
static void DummySend(void) { }
int  check_rtai_modules();

#define XSTR(x)    #x
#define STR(x)     XSTR(x)


static inline void strncpyz(char *dest, const char *src, size_t n)
{
    if (src != NULL)
    {
        strncpy(dest, src, n);
        n = strlen(src);
    }
    else
        n = 0;

    dest[n] = '\0';
}

/* This function is hacked from /usr/src/linux/include/asm-i386/system.h */
static inline void set_double(double *to, double *from)
{
    unsigned long l = ((unsigned long *)from)[0];
    unsigned long h = ((unsigned long *)from)[1];
    __asm__ __volatile__ (
    "1: movl (%0), %%eax; movl 4(%0), %%edx; lock; cmpxchg8b (%0); jnz 1b" : : "D"(to), "b"(l), "c"(h) : "ax", "dx", "memory");
}

#define RT_MODIFY_PARAM_VALUE_IF(rtcase, rttype) \
case rtcase:
\ for (paramIdx = 0; paramIdx < nParams; paramIdx++) {
\ rttype *param = (rttype *)(pMap[mapOffset + paramIdx]);
\ switch (ptinfoGetClass(ptRec)) {
\ case rt_SCALAR:
\ set_double(param, (double *)_newVal); \ if (Verbose) {
\ printf("%s : %G\n", mmiGetBlockTuningBlockName(mmi,i), *param);
\ }
\ break;
\ case rt_VECTOR:
\ param[matIdx] = ((double *) _newVal)[0];
\ if (Verbose) {
\ for (colIdx = 0; colIdx < nCols; colIdx++) {
\ printf("%s : %G\n", mmiGetBlockTuningBlockName(mmi,i), param[colIdx]);
\ }
\ }
\ break;
\ case rt_MATRIX_ROW_MAJOR:
\ param[matIdx] = ((double *) _newVal)[0];
\ if (Verbose) {
\ for (rowIdx = 0; rowIdx < nRows; rowIdx++) {
\ for(colIdx = 0; colIdx < nCols; colIdx++){
\ printf("%s : %G\n", mmiGetBlockTuningBlockName(mmi,i), param[rowIdx*nCols+colIdx]);
\ }
\ }
\ }
\ break;
\ case rt_MATRIX_COL_MAJOR:
\ param[matIdx] = ((double *) _newVal)[0];
\ if (Verbose) {
\ for (rowIdx = 0; rowIdx < nRows; rowIdx++) {
\ for(colIdx = 0; colIdx < nCols; colIdx++) {
\ printf("%s : %G\n", mmiGetBlockTuningBlockName(mmi,i), param[colIdx*nRows+rowIdx]);
\ }
\ }
\ }
\ break;
\ default:
\ return(1);
\ }
\ }
\ break;
int_T rt_ModifyParameterValue(void *mpi, int i, int matIdx, void *_newVal) { ModelMappingInfo *mmi; ParameterTuning *ptRec; void * const *pMap; uint_T nRows; uint_T nCols; uint_T nParams; uint_T mapOffset; uint_T paramIdx; uint_T rowIdx; uint_T colIdx; mmi = (ModelMappingInfo *)mpi; ptRec = (ParameterTuning*)mmiGetBlockTuningParamInfo(mmi,i); pMap = mmiGetParametersMap(mmi); nRows = ptinfoGetNumRows(ptRec); nCols = ptinfoGetNumCols(ptRec); nParams = ptinfoGetNumInstances(ptRec); mapOffset = ptinfoGetParametersOffset(ptRec); switch (ptinfoGetDataTypeEnum(ptRec)) { RT_MODIFY_PARAM_VALUE_IF(SS_DOUBLE, real_T) RT_MODIFY_PARAM_VALUE_IF(SS_SINGLE, real32_T) RT_MODIFY_PARAM_VALUE_IF(SS_INT8, int8_T) RT_MODIFY_PARAM_VALUE_IF(SS_UINT8, uint8_T) RT_MODIFY_PARAM_VALUE_IF(SS_INT16, int16_T) RT_MODIFY_PARAM_VALUE_IF(SS_UINT16, uint16_T) RT_MODIFY_PARAM_VALUE_IF(SS_INT32, int32_T) RT_MODIFY_PARAM_VALUE_IF(SS_UINT32, uint32_T) RT_MODIFY_PARAM_VALUE_IF(SS_BOOLEAN, boolean_T) default: return(1); } return(0); } #define RT_GET_PARAM_INFO_IF(rtcase, rttype) \ case rtcase:
\ for (paramIdx = 0; paramIdx < nParams; paramIdx++) {
\ rttype *param = (rttype *)(pMap[mapOffset + paramIdx]);
\ switch (ptinfoGetClass(ptRec)) {
\ case rt_SCALAR:
\ rtpi->dataValue[0] = *param;
\ break;
\ case rt_VECTOR:
\ for (colIdx = 0; colIdx < nCols; colIdx++) {
\ rtpi->dataValue[colIdx] = param[colIdx];
\ }
\ break;
\ case rt_MATRIX_ROW_MAJOR:
\ for (rowIdx = 0; rowIdx < nRows; rowIdx++) {
\ for (colIdx = 0; colIdx < nCols; colIdx++) {
\ rtpi->dataValue[rowIdx*nCols+colIdx] = param[rowIdx*nCols+colIdx];
\ }
\ }
\ break; \ case rt_MATRIX_COL_MAJOR:
\ for (rowIdx = 0; rowIdx < nRows; rowIdx++) {
\ for (colIdx = 0; colIdx < nCols; colIdx++) {
\ rtpi->dataValue[colIdx*nRows+rowIdx] = param[colIdx*nRows+rowIdx];
\ }
\ }
\ break;
\ default:
\ return(1);
\ }
\ }
\ break; int_T rt_GetParameterInfo(void *mpi, rtTargetParamInfo *rtpi, int i) { ModelMappingInfo *mmi; ParameterTuning *ptRec; void * const *pMap; uint_T nRows; uint_T nCols; uint_T nParams; uint_T mapOffset; uint_T paramIdx; uint_T rowIdx; uint_T colIdx; uint_T dataType; uint_T dataClass; mmi = (ModelMappingInfo *)mpi; ptRec = (ParameterTuning*)mmiGetBlockTuningParamInfo(mmi,i); pMap = mmiGetParametersMap(mmi); nRows = ptinfoGetNumRows(ptRec); nCols = ptinfoGetNumCols(ptRec); nParams = ptinfoGetNumInstances(ptRec); mapOffset = ptinfoGetParametersOffset(ptRec); dataType = ptinfoGetDataTypeEnum(ptRec); dataClass = ptinfoGetClass(ptRec); strncpyz(rtpi->modelName, STR(MODEL), MAX_NAMES_SIZE); strncpyz(rtpi->blockName, mmiGetBlockTuningBlockName(mmi,i), MAX_NAMES_SIZE); strncpyz(rtpi->paramName, mmiGetBlockTuningParamName(mmi,i), MAX_NAMES_SIZE); rtpi->dataType = dataType; rtpi->dataClass = dataClass; rtpi->nRows = nRows; rtpi->nCols = nCols; switch (ptinfoGetDataTypeEnum(ptRec)) { RT_GET_PARAM_INFO_IF(SS_DOUBLE, real_T) RT_GET_PARAM_INFO_IF(SS_SINGLE, real32_T) RT_GET_PARAM_INFO_IF(SS_INT8, int8_T) RT_GET_PARAM_INFO_IF(SS_UINT8, uint8_T) RT_GET_PARAM_INFO_IF(SS_INT16, int16_T) RT_GET_PARAM_INFO_IF(SS_UINT16, uint16_T) RT_GET_PARAM_INFO_IF(SS_INT32, int32_T) RT_GET_PARAM_INFO_IF(SS_UINT32, uint32_T) RT_GET_PARAM_INFO_IF(SS_BOOLEAN, boolean_T) default: return(1); } return(0); } #ifdef MULTITASKING static void *rt_SubRate(int *arg) { int sample, i; char myname[7]; int rt_SubRateTaskPriority; sample = arg[0]; rt_SubRateTaskPriority = arg[1]; rt_allow_nonroot_hrt(); for (i = 0; i < MAX_NTARGETS; i++) { sprintf(myname, "TSR%d", i); if (!rt_get_adr(nam2num(myname))) break; } if (!(rt_SubRateTasks[sample] = rt_task_init_schmod(nam2num(myname), rt_SubRateTaskPriority, 0, 0, SCHED_FIFO, CpuMap))) { printf("Cannot init rt_SubRateTask #%d\n", sample); return (void *)1; } // sem_post(&err_sem); iopl(3); rt_task_use_fpu(rt_SubRateTasks[sample], 1); rt_grow_and_lock_stack(StackInc); if (UseHRT) { rt_make_hard_real_time(); } while (!endSubRate) { rt_task_suspend(rt_SubRateTasks[sample]); if (endSubRate) break; OUTPUTS(rtM, sample); UPDATED(rtM, sample); rt_SimUpdateDiscreteTaskTime(rtmGetTPtr(rtM), rtmGetTimingData(rtM), sample); } if (UseHRT) { rt_make_soft_real_time(); } rt_task_delete(rt_SubRateTasks[sample]); return (void *)(rt_SubRateTasks[sample] = 0); } #endif static void *rt_BaseRate(void *args) { real_T tnext; char myname[7]; int i; #ifdef MULTITASKING int_T sample, *sampleHit = rtmGetSampleHitPtr(rtM); #endif int rt_BaseRateTaskPriority = *((int *)args); rt_allow_nonroot_hrt(); for (i = 0; i < MAX_NTARGETS; i++) { sprintf(myname, "TBR%d", i); if (!rt_get_adr(nam2num(myname))) break; } if (!(rt_BaseRateTask = rt_task_init_schmod(nam2num(myname), rt_BaseRateTaskPriority, 0, 0, SCHED_FIFO, CpuMap))) { printf("Cannot init rt_BaseRateTask\n"); return (void *)1; } sem_post(&err_sem); iopl(3); rt_task_use_fpu(rt_BaseRateTask, 1); rt_grow_and_lock_stack(StackInc); if (UseHRT) { rt_make_hard_real_time(); } rt_rpc(rt_MainTask,0,(void *)myname); rt_task_make_periodic(rt_BaseRateTask, rt_get_time() + rt_BaseRateTick, rt_BaseRateTick); while (!endBaseRate) { #ifdef TASKDURATION RTTSKend=rt_get_cpu_time_ns(); #endif WaitTimingEvent(TimingEventArg); if (endBaseRate) break; #ifdef TASKDURATION RTTSKinit=rt_get_cpu_time_ns(); #endif #ifdef MULTITASKING tnext = rt_SimUpdateDiscreteEvents(rtmGetNumSampleTimes(rtM), rtmGetTimingData(rtM), rtmGetSampleHitPtr(rtM), rtmGetPerTaskSampleHitsPtr(rtM)); rtsiSetSolverStopTime(rtmGetRTWSolverInfo(rtM), tnext); for (sample = FIRST_TID + 1; sample < NUMST; sample++) { if (sampleHit[sample]) { rt_task_resume(rt_SubRateTasks[sample]); } } OUTPUTS(rtM, FIRST_TID); UPDATED(rtM, FIRST_TID); if (rtmGetSampleTime(rtM, 0) == CONTINUOUS_SAMPLE_TIME) { rt_ODEUpdateContinuousStates(rtmGetRTWSolverInfo(rtM)); } else { rt_SimUpdateDiscreteTaskTime(rtmGetTPtr(rtM), rtmGetTimingData(rtM), 0); } #if FIRST_TID == 1 rt_SimUpdateDiscreteTaskTime(rtmGetTPtr(rtM), rtmGetTimingData(rtM), 1); #endif #else tnext = rt_SimGetNextSampleHit(); rtsiSetSolverStopTime(rtmGetRTWSolverInfo(rtM), tnext); OUTPUTS(rtM, 0); UPDATED(rtM, 0); rt_SimUpdateDiscreteTaskSampleHits(rtmGetNumSampleTimes(rtM), rtmGetTimingData(rtM), rtmGetSampleHitPtr(rtM), rtmGetTPtr(rtM)); if (rtmGetSampleTime(rtM,0) == CONTINUOUS_SAMPLE_TIME) { rt_ODEUpdateContinuousStates(rtmGetRTWSolverInfo(rtM)); } #endif base_rate_loop_counter++; } if (UseHRT) { rt_make_soft_real_time(); } rt_task_delete(rt_BaseRateTask); return 0; } static void *rt_HostInterface(void *args) { RT_TASK *task; unsigned int IRequest; char Request; int len; rt_allow_nonroot_hrt(); if (!(rt_HostInterfaceTask = rt_task_init_schmod(nam2num(HostInterfaceTaskName), rt_HostInterfaceTaskPriority, 0, 0, SCHED_FIFO, 0xFF))) { printf("Cannot init rt_HostInterfaceTask\n"); return (void *)1; } sem_post(&err_sem); while (!endInterface) { task = rt_receive(0, &IRequest); Request = (char)(IRequest); if (endInterface) break; switch (Request) { case 'c': { ModelMappingInfo *MMI; uint_T nBlockParams; { int Reply; MMI = (ModelMappingInfo *)rtmGetModelMappingInfo(rtM); nBlockParams = mmiGetNumBlockParams(MMI); Reply = (IsRunning << 16) | (nBlockParams & 0xffff); /* max 2^16-1 blocks */ rt_return(task, Reply); } { int i; rtTargetParamInfo rtParameters; rt_receivex(task, &rtParameters, sizeof(char), &len); rt_GetParameterInfo(MMI, &rtParameters, 0); rt_returnx(task, &rtParameters, sizeof(rtParameters)); for (i = 0; i < nBlockParams; i++) { rt_receivex(task, &rtParameters, sizeof(char), &len); rt_GetParameterInfo(MMI, &rtParameters, i); rt_returnx(task, &rtParameters, sizeof(rtParameters)); } } { int scopeIdx, Reply; float samplingTime; int ntraces; while (1) { rt_receivex(task, &scopeIdx, sizeof(int), &len); if (scopeIdx < 0) { Reply = scopeIdx; rt_returnx(task, &Reply, sizeof(int)); break; } else { ntraces = ssGetNumInputPorts(rtaiScope[scopeIdx]); rt_returnx(task, &ntraces, sizeof(int)); rt_receivex(task, &scopeIdx, sizeof(int), &len); rt_returnx(task, (char *)ssGetModelName(rtaiScope[scopeIdx]), MAX_NAMES_SIZE*sizeof(char)); rt_receivex(task, &scopeIdx, sizeof(int), &len); samplingTime = ssGetSampleTime(rtaiScope[scopeIdx],0); rt_returnx(task, &samplingTime, sizeof(float)); } } } { int logIdx, Reply; float samplingTime; int nrow, ncol, *dim; while (1) { rt_receivex(task, &logIdx, sizeof(int), &len); if (logIdx < 0) { Reply = logIdx; rt_returnx(task, &Reply, sizeof(int)); break; } else { dim = ssGetInputPortDimensions(rtaiLog[logIdx],0); nrow = dim[0]; ncol = dim[1]; rt_returnx(task, &nrow, sizeof(int)); rt_receivex(task, &logIdx, sizeof(int), &len); rt_returnx(task, &ncol, sizeof(int)); rt_receivex(task, &logIdx, sizeof(int), &len); rt_returnx(task, (char *)ssGetModelName(rtaiLog[logIdx]), MAX_NAMES_SIZE*sizeof(char)); rt_receivex(task, &logIdx, sizeof(int), &len); samplingTime = ssGetSampleTime(rtaiLog[logIdx],0); rt_returnx(task, &samplingTime, sizeof(float)); } } } { int alogIdx, Reply; /* section added to support automatic log block taken from log code */ float samplingTime; int nrow, ncol, *dim; while (1) { rt_receivex(task, &alogIdx, sizeof(int), &len); if (alogIdx < 0) { Reply = alogIdx; rt_returnx(task, &Reply, sizeof(int)); break; } else { dim = ssGetInputPortDimensions(rtaiALog[alogIdx],0); nrow = dim[0]; ncol = dim[1]; rt_returnx(task, &nrow, sizeof(int)); rt_receivex(task, &alogIdx, sizeof(int), &len); rt_returnx(task, &ncol, sizeof(int)); rt_receivex(task, &alogIdx, sizeof(int), &len); rt_returnx(task, (char *)ssGetModelName(rtaiALog[alogIdx]), MAX_NAMES_SIZE*sizeof(char)); rt_receivex(task, &alogIdx, sizeof(int), &len); samplingTime = ssGetSampleTime(rtaiALog[alogIdx],0); rt_returnx(task, &samplingTime, sizeof(float)); } } } { int ledIdx, Reply; float samplingTime; int n_leds; while (1) { rt_receivex(task, &ledIdx, sizeof(int), &len); if (ledIdx < 0) { Reply = ledIdx; rt_returnx(task, &Reply, sizeof(int)); break; } else { n_leds = ssGetNumInputPorts(rtaiLed[ledIdx]); rt_returnx(task, &n_leds, sizeof(int)); rt_receivex(task, &ledIdx, sizeof(int), &len); rt_returnx(task, (char *)ssGetModelName(rtaiLed[ledIdx]), MAX_NAMES_SIZE*sizeof(char)); rt_receivex(task, &ledIdx, sizeof(int), &len); samplingTime = ssGetSampleTime(rtaiLed[ledIdx],0); rt_returnx(task, &samplingTime, sizeof(float)); } } } { int meterIdx, Reply; float samplingTime; while (1) { rt_receivex(task, &meterIdx, sizeof(int), &len); if (meterIdx < 0) { Reply = meterIdx; rt_returnx(task, &Reply, sizeof(int)); break; } else { rt_returnx(task, (char *)ssGetModelName(rtaiMeter[meterIdx]), MAX_NAMES_SIZE*sizeof(char)); rt_receivex(task, &meterIdx, sizeof(int), &len); samplingTime = ssGetSampleTime(rtaiMeter[meterIdx],0); rt_returnx(task, &samplingTime, sizeof(float)); } } } { int synchIdx, Reply; float samplingTime; while (1) { rt_receivex(task, &synchIdx, sizeof(int), &len); if (synchIdx < 0) { Reply = synchIdx; rt_returnx(task, &Reply, sizeof(int)); break; } else { rt_returnx(task, (char *)ssGetModelName(rtaiSynchronoscope[synchIdx]), MAX_NAMES_SIZE*sizeof(char)); rt_receivex(task, &synchIdx, sizeof(int), &len); samplingTime = ssGetSampleTime(rtaiSynchronoscope[synchIdx],0); rt_returnx(task, &samplingTime, sizeof(float)); } } } break; } case 's': { int Reply = 1; rt_task_resume(rt_MainTask); rt_return(task, Reply); break; } case 't': { int Reply = 0; rt_return(task, Reply); endex = 1; break; } case 'p': { ModelMappingInfo *MMI; int index; int Reply; int matIdx; double newv; rt_return(task, IsRunning); Reply = 0; rt_receivex(task, &index, sizeof(int), &len); rt_returnx(task, &Reply, sizeof(int)); Reply = 1; MMI = (ModelMappingInfo *)rtmGetModelMappingInfo(rtM); rt_receivex(task, &newv, sizeof(double), &len); rt_returnx(task, &Reply, sizeof(int)); rt_receivex(task, &matIdx, sizeof(int), &len); rt_ModifyParameterValue(MMI, index, matIdx, &newv); rt_returnx(task, &Reply, sizeof(int)); break; } case 'g': { int i; uint_T nBlockParams; rtTargetParamInfo rtParameters; ModelMappingInfo *MMI; rt_return(task, IsRunning); MMI = (ModelMappingInfo *)rtmGetModelMappingInfo(rtM); nBlockParams = mmiGetNumBlockParams(MMI); for (i = 0; i < nBlockParams; i++) { rt_receivex(task, &rtParameters, sizeof(char), &len); rt_GetParameterInfo(MMI, &rtParameters, i); rt_returnx(task, &rtParameters, sizeof(rtParameters)); } break; } case 'd': { int ParamCnt; int Reply; rt_return(task, IsRunning); Reply = 0; rt_receivex(task, &ParamCnt, sizeof(int), &len); rt_returnx(task, &Reply, sizeof(int)); { int i; ModelMappingInfo *MMI; struct { int index; int mat_index; double value; } BatchParams[ParamCnt]; Reply = 1; MMI = (ModelMappingInfo *)rtmGetModelMappingInfo(rtM); rt_receivex(task, &BatchParams, sizeof(BatchParams), &len); for (i = 0; i < ParamCnt; i++) { rt_ModifyParameterValue(MMI, BatchParams[i].index, BatchParams[i].mat_index, &BatchParams[i].value); } } rt_returnx(task, &ParamCnt, sizeof(int)); break; } case 'm': { float ttime = (float)rtmGetT(rtM); int Reply; rt_return(task, IsRunning); rt_receivex(task, &Reply, sizeof(int), &len); rt_returnx(task, &ttime, sizeof(float)); } default: break; } } rt_task_delete(rt_HostInterfaceTask); return 0; } static int_T rt_Main(RT_MODEL * (*model_name)(void), int_T priority) { const char *status; RTIME rt_BaseTaskPeriod; struct timespec rt_MainTaskPollPeriod = { 0, POLL_PERIOD }; struct timespec err_timeout; int msg, i; char myname[7]; SEM *hard_timers_cnt = NULL; #ifdef MULTITASKING int sample; #endif rt_allow_nonroot_hrt(); for (i = 0; i < MAX_NTARGETS; i++) { sprintf(myname, "TMN%d", i); if (!rt_get_adr(nam2num(myname))) break; } if (!(rt_MainTask = rt_task_init_schmod(nam2num(myname), rt_MainTaskPriority, 0, 0, SCHED_FIFO, 0xFF))) { fprintf(stderr, "Cannot init rt_MainTask\n"); return 1; } sem_init(&err_sem, 0, 0); iopl(3); rt_InitInfAndNaN(sizeof(real_T)); rtM = model_name(); if (rtM == NULL) { fprintf(stderr, "Memory allocation error during target registration.\n"); goto finish; } if (rtmGetErrorStatus(rtM) != NULL) { fprintf(stderr, "Error during target registration: %s.\n", rtmGetErrorStatus(rtM)); goto finish; } if (FinalTime > 0.0 || FinalTime == RUN_FOREVER) { rtmSetTFinal(rtM, (real_T)FinalTime); } INITIALIZE_SIZES(rtM); INITIALIZE_SAMPLE_TIMES(rtM); status = rt_SimInitTimingEngine(rtmGetNumSampleTimes(rtM), rtmGetStepSize(rtM), rtmGetSampleTimePtr(rtM), rtmGetOffsetTimePtr(rtM), rtmGetSampleHitPtr(rtM), rtmGetSampleTimeTaskIDPtr(rtM), rtmGetTStart(rtM), &rtmGetSimTimeStep(rtM), &rtmGetTimingData(rtM)); if (status != NULL) { fprintf(stderr, "Failed to initialize target sample time engine: %s.\n", status); goto finish; } rt_ODECreateIntegrationData(rtmGetRTWSolverInfo(rtM)); START(rtM); if (rtmGetErrorStatus(rtM) != NULL) { fprintf(stderr, "Failed in target initialization.\n"); TERMINATE(rtM); fprintf(stderr, "Target is terminated.\n"); goto finish; } if ((pthread_create(&rt_HostInterfaceThread, NULL, rt_HostInterface, NULL)) != 0) { fprintf(stderr, "Failed to create HostInterfaceThread.\n"); TERMINATE(rtM); fprintf(stderr, "Target is terminated.\n"); goto finish; } err_timeout.tv_sec = (long int)(time(NULL)) + 1; err_timeout.tv_nsec = 0; if ((sem_timedwait(&err_sem, &err_timeout)) != 0) { TERMINATE(rtM); fprintf(stderr, "Target is terminated.\n"); goto finish; } #ifdef MULTITASKING rt_SubRateTasks = malloc(NUMST*sizeof(RT_TASK *)); rt_SubRateThreads = malloc(NUMST*sizeof(pthread_t)); for (sample = FIRST_TID + 1; sample < NUMST; sample++) { int arg[2]; arg[0] = sample; arg[1] = priority + sample; pthread_create(rt_SubRateThreads + sample, NULL, (void *)rt_SubRate, (void *)arg); } #endif if ((pthread_create(&rt_BaseRateThread, NULL, rt_BaseRate, &priority)) != 0) { fprintf(stderr, "Failed to create BaseRateThread.\n"); endInterface = 1; rt_send(rt_HostInterfaceTask, 0); pthread_join(rt_HostInterfaceThread, NULL); TERMINATE(rtM); fprintf(stderr, "Target is terminated.\n"); goto finish; } err_timeout.tv_sec = (long int)(time(NULL)) + 1; err_timeout.tv_nsec = 0; if ((sem_timedwait(&err_sem, &err_timeout)) != 0) { endInterface = 1; rt_send(rt_HostInterfaceTask, 0); pthread_join(rt_HostInterfaceThread, NULL); TERMINATE(rtM); fprintf(stderr, "Target is terminated.\n"); goto finish; } rt_BaseTaskPeriod = (RTIME)(1000000000.0*rtmGetStepSize(rtM)); if (InternalTimer) { WaitTimingEvent = (void *)rt_task_wait_period; if (!(hard_timers_cnt = rt_get_adr(nam2num("HTMRCN")))) { if (!ClockTick) { rt_set_oneshot_mode(); start_rt_timer(0); rt_BaseRateTick = nano2count(rt_BaseTaskPeriod); } else { rt_set_periodic_mode(); rt_BaseRateTick = start_rt_timer(nano2count(rt_BaseTaskPeriod)); } hard_timers_cnt = rt_sem_init(nam2num("HTMRCN"), 0); } else { rt_BaseRateTick = nano2count(rt_BaseTaskPeriod); rt_sem_signal(hard_timers_cnt); } } else { WaitTimingEvent = (void *)DummyWait; SendTimingEvent = (void *)DummySend; } if (Verbose) { int j; printf("\nTarget info\n"); printf("===========\n"); printf(" Model name : %s\n", STR(MODEL)); printf(" Base sample time : %f [s]\n", rtmGetStepSize(rtM)); printf(" Number of sample times : %d\n", rtmGetNumSampleTimes(rtM)); for (j = 0; j < rtmGetNumSampleTimes(rtM); j++) { printf(" Sample Time %d : %f [s]\n", j, rtmGetSampleTimePtr(rtM)[j]); } printf("\n"); } if (WaitToStart) { if (Verbose) { printf("Target is waiting to start.\n"); } rt_task_suspend(rt_MainTask); } rt_receive(0, &msg); rt_return(rt_BaseRateTask,0); IsRunning = 1; if (Verbose) { printf("Target is running.\n"); } while (!endex) { if (rtmGetTFinal(rtM) != RUN_FOREVER && (rtmGetTFinal(rtM) - rtmGetT(rtM)) <= rtmGetT(rtM)*DBL_EPSILON) { if (Verbose) { printf("Final time occured.\n"); } break; } if (rtmGetErrorStatus(rtM) != NULL) { fprintf(stderr, "%s.\n", rtmGetErrorStatus(rtM)); break; } nanosleep(&rt_MainTaskPollPeriod, NULL); } endBaseRate = 1; if (!InternalTimer) { SendTimingEvent(TimingEventArg); } pthread_join(rt_BaseRateThread, NULL); #ifdef MULTITASKING endSubRate = 1; for (sample = FIRST_TID + 1; sample < NUMST; sample++) { if (rt_SubRateTasks[sample]) { rt_task_resume(rt_SubRateTasks[sample]); } pthread_join(rt_SubRateThreads[sample], NULL); } #endif IsRunning = 0; if (Verbose) { printf("Target is stopped.\n"); } endInterface = 1; rt_send(rt_HostInterfaceTask, 0); pthread_join(rt_HostInterfaceThread, NULL); if (InternalTimer) { if (!rt_sem_wait_if(hard_timers_cnt)) { rt_sem_delete(hard_timers_cnt); } } #ifdef MULTITASKING free(rt_SubRateTasks); free(rt_SubRateThreads); #endif TERMINATE(rtM); printf("Target is terminated.\n"); finish: base_rate_loop_counter = -1; sem_destroy(&err_sem); rt_task_delete(rt_MainTask); return 0; } static void endme(int dummy) { signal(SIGINT, endme); signal(SIGTERM, endme); endex = 1; } struct option options[] = { { "usage", 0, 0, 'u' }, { "verbose", 0, 0, 'v' }, { "version", 0, 0, 'V' }, { "soft", 0, 0, 's' }, { "wait", 0, 0, 'w' }, { "priority", 1, 0, 'p' }, { "finaltime", 1, 0, 'f' }, { "name", 1, 0, 'n' }, { "idscope", 1, 0, 'i' }, { "idlog", 1, 0, 'l' }, { "idalog", 1, 0, 'a' }, { "idmeter", 1, 0, 't' }, { "idled", 1, 0, 'd' }, { "idsynch", 1, 0, 'y' }, { "cpumap", 1, 0, 'c' }, { "external", 0, 0, 'e' }, { "oneshot", 0, 0, 'o' }, { "stack", 1, 0, 'm' } }; void print_usage(void) { fputs( ("\nUsage: 'RT-model-name' [OPTIONS]\n" "\n" "OPTIONS:\n" " -u, --usage\n" " print usage\n" " -v, --verbose\n" " verbose output\n" " -V, --version\n" " print rt_main version\n" " -s, --soft\n" " run RT-model in soft real time (default hard RT)\n" " -w, --wait\n" " wait to start\n" " -p <priority>, --priority <priority>\n" " set the priority at which the RT-model's highest priority task will run (default 0)\n" " -f <finaltime>, --finaltime <finaltime>\n" " set the final time (default infinite)\n" " -n <ifname>, --name <ifname>\n" " set the name of the host interface task (default IFTASK)\n" " -i <scopeid>, --idscope <scopeid>\n" " set the scope mailboxes identifier (default RTS)\n" " -l <logid>, --idlog <logid>\n" " set the log mailboxes identifier (default RTL)\n" " -a <alogid>, --idalog <alogid>\n" " set the automatic log mailboxes identifier (default RAL)\n" " -t <meterid>, --idmeter <meterid>\n" " set the meter mailboxes identifier (default RTM)\n" " -d <ledid>, --idled <ledid>\n" " set the led mailboxes identifier (default RTE)\n" " -y <synchid>, --idsynch <synchid>\n" " set the synchronoscope mailboxes identifier (default RTY)\n" " -c <cpumap>, --cpumap <cpumap>\n" " (1 << cpunum) on which the RT-model runs (default: let RTAI choose)\n" " -e, --external\n" " RT-model timed by an external resume (default internal)\n" " -o, --oneshot\n" " the hard timer will run in oneshot mode (default periodic)\n" " -m <stack>, --stack <stack>\n" " set a guaranteed stack size extension (default 30000)\n" "\n") ,stderr); exit(0); } int MAIN_NAME(int argc, char *argv[]) { int priority = 0; int c, option_index = 0; signal(SIGINT, endme); signal(SIGTERM, endme); while (1) { c = getopt_long(argc, argv, "uvVsweop:f:n:i:l:a:d:t:y:c:m:", options, &option_index); if (c == -1) break; switch (c) { case 'v': Verbose = 1; break; case 'V': fputs("rt_main version " RTAILAB_VERSION "\n", stderr); exit(0); case 'p': if (isalpha(optarg[0])) { fprintf(stderr, "Invalid priority value\n"); exit(1); } priority = atoi(optarg); if (priority < 0) { fprintf(stderr, "Invalid priority value\n"); exit(1); } break; case 's': UseHRT = 0; break; case 'f': if (strstr(optarg, "inf")) { FinalTime = RUN_FOREVER; } else { if (isalpha(optarg[0])) { fprintf(stderr, "Invalid final time value\n"); exit(1); } FinalTime = atof(optarg); } break; case 'w': WaitToStart = 1; break; case 'u': print_usage(); exit(0); case 'n': HostInterfaceTaskName = strdup(optarg); break; case 'i': TargetScopeMbxID = strdup(optarg); break; case 'l': TargetLogMbxID = strdup(optarg); break; case 't': TargetMeterMbxID = strdup(optarg); break; case 'a': TargetALogMbxID = strdup(optarg); break; case 'd': TargetLedMbxID = strdup(optarg); break; case 'y': TargetSynchronoscopeMbxID = strdup(optarg); break; case 'c': if (!(CpuMap = atoi(optarg))) { CpuMap = 0xF; } break; case 'e': InternalTimer = 0; break; case 'o': ClockTick = 0; break; case 'm': StackInc = atoi(optarg); break; default: break; } } if (Verbose) { printf("\nTarget settings\n"); printf("===============\n"); printf(" Real-time : %s\n", UseHRT ? "HARD" : "SOFT"); printf(" Timing : %s / ", InternalTimer ? "internal" : "external"); printf("%s\n", ClockTick ? "periodic" : "oneshot"); printf(" Priority : %d\n", priority); if (FinalTime > 0) { printf(" Finaltime : %f [s]\n", FinalTime); } else { printf(" Finaltime : RUN FOREVER\n"); } printf(" CPU map : %x\n\n", CpuMap); } if (check_rtai_modules()) { return rt_Main(MODEL, priority); } else { printf("Required rtai modules are not loaded! -- exiting\n"); return -1; } } /* * Code to check for required RTAI modules. * jbrandt@nrao.edu, 25 Jun 2008 * * Returns TRUE if all modules listed are loaded. */ static char *modulelist[] = { "netrpc", "rtdm", "msg", "mbx", "sem", "lxrt", "hal" }; int check_rtai_modules() { FILE *fin; char line[256]; int module_mask = 0; int i; int nmodules = sizeof(modulelist)/sizeof(char *); fin=fopen("/proc/modules", "r"); if (fin == NULL) { printf("Error opening /proc/modules, cannot continue\n"); return FALSE; } while (!feof(fin)) { fgets(line, sizeof(line), fin); if (!strncmp("rtai_", line, 5)) { for (i=0; i<nmodules; ++i) { if (!strncmp(&line[5], modulelist[i], strlen(modulelist[i]))) { module_mask |= (1 << i); } } } } fclose(fin); /* Are all modules loaded? */ return (module_mask == ((1 << nmodules)-1)); }

-- JoeBrandt - 20 Aug 2008
Topic revision: r1 - 2008-08-20, JoeBrandt
This site is powered by FoswikiCopyright © by the contributing authors. All material on this collaboration platform is the property of the contributing authors.
Ideas, requests, problems regarding NRAO Public Wiki? Send feedback