/** \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;
}
}
}
}
