Matrix periodic_function(Matrix* inputChannel,short numChannels){
Matrix ret=empty_matrix(1,1);
Matrix temp=empty_matrix(1,1);
int i=0;
temp=matrix_mul_double(&inputChannel[0], gs->Ch0Gain);
ret=matrix_sum(&ret,&temp);
if(numChannels>=2){
temp=matrix_mul_double(&inputChannel[1], gs->Ch1Gain);
ret=matrix_sum(&ret,&temp);
if(numChannels>=3){
temp=matrix_mul_double(&inputChannel[2], gs->Ch2Gain);
ret=matrix_sum(&ret,&temp);
for(i=3;i<numChannels;i++){
ret=matrix_sum(&ret,&inputChannel[i]);
}
}
}
ret=matrix_mul_double(&ret,gs->OutputGain);
return ret;
}
Matrix periodic_function(Matrix* inputChannel,short numChannels){
Matrix ret=empty_matrix(1,1);
ret.matrix[0][0]=fakeWave;
return ret;
}
Matrix periodic_function(Matrix* inputChannel,short numChannels){
Matrix ret=empty_matrix(1,1);
ret.matrix[0][0]=(float)blackbox_read()*4/4095;
return ret;
}
Matrix periodic_function(Matrix* inputChannel,short numChannels){
Matrix ret=empty_matrix(1,1);
ret=systemBlock(gs->B,gs->A,inputChannel[0]);
return ret;
}
Matrix systemBlock(Matrix B,Matrix A,Matrix u){
static double uA[CMAX];
static double yA[CMAX];
int i;
double aux=0;
Matrix yo=empty_matrix(1,1);
for(i=0;i<B.columns-1;i++){
uA[i]=uA[i+1];
}
uA[B.columns-1]=u.matrix[0][0];
for(i=0;i<B.columns;i++){
aux=aux+B.matrix[0][i]*uA[B.columns-1-i];
}
for(i=0;i<A.columns-1;i++){
aux=aux-A.matrix[0][i+1]*yA[A.columns-2-i];
}
for(i=0;i<A.columns-2;i++){
yA[i]=yA[i+1];
}
yA[A.columns-2]=aux;
yo.matrix[0][0]=aux;
return yo;
}
void loop(void *arg)
{
RT_HEAP sampling_heap;
current_period=bind_shm(&sampling_heap,"tickPeriod",sizeof(long));
Matrix outputMatrix=empty_matrix(1,1);
DEBUG("Outputing a %s wave of amplitude %4.2f, freq %4.2f, duty %4.2f, dc %4.2f\n",gs->wave,gs->wave_amp,gs->wave_freq,gs->wave_duty,gs->wave_dc);
while (running) {
read_inputs();
outputMatrix=periodic_function(io.input_result,io.input_num);
write_outputs(outputMatrix);
}
unbind_shm(&sampling_heap);
}
Matrix periodic_function(Matrix* inputChannel,short numChannels){
Matrix ret=empty_matrix(1,1);
double sample;
static int n=0;
long period=*current_period;
int halfpointx;
float slope;
switch(gs->wave[0]){
case 'S': case 's':
sample=sinf(2*M_PI*gs->wave_freq*n*period*pow(10,-9))*gs->wave_amp/2+gs->wave_dc;
break;
case 'T': case 't':
halfpointx=0.5/(gs->wave_freq*period*pow(10,-9));
slope=gs->wave_amp/halfpointx;
if(n<=halfpointx)
sample=slope*n-gs->wave_amp/2;
else
sample=-slope*n+(3*gs->wave_amp/2);
//wrap around
if(n>(2*halfpointx))
n=-1;
break;
case 'P': case 'p':
if(n<=(gs->wave_duty*0.01)/(gs->wave_freq*period*pow(10,-9)))
sample=gs->wave_dc+gs->wave_amp/2;
else
sample=gs->wave_dc-gs->wave_amp/2;
//wrap around
if(n>(1/(gs->wave_freq*period*pow(10,-9))))
n=-1;
break;
}
n++;
ret.matrix[0][0]=sample;
return ret;
}
void loop(void *arg){
Matrix outputMatrix=empty_matrix(1,1);
RT_TASK pwm_task;
lpt_init();
rt_task_spawn(&pwm_task,NULL,0,gs->task_prio,0,&pwm,NULL);
while (running) {
read_inputs();
//outputMatrix=periodic_function(io.input_result,io.input_num);
write_outputs(outputMatrix);
}
rt_task_delete(&pwm_task);
lpt_close();
}
void loop(void *arg){
RT_HEAP sampling_heap;
Matrix outputMatrix=empty_matrix(1,1);
long* current_period;
int create;
//This works as a global variable
current_period=(long*) create_shm(&sampling_heap,"tickPeriod",sizeof(long),&create);
*current_period=gs->sampling_period*1000;
if(*current_period<SAFEZONE)
*current_period=SAFEZONE;
rt_task_set_periodic(NULL, TM_NOW,*current_period);
while(running){
rt_task_wait_period(NULL);
debug_get_start_time();
//XL forces this block to have inputs,
//so we force the count to zero to ignore it
io.input_num = 0;
debug_store_inputs();
write_output_queues(&outputMatrix);
debug_write_queue();
outputMatrix.matrix[0][0]++;
//Change period if changed in GUI
if(*current_period!=gs->sampling_period*1000){
*current_period=gs->sampling_period*1000;
if(*current_period<SAFEZONE)
*current_period=SAFEZONE;
rt_task_set_periodic(NULL, TM_NOW,*current_period);
}
}
delete_shm(&sampling_heap,current_period);
}
void loop(void *arg){
Matrix outputMatrix=empty_matrix(1,1);
/*
* Insert initialization code here.
* e.g. open a file.
*/
while (running) {
read_inputs();
outputMatrix=periodic_function(io.input_result,io.input_num);
write_outputs(outputMatrix);
}
/*
* Insert finalization code here
* e.g. close a file.
*/
}
void my_main( int polygons ) {
int i;
double step = .05;
double xval, yval, zval;
struct matrix *transform;
struct matrix *tmp;
struct stack *s;
screen t;
color g;
g = change_color(4);
s = new_stack();
tmp = new_matrix(4, 1000);
clear_screen( t );
for (i=0;i<lastop;i++) {
switch (op[i].opcode) {
case COMMENT:
break;
case PUSH:
push(s);
break;
case POP:
pop(s);
break;
case MOVE:
xval = op[i].op.move.d[0];
yval = op[i].op.move.d[1];
zval = op[i].op.move.d[2];
transform = make_translate(xval, yval, zval);
matrix_mult(s->data[s->top], transform);
s->data[s->top] = transform;
break;
case SCALE:
xval = op[i].op.scale.d[0];
yval = op[i].op.scale.d[1];
zval = op[i].op.scale.d[2];
transform - make_scale(xval, yval, zval);
matrix_mult(s->data[s->top], transform);
s->data[s->top] = transform;
break;
case ROTATE:
switch ((int)op[i].op.rotate.axis)
{
double theta = op[i].op.rotate.degrees;
case 0:
transform = make_rotX(theta);
break;
case 1:
transform = make_rotY(theta);
break;
case 2:
transform = make_rotZ(theta);
break;
}
matrix_mult(s->data[s->top], transform);
s->data[s->top] = transform;
break;
case BOX:
empty_matrix(tmp);
xval = op[i].op.box.d0[0];
yval = op[i].op.box.d0[1];
zval = op[i].op.box.d0[2];
double width = op[i].op.box.d1[0];
double height = op[i].op.box.d1[1];
double depth = op[i].op.box.d1[2];
add_box(tmp, xval, yval, zval, width, height, depth);
matrix_mult(s->data[s->top], tmp);
draw_polygons(tmp, t, g);
break;
case SPHERE:
empty_matrix(tmp);
xval = op[i].op.sphere.d[0];
yval = op[i].op.sphere.d[1];
zval = op[i].op.sphere.d[2];
double radius = op[i].op.sphere.r;
add_sphere(tmp, xval, yval, zval, radius, step);
matrix_mult(s->data[s->top], tmp);
draw_polygons(tmp, t, g);
break;
case TORUS:
empty_matrix(tmp);
xval = op[i].op.torus.d[0];
yval = op[i].op.torus.d[1];
zval = op[i].op.torus.d[2];
double r1 = op[i].op.torus.r0;
double r2 = op[i].op.torus.r1;
add_torus(tmp, xval, yval, zval, r1, r2, step);
matrix_mult(s->data[s->top], tmp);
draw_polygons(tmp, t, g);
break;
case LINE:
empty_matrix(tmp);
add_edge(tmp,
op[i].op.line.p0[0],
op[i].op.line.p0[1],
op[i].op.line.p0[2],
op[i].op.line.p1[0],
op[i].op.line.p1[1],
op[i].op.line.p1[2]);
draw_lines(tmp, t, g);
break;
case SAVE:
save_extension(t, op[i].op.save.p->name);
break;
case DISPLAY:
display(t);
break;
}
}
}
static void
read_box(FILE *fob, mxArray **data, double dbu_to_uu)
{
mxArray *pstruct;
mxArray *pprop = NULL;
uint16_t rtype, rlen;
int nprop = 0;
element_t box;
const char *fields[] = {"internal", "xy", "prop"};
/* initialize element */
init_element(&box, GDS_BOX);
/* output data structure */
pstruct = mxCreateStructMatrix(1,1, 3, fields);
/* read element properties */
while (1) {
if ( read_record_hdr(fob, &rtype, &rlen) )
mexErrMsgTxt("gds_read_element (box) : could not read record header.");
if (rtype == ENDEL)
break;
switch (rtype) {
case XY:
mxSetFieldByNumber(pstruct, 0, 1, read_xy(fob, rlen, dbu_to_uu));
break;
case LAYER:
box.layer = read_layer(fob);
break;
case BOXTYPE:
box.dtype = read_type(fob);
break;
case ELFLAGS:
box.elflags = read_elflags(fob);
box.has |= HAS_ELFLAGS;
break;
case PLEX:
box.plex = read_plex(fob);
box.has |= HAS_PLEX;
break;
case PROPATTR:
pprop = resize_property_structure(pprop, nprop+1);
mxSetFieldByNumber(pprop, nprop, 0, read_propattr(fob));
break;
case PROPVALUE:
mxSetFieldByNumber(pprop, nprop, 1, read_propvalue(fob,rlen));
nprop += 1;
break;
default:
mexPrintf("Unknown record id: 0x%x\n", rtype);
mexErrMsgTxt("BOX : found unknown element property.");
}
}
/* set prop field */
if ( nprop ) {
mxSetFieldByNumber(pstruct, 0, 2, pprop);
}
else {
mxSetFieldByNumber(pstruct, 0, 2, empty_matrix());
}
/* store structure with element data */
mxSetFieldByNumber(pstruct, 0, 0, copy_element_to_array(&box));
/* return data */
*data = pstruct;
}
static void
read_text(FILE *fob, mxArray **data, double dbu_to_uu)
{
mxArray *pstruct;
mxArray *pprop = NULL;
uint16_t rtype, rlen;
int nprop = 0;
char tstr[TXTLEN+4];
element_t text;
const char *fields[] = {"internal", "xy", "prop", "text"};
/* initialize element */
init_element(&text, GDS_TEXT);
/* output data structure */
pstruct = mxCreateStructMatrix(1,1, 4, fields);
/* read element properties */
while (1) {
if ( read_record_hdr(fob, &rtype, &rlen) )
mexErrMsgTxt("gds_read_element (text) : could not read record header.");
if (rtype == ENDEL)
break;
switch (rtype) {
case STRING:
if ( read_string(fob, tstr, rlen) )
mexErrMsgTxt("gds_read_element (text) : could not read string.");
struct_set_string(pstruct, 3, tstr);
break;
case TEXTTYPE:
text.dtype = read_type(fob);
break;
case XY:
mxSetFieldByNumber(pstruct, 0, 1, read_xy(fob, rlen, dbu_to_uu));
break;
case LAYER:
text.layer = read_layer(fob);
break;
case PATHTYPE:
text.ptype = read_type(fob);
text.has |= HAS_PTYPE;
break;
case WIDTH:
text.width = dbu_to_uu * read_width(fob);
text.has |= HAS_WIDTH;
break;
case PRESENTATION:
if ( read_word(fob, &text.present) )
mexErrMsgTxt("gds_read_element (text) : could not read presentation data.");
text.has |= HAS_PRESTN;
break;
case STRANS:
if ( read_word(fob, &text.strans.flags) )
mexErrMsgTxt("gds_read_element (text) : could not read strans data.");
text.has |= HAS_STRANS;
break;
case MAG:
if ( read_real8(fob, &text.strans.mag) )
mexErrMsgTxt("gds_read_element (text) : could not read magnification.");
text.has |= HAS_MAG;
break;
case ANGLE:
if ( read_real8(fob, &text.strans.angle) )
mexErrMsgTxt("gds_read_element (text) : could not read angle.");
text.has |= HAS_ANGLE;
break;
case ELFLAGS:
text.elflags = read_elflags(fob);
text.has |= HAS_ELFLAGS;
break;
case PLEX:
text.plex = read_plex(fob);
text.has |= HAS_PLEX;
break;
case PROPATTR:
pprop = resize_property_structure(pprop, nprop+1);
mxSetFieldByNumber(pprop, nprop, 0, read_propattr(fob));
break;
case PROPVALUE:
mxSetFieldByNumber(pprop, nprop, 1, read_propvalue(fob,rlen));
nprop += 1;
break;
default:
mexPrintf("Unknown record id: 0x%x\n", rtype);
mexErrMsgTxt("TEXT : found unknown element property.");
}
}
/* set prop field */
if ( nprop ) {
mxSetFieldByNumber(pstruct, 0, 2, pprop);
}
else {
mxSetFieldByNumber(pstruct, 0, 2, empty_matrix());
}
/* store structure with element data */
mxSetFieldByNumber(pstruct, 0, 0, copy_element_to_array(&text));
/* return data */
*data = pstruct;
}
static void
read_aref(FILE *fob, mxArray **data, double dbu_to_uu)
{
mxArray *pstruct;
mxArray *pprop = NULL;
uint16_t rtype, rlen;
int nprop = 0;
element_t aref;
const char *fields[] = {"internal", "xy", "prop"};
/* initialize element */
init_element(&aref, GDS_AREF);
/* output data structure */
pstruct = mxCreateStructMatrix(1,1, 3, fields);
/* read element properties */
while (1) {
if ( read_record_hdr(fob, &rtype, &rlen) )
mexErrMsgTxt("gds_read_element (aref) : could not read record header.");
if (rtype == ENDEL)
break;
switch (rtype) {
case XY:
mxSetFieldByNumber(pstruct, 0, 1, read_xy(fob, rlen, dbu_to_uu));
break;
case SNAME:
if ( read_string(fob, aref.sname, rlen) )
mexErrMsgTxt("gds_read_element (sref) : could not read structure name.");
break;
case COLROW:
read_colrow(fob, &aref.nrow, &aref.ncol);
break;
case STRANS:
if ( read_word(fob, &aref.strans.flags) )
mexErrMsgTxt("gds_read_element (aref) : could not read strans data.");
aref.has |= HAS_STRANS;
break;
case MAG:
if ( read_real8(fob, &aref.strans.mag) )
mexErrMsgTxt("gds_read_element (aref) : could not read magnification.");
aref.has |= HAS_MAG;
break;
case ANGLE:
if ( read_real8(fob, &aref.strans.angle) )
mexErrMsgTxt("gds_read_element (aref) : could not read angle.");
aref.has |= HAS_ANGLE;
break;
case ELFLAGS:
aref.elflags = read_elflags(fob);
aref.has |= HAS_ELFLAGS;
break;
case PLEX:
aref.plex = read_plex(fob);
aref.has |= HAS_PLEX;
break;
case PROPATTR:
pprop = resize_property_structure(pprop, nprop+1);
mxSetFieldByNumber(pprop, nprop, 0, read_propattr(fob));
break;
case PROPVALUE:
mxSetFieldByNumber(pprop, nprop, 1, read_propvalue(fob,rlen));
nprop += 1;
break;
default:
mexPrintf("Unknown record id: 0x%x\n", rtype);
mexErrMsgTxt("AREF : found unknown element property.");
}
}
/* set prop field */
if ( nprop ) {
mxSetFieldByNumber(pstruct, 0, 2, pprop);
}
else {
mxSetFieldByNumber(pstruct, 0, 2, empty_matrix());
}
/* store structure with element data */
mxSetFieldByNumber(pstruct, 0, 0, copy_element_to_array(&aref));
/* return data */
*data = pstruct;
}
static void
read_sref(FILE *fob, mxArray **data, double dbu_to_uu)
{
mxArray *pstruct;
mxArray *pprop = NULL;
mxArray *pa;
double *pd;
tList xylist;
uint16_t rtype, rlen;
int nprop = 0;
int mtotal = 0;
int k, m, nle;
element_t sref;
const char *fields[] = {"internal", "xy", "prop"};
xy_block vertex;
/* initialize element */
init_element(&sref, GDS_SREF);
/* output data structure */
pstruct = mxCreateStructMatrix(1,1, 3, fields);
/* create a list for the XY data record(s) */
if ( create_list(&xylist) == -1 )
mexErrMsgTxt("gds_read_element (sref) : could not create list for XY records.");
/* read element properties */
while (1) {
if ( read_record_hdr(fob, &rtype, &rlen) )
mexErrMsgTxt("gds_read_element (sref) : could not read record header.");
if (rtype == ENDEL)
break;
switch (rtype) {
case XY:
m = rlen / (2*sizeof(int32_t));
vertex.mxy = m;
vertex.xy = read_xy2(fob, m, dbu_to_uu);
list_insert_object(xylist, &vertex, sizeof(xy_block), AFTER);
mtotal += m;
break;
case SNAME:
if ( read_string(fob, sref.sname, rlen) )
mexErrMsgTxt("gds_read_element (sref) : could not read structure name.");
break;
case STRANS:
if ( read_word(fob, &sref.strans.flags) )
mexErrMsgTxt("gds_read_element (sref) : could not read strans data.");
sref.has |= HAS_STRANS;
break;
case MAG:
if ( read_real8(fob, &sref.strans.mag) )
mexErrMsgTxt("gds_read_element (sref) : could not read magnification.");
sref.has |= HAS_MAG;
break;
case ANGLE:
if ( read_real8(fob, &sref.strans.angle) )
mexErrMsgTxt("gds_read_element (sref) : could not read angle.");
sref.has |= HAS_ANGLE;
break;
case ELFLAGS:
sref.elflags = read_elflags(fob);
sref.has |= HAS_ELFLAGS;
break;
case PLEX:
sref.plex = read_plex(fob);
sref.has |= HAS_PLEX;
break;
case PROPATTR:
pprop = resize_property_structure(pprop, nprop+1);
mxSetFieldByNumber(pprop, nprop, 0, read_propattr(fob));
break;
case PROPVALUE:
mxSetFieldByNumber(pprop, nprop, 1, read_propvalue(fob,rlen));
nprop += 1;
break;
default:
mexPrintf("Unknown record id: 0x%x\n", rtype);
mexErrMsgTxt("SREF : found unknown element property.");
}
}
/* catenate XY records */
nle = list_entries(xylist);
if ( !nle )
mexErrMsgTxt("gds_read_element (sref) : element has no XY record.");
pa = mxCreateDoubleMatrix(mtotal,2, mxREAL);
pd = mxGetData(pa);
list_head(xylist);
for (k=0; k<nle; k++) {
get_current_object(xylist, &vertex, sizeof(xy_block));
memcpy(pd, vertex.xy, 2*vertex.mxy*sizeof(double));
pd += 2*vertex.mxy;
mxFree(vertex.xy);
}
mxSetFieldByNumber(pstruct, 0, 1, pa);
erase_list_entries(xylist);
delete_list(&xylist);
/* set prop field */
if ( nprop ) {
mxSetFieldByNumber(pstruct, 0, 2, pprop);
}
else {
mxSetFieldByNumber(pstruct, 0, 2, empty_matrix());
}
/* store structure with element data */
mxSetFieldByNumber(pstruct, 0, 0, copy_element_to_array(&sref));
/* return data */
*data = pstruct;
}
static void
read_path(FILE *fob, mxArray **data, double dbu_to_uu)
{
mxArray *pstruct;
mxArray *pprop = NULL;
mxArray *pc;
tList xylist;
uint16_t rtype, rlen;
int nprop = 0;
int nle, k;
element_t path;
const char *fields[] = {"internal", "xy", "prop"};
/* initialize element */
init_element(&path, GDS_PATH);
/* output data structure */
pstruct = mxCreateStructMatrix(1,1, 3, fields);
/* create a list for the XY data record(s) */
if ( create_list(&xylist) == -1 )
mexErrMsgTxt("gds_read_element (path) : could not create list for XY records.");
/* read element properties */
while (1) {
if ( read_record_hdr(fob, &rtype, &rlen) )
mexErrMsgTxt("gds_read_element (path) : could not read record header.");
if (rtype == ENDEL)
break;
switch (rtype) {
case XY:
if ( list_insert(xylist, read_xy(fob, rlen, dbu_to_uu), AFTER) == -1)
mexErrMsgTxt("gds_read_element (path) : list insertion failed.");
break;
case LAYER:
path.layer = read_layer(fob);
break;
case PATHTYPE:
path.ptype = read_type(fob);
path.has |= HAS_PTYPE;
break;
case WIDTH:
path.width = dbu_to_uu * (double)read_width(fob);
path.has |= HAS_WIDTH;
break;
case BGNEXTN:
path.bgnextn = dbu_to_uu * read_extn(fob);
path.has |= HAS_BGNEXTN;
break;
case ENDEXTN:
path.endextn = dbu_to_uu * read_extn(fob);
path.has |= HAS_ENDEXTN;
break;
case DATATYPE:
path.dtype = read_type(fob);
break;
case ELFLAGS:
path.elflags = read_elflags(fob);
path.has |= HAS_ELFLAGS;
break;
case PLEX:
path.plex = read_plex(fob);
path.has |= HAS_PLEX;
break;
case PROPATTR:
pprop = resize_property_structure(pprop, nprop+1);
mxSetFieldByNumber(pprop, nprop, 0, read_propattr(fob));
break;
case PROPVALUE:
mxSetFieldByNumber(pprop, nprop, 1, read_propvalue(fob,rlen));
nprop += 1;
break;
default:
mexPrintf("Unknown record id: 0x%x\n", rtype);
mexErrMsgTxt("PATH : found unknown element property.");
}
}
/* cell array with XY records */
nle = list_entries(xylist);
if ( !nle )
mexErrMsgTxt("gds_read_element (path) : element has no XY record.");
pc = mxCreateCellMatrix(1, nle);
list_head(xylist);
for (k=0; k<nle; k++)
mxSetCell(pc, k, (mxArray *)get_current_entry(xylist, NULL));
mxSetFieldByNumber(pstruct, 0, 1, pc);
/* set prop field */
if ( nprop ) {
mxSetFieldByNumber(pstruct, 0, 2, pprop);
}
else {
mxSetFieldByNumber(pstruct, 0, 2, empty_matrix());
}
/* store structure with element data */
mxSetFieldByNumber(pstruct, 0, 0, copy_element_to_array(&path));
/* return data */
*data = pstruct;
}
