/** \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 canimxy3d(scicos_block * block, scicos_flag flag) { char const* pFigureUID; sco_data *sco; int 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; } appendData(block, block->inptr[0], block->inptr[1], block->inptr[2]); for (j = 0; j < block->insz[0]; j++) { result = pushData(block, j); if (result == FALSE) { Coserror("%s: unable to push some data.", "cscopxy3d"); break; } } break; case Ending: freeScoData(block); break; default: break; } }
/** \fn void cmatview(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 cmatview(scicos_block * block, scicos_flag flag) { char const* pFigureUID; double *u; sco_data *sco; 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; } u = GetRealInPortPtrs(block, 1); result = pushData(block, u); if (result == FALSE) { Coserror("%s: unable to push some data.", "cmatview"); break; } break; case Ending: freeScoData(block); break; default: break; } }
/** \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 cscopxy3d(scicos_block * block, scicos_flag flag) { int iFigureUID; sco_data *sco; int j; 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; case StateUpdate: iFigureUID = getFigure(block); if (iFigureUID == 0) { // allocation error set_block_error(-5); break; } appendData(block, GetRealInPortPtrs(block, 1), GetRealInPortPtrs(block, 2), GetRealInPortPtrs(block, 3)); for (j = 0; j < block->insz[0]; j++) { result = pushData(block, j); if (result == FALSE) { Coserror("%s: unable to push some data.", "cscopxy3d"); break; } } break; case Ending: freeScoData(block); break; default: break; } }
/** \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; } } } }
void sci_zeroOrderHold(scicos_block *block,scicos::enumScicosFlags flag) { // get block data pointers, etc double *_z=GetDstate(block); double *_u1=GetRealInPortPtrs(block,1); double *_u2=GetRealInPortPtrs(block,2); double *_y1=GetRealOutPortPtrs(block,1); int *_ipar=GetIparPtrs(block); int & evtFlag = GetNevIn(block); int & evtPortTime = _ipar[0]; int & evtPortReset = _ipar[1]; // compute flags int evtFlagTime = scicos::evtPortNumToFlag(evtPortTime); int evtFlagReset = scicos::evtPortNumToFlag(evtPortReset); // loop over all rows of data int i,j; int nRows = GetInPortRows(block,1); int nCols = GetInPortCols(block,1); size_t nBytes = sizeof(double)*nRows*nCols; for(i=0; i<nRows; i++) { for(j=0; j<nCols; j++) { if (flag ==scicos::computeOutput || flag ==scicos::reinitialize || flag ==scicos::initialize) memcpy(_y1,_z,nBytes); else if (flag == scicos::updateState) { // bitwise comparison for flag if(evtFlag & evtFlagReset && _u2) { memcpy(_z,_u2,nBytes); } else if(evtFlag & evtFlagTime && _u1) { memcpy(_z,_u1,nBytes); } else { printf("\nunhandled event flat %d\n",evtFlag); printf("\nknown flags:\n"); printf("\ttime flag: %d\n",evtFlagTime); printf("\ttime flag & event flag: %d\n",evtFlagTime & evtFlag); printf("\treset flag: %d\n",evtFlagReset); printf("\treset flag & event flag: %d\n",evtFlagReset & evtFlag); } } else if (flag == scicos::terminate) { } else { char msg[50]; sprintf(msg,"unhandled block flag %d\n",flag); Coserror(msg); } } } }
/** \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 mat_inv(scicos_block * block, int flag) { double *u = NULL; double *y = NULL; int nu = 0; int info = 0; int i = 0; mat_inv_struct** work = (mat_inv_struct**) block->work; mat_inv_struct *ptr = NULL; nu = GetInPortRows(block, 1); u = GetRealInPortPtrs(block, 1); y = GetRealOutPortPtrs(block, 1); /*init : initialization */ if (flag == 4) { if ((*work = (mat_inv_struct *) scicos_malloc(sizeof(mat_inv_struct))) == NULL) { set_block_error(-16); return; } ptr = *work; if ((ptr->ipiv = (int *)scicos_malloc(sizeof(int) * nu)) == NULL) { set_block_error(-16); scicos_free(ptr); return; } if ((ptr->dwork = (double *)scicos_malloc(sizeof(double) * nu)) == NULL) { set_block_error(-16); scicos_free(ptr->ipiv); scicos_free(ptr); return; } } /* Terminaison */ else if (flag == 5) { ptr = *work; if ((ptr->dwork) != NULL) { scicos_free(ptr->ipiv); scicos_free(ptr->dwork); scicos_free(ptr); return; } } else { ptr = *work; for (i = 0; i < (nu * nu); i++) { y[i] = u[i]; } C2F(dgetrf) (&nu, &nu, &y[0], &nu, ptr->ipiv, &info); if (info != 0) { if (flag != 6) { Coserror(_("The LU factorization has been completed, but the factor U is exactly singular : U(%d,%d) is exactly zero."), info, info); return; } } C2F(dgetri) (&nu, y, &nu, ptr->ipiv, ptr->dwork, &nu, &info); } }
void sci_geoMag(scicos_block *block, scicos::enumScicosFlags flag) { static mavsim::GeoMag* geoMag = NULL; // constants // data double * u1=(double*)GetInPortPtrs(block,1); double * y1=(double*)GetOutPortPtrs(block,1); double * rpar=block->rpar; int * ipar=block->ipar; // aliases // double & lat = u1[0]; double & lon = u1[1]; double & alt = u1[2]; double & dip = y1[0]; double & dec = y1[1]; double & H0 = y1[2]; double & decYear = rpar[0]; int & nTerms = ipar[0]; //make sure you have initialized the block if(!geoMag && flag!=scicos::initialize) { sci_geoMag(block,scicos::initialize); } //handle flags if (flag==scicos::initialize || flag==scicos::reinitialize) { std::cout << "initializing" << std::endl; if (!geoMag) { try { geoMag = new mavsim::GeoMag("DATADIR/WMM.COF",nTerms); } catch (const std::runtime_error & e) { Coserror((char *)e.what()); } } } else if (flag==scicos::terminate) { std::cout << "terminating" << std::endl; if (geoMag) { delete geoMag; geoMag = NULL; } } else if (flag==scicos::updateState) { //std::cout << "updating state" << std::endl; } else if (flag==scicos::computeOutput) { //std::cout << "computing Output" << std::endl; if(geoMag) { geoMag->update(lat*180/M_PI,lon*180/M_PI,alt,decYear); dip = geoMag->dip*M_PI/180; dec = geoMag->dec*M_PI/180; H0 = geoMag->ti; } } else { std::cout << "unhandled flag: " << flag << std::endl; } }
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; } } } }