/* flag == 3 */ static int eventout(scicos_block *block) { struct FlagTestDev * comdev = (struct FlagTestDev *) (*block->work); if (get_scicos_time()==comdev->time){ comdev->step++; } else { comdev->time=get_scicos_time(); comdev->step=1; } printf("Flag 3: update event output, step=%d, t=%f\n",comdev->step,comdev->time); return 0; }
/*--------------------------------------------------------------------------*/ SCICOS_BLOCKS_IMPEXP void backlash(scicos_block *block,int flag) { double* rw = NULL,t = 0.; if (flag == 4){/* the workspace is used to store previous values */ if ((*block->work= scicos_malloc(sizeof(double)* 4))== NULL ) { set_block_error(-16); return; } rw=*block->work; t=get_scicos_time(); rw[0]=t; rw[1]=t; rw[2]=block->rpar[0]; rw[3]=block->rpar[0]; }else if (flag == 5){ scicos_free(*block->work); }else if (flag == 1) { rw=*block->work; t=get_scicos_time(); if(t>rw[1]) { rw[0]=rw[1]; rw[2]=rw[3]; } rw[1]=t; if(block->inptr[0][0]>rw[2]+block->rpar[1]/2){ rw[3]=block->inptr[0][0]-block->rpar[1]/2; } else if (block->inptr[0][0]<rw[2]-block->rpar[1]/2){ rw[3]=block->inptr[0][0]+block->rpar[1]/2; } else { rw[3]=rw[2]; } block->outptr[0][0]=rw[3]; } else if (flag == 9) { rw=*block->work; t=get_scicos_time(); if(t>rw[1]){ block->g[0] = block->inptr[0][0]-block->rpar[1]/2-rw[3]; block->g[1] = block->inptr[0][0]+block->rpar[1]/2-rw[3]; }else{ block->g[0] = block->inptr[0][0]-block->rpar[1]/2-rw[2]; block->g[1] = block->inptr[0][0]+block->rpar[1]/2-rw[2]; } } }
/*--------------------------------------------------------------------------*/ SCICOS_BLOCKS_IMPEXP void m_frequ(scicos_block *block, int flag) { double *mat = NULL; double *Dt = NULL; double *off = NULL; SCSINT32_COP *icount = NULL; double t = 0.0; time_counter_t** work = (time_counter_t**) block->work; time_counter_t *counter = NULL; int m = 0; mat = GetRealOparPtrs(block, 1); Dt = GetRealOparPtrs(block, 2); off = GetRealOparPtrs(block, 3); icount = Getint32OparPtrs(block, 4); m = GetOparSize(block, 1, 1); switch (flag) { case 4 : /* the workspace is used to store discrete counter value */ { if ((*work = (time_counter_t*) scicos_malloc(sizeof(time_counter_t) * 2)) == NULL) { set_block_error(-16); return; } counter = *work; *counter = *icount; (*(counter + 1)) = 0; break; } /* event date computation */ case 3 : { counter = *work; t = get_scicos_time(); *counter += (int)mat[*(counter + 1)]; /*increase counter*/ block->evout[(int)mat[*(counter + 1) + m] - 1] = *off + ((double) * counter / (*Dt)) - t; (*(counter + 1))++; *(counter + 1) = *(counter + 1) % m; break; } /* finish */ case 5 : { scicos_free(*work); /*free the workspace*/ break; } default : break; } }
static void inout(scicos_block *block) { double w,pi=3.1415927; double t = get_scicos_time(); double * rpar = GetRparPtrs(block); double *y = block->outptr[0]; if (t<rpar[4]) y[0]=0.0; else { w=2*pi*rpar[1]*(t-rpar[4])-rpar[2]; y[0]=rpar[0]*sin(w)+rpar[3]; } }
static int inout(scicos_block *block) { struct FlagTestDev * comdev = (struct FlagTestDev *) (*block->work); int i; if (get_scicos_time()==comdev->time){ comdev->step++; } else { comdev->time=get_scicos_time(); comdev->step=1; } printf("Flag 1: update output, step=%d, t=%f\n",comdev->step,comdev->time); if (GetNin(block)>0){ double *u1 = GetRealInPortPtrs(block,1); comdev->data=u1[0]; } if (GetNout(block)>0){ double *y1 = GetRealOutPortPtrs(block,1); y1[0]=comdev->data; } return 0; }
/* Copyright INRIA * Scicos block simulator * event delay with discrete counter */ SCICOS_BLOCKS_IMPEXP void evtdly4(scicos_block *block, int flag) { double t = 0.; time_counter_t** work = (time_counter_t**) block->work; time_counter_t* i = NULL; switch (flag) { /* init */ case 4 : /* the workspace is used to store discrete counter value */ { if ((*work = (time_counter_t*) scicos_malloc(sizeof(time_counter_t))) == NULL) { set_block_error(-16); return; } i = *work; (*i) = 0; break; } /* event date computation */ case 3 : { double dt; i = *work; t = get_scicos_time(); (*i)++; /*increase counter*/ dt = block->rpar[1] + (*i) * block->rpar[0] - t; /* on event enabled, use the default delay if not scheduled */ if (block->rpar[1] >= 0 && dt < 0) { dt = block->rpar[0]; } block->evout[0] = dt; break; } /* finish */ case 5 : { scicos_free(*work); /*free the workspace*/ break; } default : break; } }
static void inout(scicos_block *block) { double *u; int ntraces=block->nin; struct { float t; float u[ntraces]; } data; int i; data.t=(float) get_scicos_time(); for (i = 0; i < ntraces; i++) { u=block->inptr[i]; data.u[i] = (float) u[0]; } rtf_put(block->ipar[0],&data, sizeof(data)); }
static void inout(scicos_block *block) { double *u; MBX *mbx = *(block->work); int ntraces=GetNin(block); struct { double t; double u[ntraces]; } data; int i; data.t=get_scicos_time(); for (i = 0; i < ntraces; i++) { u = block->inptr[i]; data.u[i] = u[0]; } RT_mbx_send_if(0, 0, mbx, &data, sizeof(data)); }
/** \fn void cscope(scicos_block * block,int flag) \brief the computational function \param block A pointer to a scicos_block \param flag An int which indicates the state of the block (init, update, ending) */ SCICOS_BLOCKS_IMPEXP void cevscpe(scicos_block * block, scicos_flag flag) { int iFigureUID; double t; int i; int mask; int nclk = block->nipar - 6; sco_data *sco; BOOL result; switch (flag) { case Initialization: sco = getScoData(block); if (sco == NULL) { set_block_error(-5); } iFigureUID = getFigure(block); if (iFigureUID == 0) { // allocation error set_block_error(-5); break; } setSegsBuffers(block, DEFAULT_MAX_NUMBER_OF_POINTS); break; case StateUpdate: iFigureUID = getFigure(block); if (iFigureUID == 0) { // allocation error set_block_error(-5); break; } t = get_scicos_time(); // select only the masked indexes for (i = 0; i < nclk; i++) { mask = 1 << i; if ((block->nevprt & mask) == mask) { appendData(block, i, t); result = pushData(block, i); if (result == FALSE) { Coserror("%s: unable to push some data.", "cevscpe"); break; } } } break; case Ending: freeScoData(block); break; default: break; } }
void sci_mavlinkHilState(scicos_block *block, scicos::enumScicosFlags flag) { // data double * u=GetRealInPortPtrs(block,1); double * y=GetRealOutPortPtrs(block,1); int * ipar=block->ipar; static char * device; static int baudRate; static char ** stringArray; static int * intArray; static int count = 0; static uint16_t packet_drops = 0; //handle flags if (flag==scicos::initialize || flag==scicos::reinitialize) { if (mavlink_comm_0_port == NULL) { getIpars(1,1,ipar,&stringArray,&intArray); device = stringArray[0]; baudRate = intArray[0]; try { mavlink_comm_0_port = new BufferedAsyncSerial(device,baudRate); } catch(const boost::system::system_error & e) { Coserror((char *)e.what()); } } } else if (flag==scicos::terminate) { if (mavlink_comm_0_port) { delete mavlink_comm_0_port; mavlink_comm_0_port = NULL; } } else if (flag==scicos::updateState) { } else if (flag==scicos::computeDeriv) { } else if (flag==scicos::computeOutput) { // channel mavlink_channel_t chan = MAVLINK_COMM_0; // loop rates // TODO: clean this up to use scicos events w/ timers static int attitudeRate = 50; static int positionRate = 10; static int airspeedRate = 1; // initial times double scicosTime = get_scicos_time(); static double attitudeTimeStamp = scicosTime; static double positionTimeStamp = scicosTime; static double airspeedTimeStamp = scicosTime; // send airspeed message if (scicosTime - airspeedTimeStamp > 1.0/airspeedRate) { airspeedTimeStamp = scicosTime; // airspeed (true velocity m/s) float Vt = u[0]; //double rawPress = 1; //double airspeed = 1; //mavlink_msg_raw_pressure_send(chan,timeStamp,airspeed,rawPress,0); } else if (scicosTime - airspeedTimeStamp < 0) airspeedTimeStamp = scicosTime; // send attitude message if (scicosTime - attitudeTimeStamp > 1.0/attitudeRate) { attitudeTimeStamp = scicosTime; // attitude states (rad) float roll = u[1]; float pitch = u[2]; float yaw = u[3]; // body rates float rollRate = u[4]; float pitchRate = u[5]; float yawRate = u[6]; mavlink_msg_attitude_send(chan,attitudeTimeStamp,roll,pitch,yaw, rollRate,pitchRate,yawRate); } else if (scicosTime - attitudeTimeStamp < 0) attitudeTimeStamp = scicosTime; // send gps mesage if (scicosTime - positionTimeStamp > 1.0/positionRate) { positionTimeStamp = scicosTime; // gps double cog = u[7]; double sog = u[8]; double lat = u[9]*rad2deg; double lon = u[10]*rad2deg; double alt = u[11]; //double rawPress = 1; //double airspeed = 1; mavlink_msg_gps_raw_send(chan,positionTimeStamp,1,lat,lon,alt,2,10,sog,cog); //mavlink_msg_raw_pressure_send(chan,timeStamp,airspeed,rawPress,0); } else if (scicosTime - positionTimeStamp < 0) positionTimeStamp = scicosTime; // receive messages mavlink_message_t msg; mavlink_status_t status; while(comm_get_available(MAVLINK_COMM_0)) { uint8_t c = comm_receive_ch(MAVLINK_COMM_0); // try to get new message if(mavlink_parse_char(MAVLINK_COMM_0,c,&msg,&status)) { switch(msg.msgid) { case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: { //std::cout << "receiving messages" << std::endl; mavlink_rc_channels_scaled_t rc_channels; mavlink_msg_rc_channels_scaled_decode(&msg,&rc_channels); y[0] = rc_channels.chan1_scaled/10000.0f; y[1] = rc_channels.chan2_scaled/10000.0f; y[2] = rc_channels.chan3_scaled/10000.0f; y[3] = rc_channels.chan4_scaled/10000.0f; y[4] = rc_channels.chan5_scaled/10000.0f; y[5] = rc_channels.chan6_scaled/10000.0f; y[6] = rc_channels.chan7_scaled/10000.0f; y[7] = rc_channels.chan8_scaled/10000.0f; break; } } // update packet drop counter packet_drops += status.packet_rx_drop_count; } } } }
/** \fn void cmscope(scicos_block * block,int flag) \brief the computational function \param block A pointer to a scicos_block \param flag An int which indicates the state of the block (init, update, ending) */ SCICOS_BLOCKS_IMPEXP void cmscope(scicos_block * block, scicos_flag flag) { char const* pFigureUID; double t; double *u; sco_data *sco; int i, j; BOOL result; switch (flag) { case Initialization: sco = getScoData(block); if (sco == NULL) { set_block_error(-5); break; } pFigureUID = getFigure(block); if (pFigureUID == NULL) { // allocation error set_block_error(-5); break; } break; case StateUpdate: pFigureUID = getFigure(block); if (pFigureUID == NULL) { // allocation error set_block_error(-5); break; } t = get_scicos_time(); for (i = 0; i < block->nin; i++) { u = (double *)block->inptr[i]; appendData(block, i, t, u); for (j = 0; j < block->insz[i]; j++) { result = pushData(block, i, j); if (result == FALSE) { Coserror("%s: unable to push some data.", "cmscope"); break; } } } break; case Ending: freeScoData(block); break; default: break; } }
/*--------------------------------------------------------------------------*/ SCICOS_BLOCKS_IMPEXP void time_delay(scicos_block *block, int flag) { /* rpar[0]=delay, rpar[1]=init value, ipar[0]=buffer length */ double** work = (double**) block->work; double *pw = NULL, del = 0., t = 0., td = 0., eps = 0.; int* iw = NULL; int i = 0, j = 0, k = 0; if (flag == 4) { /* the workspace is used to store previous values */ if ((*work = (double*) scicos_malloc(sizeof(int) + sizeof(double) * block->ipar[0] * (1 + block->insz[0]))) == NULL ) { set_block_error(-16); return; } eps = 1.0e-9; /* shift times to left to avoid replacing 0 */ pw = *work; pw[0] = -block->rpar[0] * (block->ipar[0] - 1) - eps; for (j = 1; j < block->insz[0] + 1; j++) { pw[block->ipar[0]*j] = block->rpar[1]; } for (i = 1; i < block->ipar[0]; i++) { pw[i] = pw[i - 1] + block->rpar[0] - eps; for (j = 1; j < block->insz[0] + 1; j++) { pw[i + block->ipar[0]*j] = block->rpar[1]; } } iw = (int *)(pw + block->ipar[0] * (1 + block->insz[0])); *iw = 0; for (k = 0; k < block->insz[0]; k++) { block->outptr[0][k] = block->rpar[1]; } } else if (flag == 5) { scicos_free(*work); } else if (flag == 0 || flag == 2) { if (flag == 2) { do_cold_restart(); } pw = *work; iw = (int *)(pw + block->ipar[0] * (1 + block->insz[0])); t = get_scicos_time(); td = t - block->rpar[0]; if (td < pw[*iw]) { scicos_print(_("delayed time=%f but last stored time=%f \n"), td, pw[*iw]); scicos_print(_("Consider increasing the length of buffer in delay block \n")); } if (t > pw[(block->ipar[0] + *iw - 1) % block->ipar[0]]) { for (j = 1; j < block->insz[0] + 1; j++) { pw[*iw + block->ipar[0]*j] = block->inptr[0][j - 1]; } pw[*iw] = t; /*scicos_print(_("**time is %f. I put %f, in %d \n"), t,block->inptr[0][0],*iw);*/ *iw = (*iw + 1) % block->ipar[0]; } else { for (j = 1; j < block->insz[0] + 1; j++) { pw[(block->ipar[0] + *iw - 1) % block->ipar[0] + block->ipar[0]*j] = block->inptr[0][j - 1]; } pw[(block->ipar[0] + *iw - 1) % block->ipar[0]] = t; /*scicos_print("**time is %f. I put %f, in %d \n", t,block->inptr[0][0],*iw);*/ } } else if (flag == 1) { pw = *work; iw = (int *) (pw + block->ipar[0] * (1 + block->insz[0])); t = get_scicos_time(); td = t - block->rpar[0]; i = 0; j = block->ipar[0] - 1; while (j - i > 1) { k = (i + j) / 2; if (td < pw[(k + *iw) % block->ipar[0]]) { j = k; } else if (td > pw[(k + *iw) % block->ipar[0]]) { i = k; } else { i = k; j = k; break; } } i = (i + *iw) % block->ipar[0]; j = (j + *iw) % block->ipar[0]; del = pw[j] - pw[i]; /* scicos_print(_("time is %f. interpolating %d and %d, i.e. %f, %f\n"), t,i,j,pw[i],pw[j]); scicos_print(_("values are %f %f.\n"),pw[i+block->ipar[0]],pw[j+block->ipar[0]]);*/ if (del != 0.0) { for (k = 1; k < block->insz[0] + 1; k++) { block->outptr[0][k - 1] = ((pw[j] - td) * pw[i + block->ipar[0] * k] + (td - pw[i]) * pw[j + block->ipar[0] * k]) / del; } } else { for (k = 1; k < block->insz[0] + 1; k++) { block->outptr[0][k - 1] = pw[i + block->ipar[0] * k]; } } } }
/*--------------------------------------------------------------------------*/ SCICOS_BLOCKS_IMPEXP void variable_delay(scicos_block *block, int flag) { /* rpar[0]=max delay, rpar[1]=init value, ipar[0]=buffer length */ double** work = (double**) block->work; double* pw = NULL, del = 0., t = 0., td = 0.; int* iw = NULL; int i = 0, j = 0, k = 0; if (flag == 4) /* the workspace is used to store previous values */ { if ((*work = (double*) scicos_malloc(sizeof(int) + sizeof(double) * block->ipar[0] * (1 + block->insz[0]))) == NULL ) { set_block_error(-16); return; } pw = *work; pw[0] = -block->rpar[0] * block->ipar[0]; for (i = 1; i < block->ipar[0]; i++) { pw[i] = pw[i - 1] + block->rpar[0]; for (j = 1; j < block->insz[0] + 1; j++) { pw[i + block->ipar[0]*j] = block->rpar[1]; } } iw = (int *) (pw + block->ipar[0] * (1 + block->insz[0])); *iw = 0; } else if (flag == 5) { scicos_free(*work); } else if (flag == 1) { if (get_phase_simulation() == 1) { do_cold_restart(); } pw = *work; iw = (int *) (pw + block->ipar[0] * (1 + block->insz[0])); t = get_scicos_time(); del = Min(Max(0, block->inptr[1][0]), block->rpar[0]); td = t - del; if (td < pw[*iw]) { scicos_print(_("delayed time=%f but last stored time=%f\n"), td, pw[*iw]); scicos_print(_("Consider increasing the length of buffer in variable delay block\n")); } if (t > pw[(block->ipar[0] + *iw - 1) % block->ipar[0]]) { for (j = 1; j < block->insz[0] + 1; j++) { pw[*iw + block->ipar[0]*j] = block->inptr[0][j - 1]; } pw[*iw] = t; *iw = (*iw + 1) % block->ipar[0]; } else { for (j = 1; j < block->insz[0] + 1; j++) { pw[(block->ipar[0] + *iw - 1) % block->ipar[0] + block->ipar[0]*j] = block->inptr[0][j - 1]; } pw[(block->ipar[0] + *iw - 1) % block->ipar[0]] = t; } i = 0; j = block->ipar[0] - 1; while (j - i > 1) { k = (i + j) / 2; if (td < pw[(k + *iw) % block->ipar[0]]) { j = k; } else if (td > pw[(k + *iw) % block->ipar[0]]) { i = k; } else { i = k; j = k; break; } } i = (i + *iw) % block->ipar[0]; j = (j + *iw) % block->ipar[0]; del = pw[j] - pw[i]; if (del != 0.0) { for (k = 1; k < block->insz[0] + 1; k++) { block->outptr[0][k - 1] = ((pw[j] - td) * pw[i + block->ipar[0] * k] + (td - pw[i]) * pw[j + block->ipar[0] * k]) / del; } } else { for (k = 1; k < block->insz[0] + 1; k++) { block->outptr[0][k - 1] = pw[i + block->ipar[0] * k]; } } } }
/*--------------------------------------------------------------------------*/ SCICOS_BLOCKS_IMPEXP void ratelimiter(scicos_block *block,int flag) { /* rpar[0]=rising rate limit, rpar[1]=falling rate limit */ double* pw = NULL; double rate = 0. , t = 0.; if (flag == 4) { /* the workspace is used to store previous values */ if ((*block->work = scicos_malloc(sizeof(double)*4))== NULL ) { set_block_error(-16); return; } pw=*block->work; pw[0]=0.0; pw[1]=0.0; pw[2]=0.0; pw[3]=0.0; } else if (flag == 5) { scicos_free(*block->work); } else if (flag==1) { if (get_phase_simulation()==1) do_cold_restart(); pw=*block->work; t=get_scicos_time(); if(t>pw[2]) { pw[0]=pw[2]; pw[1]=pw[3]; rate=(block->inptr[0][0]-pw[1])/(t-pw[0]); } else if(t<=pw[2]) { if(t>pw[0]) { rate=(block->inptr[0][0]-pw[1])/(t-pw[0]); } else { rate=0.0; } } if(rate>block->rpar[0]) { block->outptr[0][0]=(t-pw[0])*block->rpar[0]+pw[1]; } else if(rate<block->rpar[1]) { block->outptr[0][0]=(t-pw[0])*block->rpar[1]+pw[1]; } else { block->outptr[0][0]=block->inptr[0][0]; } pw[2]=t; pw[3]=block->outptr[0][0]; } }
void sci_mavlinkHilTracker(scicos_block *block, scicos::enumScicosFlags flag) { // data double * u=GetRealInPortPtrs(block,1); double * y=GetRealOutPortPtrs(block,1); int * ipar=block->ipar; static char * device; static int baudRate; static char ** stringArray; static int * intArray; static int count = 0; static uint16_t packet_drops = 0; //handle flags if (flag==scicos::initialize || flag==scicos::reinitialize) { if (mavlink_comm_2_port == NULL) { getIpars(1,1,ipar,&stringArray,&intArray); device = stringArray[0]; baudRate = intArray[0]; try { mavlink_comm_2_port = new BufferedAsyncSerial(device,baudRate); } catch(const boost::system::system_error & e) { Coserror((char *)e.what()); } } } else if (flag==scicos::terminate) { if (mavlink_comm_2_port) { delete mavlink_comm_2_port; mavlink_comm_2_port = NULL; } } else if (flag==scicos::updateState) { } else if (flag==scicos::computeDeriv) { } else if (flag==scicos::computeOutput) { // channel mavlink_channel_t chan = MAVLINK_COMM_2; // loop rates // TODO: clean this up to use scicos events w/ timers static int positionRate = 10; // initial times double scicosTime = get_scicos_time(); static double positionTimeStamp = scicosTime; // send global position if (scicosTime - positionTimeStamp > 1.0/positionRate) { positionTimeStamp = scicosTime; // gps double lat = u[0]*rad2deg; double lon = u[1]*rad2deg; double alt = u[2]; double vN = u[3]; double vE = u[4]; double vD = u[5]; mavlink_msg_global_position_send(chan,positionTimeStamp,lat,lon,alt,vN,vE,vD); //std::cout << "sending global position" << std::endl; } else if (scicosTime - positionTimeStamp < 0) positionTimeStamp = scicosTime; // receive messages mavlink_message_t msg; mavlink_status_t status; while(comm_get_available(MAVLINK_COMM_2)) { //std::cout << "serial available" << std::endl; uint8_t c = comm_receive_ch(MAVLINK_COMM_2); // try to get new message if(mavlink_parse_char(MAVLINK_COMM_2,c,&msg,&status)) { switch(msg.msgid) { case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: { //std::cout << "receiving messages" << std::endl; mavlink_rc_channels_scaled_t rc_channels; mavlink_msg_rc_channels_scaled_decode(&msg,&rc_channels); y[0] = rc_channels.chan1_scaled/10000.0f; y[1] = rc_channels.chan2_scaled/10000.0f; y[2] = rc_channels.chan3_scaled/10000.0f; y[3] = rc_channels.chan4_scaled/10000.0f; y[4] = rc_channels.chan5_scaled/10000.0f; y[5] = rc_channels.chan6_scaled/10000.0f; y[6] = rc_channels.chan7_scaled/10000.0f; y[7] = rc_channels.chan8_scaled/10000.0f; break; } } // update packet drop counter packet_drops += status.packet_rx_drop_count; } } } }