struct psfex *psfex_new(long *masksize, // [MASK_DIM]
long poldeg,
double *contextoffset, // [POLY_DIM]
double *contextscale, // [POLY_DIM]
double psf_samp)
{
struct psfex *self;
int i;
long deg[POLY_MAXDIM], group[POLY_MAXDIM];
if ((self = calloc(1, sizeof(struct psfex))) == NULL) {
fprintf(stderr,"Failed to allocate struct psfex\n");
exit(1);
}
self->maskcomp = NULL;
// copy masksize
memcpy(self->masksize, masksize, MASK_DIM * sizeof(long));
// copy contextoffset
memcpy(self->contextoffset, contextoffset, POLY_DIM * sizeof(double));
// copy contextscale
memcpy(self->contextscale, contextscale, POLY_DIM * sizeof(double));
self->masknpix = self->masksize[0] * self->masksize[1];
// set up poly struct
for (i=0;i<POLY_DIM;i++) {
group[i] = 1;
}
deg[0] = poldeg;
self->poly = poly_init(group, POLY_DIM, deg, POLY_NGROUP);
self->pixstep = 1./psf_samp;
// allocate memory for mask ...
if ((self->maskcomp = (double *) calloc(self->masknpix * self->masksize[2], sizeof(double))) == NULL) {
self=psfex_free(self);
fprintf(stderr,"Failed to allocate maskcomp\n");
exit(1);
}
// and set the reconstruction size, using psf sampling factor. Make sure it's odd
self->reconsize[0] = (long) ceil((float) self->masksize[0] * (float) psf_samp);
if ((self->reconsize[0] % 2) == 0) { self->reconsize[0]++; }
self->reconsize[1] = (long) ceil((float) self->masksize[1] * (float) psf_samp);
if ((self->reconsize[1] % 2) == 0) { self->reconsize[1]++; }
return self;
}
int main(int argc, char **argv)
{
// 16-bit, stereo, 44.1KHz, 1 generator, no file to dump to.
poly_init(16,2,44100,1,NULL);
printf("Usage: wavetest [freq=440] [tap=6] [size=15] [seed=1] [mult=2]\n");
unsigned int size = 15;
unsigned int tap = 6;
unsigned int seed = 5;
unsigned int mult = 2;
float freq = 440.0;
if (argc > 1)
{
freq = (float)atoi(argv[1]);
printf("Using freq %f\n",freq);
}
poly_init_generator(0,poly_noise,1.0,freq);
if (argc > 2)
{
tap = atoi(argv[2]);
printf("Using tap bit %d\n",tap);
}
if (argc > 3)
{
size = atoi(argv[3]);
printf("Using size %d\n",size);
}
if (argc > 4)
{
seed = atoi(argv[4]);
printf("Using seed %d\n",seed);
}
if (argc > 5)
{
mult = atoi(argv[5]);
printf("Using mult %d\n",mult);
}
poly_seed_noise(0,seed);
poly_set_noise_tap(0,tap);
poly_set_noise_size(0,size);
poly_set_noise_mult(0,mult);
poly_start();
printf("Strike enter to exit.\n");
scanf("uh");
return 0;
}
int
main(void)
{
int ret;
poly_init();
ret = test_bn_poly_qua_rts();
if (ret == 0) {
bu_log("\nFunction computes correctly\n");
return ret;
} else
exit(EXIT_FAILURE);
return 0;
}
Hint poly_crc (void * list_ptr, Huint size)
{
Hint result = SUCCESS;
// Use Accelerator?
Hbool use_accelerator = poly_init(CRC, size);
// Start transferring data to BRAM
if(transfer_dma( (void *) list_ptr, (void *) ACC_BRAMC, size *4))
return FAILURE;
if (use_accelerator) {
int e = 0;
putfslx( size, 0, FSL_DEFAULT);//send end address
putfslx( 0, 0, FSL_DEFAULT); //send start address
getfslx(e, 0, FSL_DEFAULT);
if (e != 1) return FAILURE;
} else {
Huint vhwti_base = 0;
// Get VHWTI from PVRs
getpvr(1,vhwti_base);
hthread_time_t start = hthread_time_get();
// Run crc in software
result = (sw_crc((void *) ACC_BRAMC, size));
hthread_time_t stop = hthread_time_get();
hthread_time_t diff;
hthread_time_diff(diff, stop,start);
volatile hthread_time_t * ptr = (hthread_time_t *) (vhwti_base + 0x100);
*ptr += diff;
}
// Start transferring data from BRAM
if(transfer_dma( (void *) ACC_BRAMC, (void *) list_ptr, size *4))
return FAILURE;
return result;
}
int sporth_poly(sporth_stack *stack, void *ud)
{
plumber_data *pd = ud;
sporth_poly_d *poly;
poly_voice *voice;
poly_event *evt;
uint32_t nvoices;
char *ftname;
char *file;
uint32_t n, p;
int id;
switch(pd->mode) {
case PLUMBER_CREATE:
#ifdef DEBUG_MODE
fprintf(stderr, "poly: Creating\n");
#endif
poly = malloc(sizeof(sporth_poly_d));
plumber_add_ugen(pd, SPORTH_POLY, poly);
if(sporth_check_args(stack, "ffss") != SPORTH_OK) {
fprintf(stderr,"Invalid arguments for poly\n");
stack->error++;
return PLUMBER_NOTOK;
}
ftname = sporth_stack_pop_string(stack);
file = sporth_stack_pop_string(stack);
poly->max_params = (uint32_t)sporth_stack_pop_float(stack);
poly->max_voices = (uint32_t)sporth_stack_pop_float(stack);
poly->dur = malloc(sizeof(uint32_t) * poly->max_voices);
poly_init(&poly->poly);
if(poly_binary_parse(&poly->poly, file, pd->sp->sr) != 0) {
fprintf(stderr, "Could not read file %s\n", file);
stack->error++;
return PLUMBER_NOTOK;
}
poly_end(&poly->poly);
poly_cluster_init(&poly->clust, poly->max_voices);
sp_ftbl_create(pd->sp, &poly->ft,
1 + poly->max_voices * (2 + poly->max_params));
memset(poly->ft->tbl, 0, poly->ft->size * sizeof(SPFLOAT));
poly->ft->tbl[0] = poly->max_params;
plumber_ftmap_add(pd, ftname, poly->ft);
free(ftname);
free(file);
break;
case PLUMBER_INIT:
#ifdef DEBUG_MODE
fprintf(stderr, "poly: Initialising\n");
#endif
ftname = sporth_stack_pop_string(stack);
file = sporth_stack_pop_string(stack);
poly->max_params = (uint32_t)sporth_stack_pop_float(stack);
poly->max_voices = (uint32_t)sporth_stack_pop_float(stack);
free(ftname);
free(file);
break;
case PLUMBER_COMPUTE:
sporth_stack_pop_float(stack);
sporth_stack_pop_float(stack);
poly = pd->last->ud;
poly_compute(&poly->poly);
for(n = 0; n < poly->max_voices; n++) {
poly->ft->tbl[1 + n * (poly->max_params + 2)] = 0.0;
}
poly_itr_reset(&poly->poly);
for(n = 0; n < poly_nevents(&poly->poly); n++) {
evt = poly_itr_next(&poly->poly);
if(!poly_cluster_add(&poly->clust, &id)) {
poly->dur[id] = evt->p[0] * pd->sp->sr;
poly->ft->tbl[1 + id * (poly->max_params + 2)] = 1.0;
poly->ft->tbl[2 + id * (poly->max_params + 2)] = evt->p[0];
for(p = 1; p < evt->nvals; p++) {
poly->ft->tbl[2 + id * (poly->max_params + 2) + p] = evt->p[p];
}
}
}
poly_cluster_reset(&poly->clust);
nvoices = poly_cluster_nvoices(&poly->clust);
for(n = 0; n < nvoices; n++) {
voice = poly_next_voice(&poly->clust);
poly->dur[voice->val] -= 1;
}
poly_cluster_reset(&poly->clust);
for(n = 0; n < nvoices; n++) {
voice = poly_next_voice(&poly->clust);
if(poly->dur[voice->val] <= 0) {
poly_cluster_remove(&poly->clust, voice->val);
}
}
break;
case PLUMBER_DESTROY:
poly = pd->last->ud;
poly_cluster_destroy(&poly->clust);
poly_destroy(&poly->poly);
free(poly->dur);
free(poly);
break;
default:
fprintf(stderr, "poly: Uknown mode!\n");
break;
}
return PLUMBER_OK;
}
static void trainSetSpeed(const int speed, const int stopTime, const int delayer, Driver* me) {
char msg[4];
msg[1] = (char)me->trainNum;
if (me->lastSensorActualTime > 0) {
// a/d related stuff
int newSpeed = speed >=0 ? speed : 0;
int now = Time(me->timeserver) * 10;
if (me->speed == newSpeed) {
// do nothing
}
else if (me->speed == 0) {
// accelerating from 0
int v0 = getVelocity(me);
int v1 = me->v[newSpeed][ACCELERATE];
int t0 = now + 8; // compensate for time it takes to send to train
int t1 = now + 8 + me->a[newSpeed];
poly_init(&me->adPoly, t0, t1, v0, v1);
me->isAding = 1;
me->lastReportDist = 0;
me->adEndTime = t1;
} else if (newSpeed == 0) {
// decelerating to 0
int v0 = getVelocity(me);
int v1 = me->v[newSpeed][DECELERATE];
int t0 = now + 8; // compensate for time it takes to send to train
int t1 = now + 8 + getStoppingTime(me);
poly_init(&me->adPoly, t0, t1, v0, v1);
me->isAding = 1;
me->lastReportDist = 0;
me->adEndTime = t1;
}
}
TrainDebug(me, "Train Setting Speed %d", speed);
if (speed >= 0) {
if (delayer) {
TrainDebug(me, "Reversing speed.------- %d", speed);
msg[0] = 0xf;
msg[1] = (char)me->trainNum;
msg[2] = (char)speed;
msg[3] = (char)me->trainNum;
Putstr(me->com1, msg, 4);
//TrainDebug(me, "Next Sensor: %d %d", me->nextSensorIsTerminal, me->lastSensorIsTerminal);
// Update prediction
if (me->nextSensorIsTerminal) {
me->nextSensorBox = me->nextSensorBox == EX ? EN : EX;
//TrainDebug(me, "LAst Sensor: %d ", me->lastSensorVal);
} else {
int action = me->nextSensorVal%2 == 1 ? 1 : -1;
me->nextSensorVal = me->nextSensorVal + action;
}
if (me->lastSensorIsTerminal) {
me->lastSensorBox = me->lastSensorBox == EX ? EN : EX;
} else {
int action = me->lastSensorVal%2 == 1 ? 1 : -1;
me->lastSensorVal = me->lastSensorVal + action;
}
float distTemp = me->distanceFromLastSensor;
me->distanceFromLastSensor = me->distanceToNextSensor;
me->distanceToNextSensor = distTemp;
char valTemp = me->nextSensorVal;
me->nextSensorVal = me->lastSensorVal;
me->lastSensorVal = valTemp;
char boxTemp = me->nextSensorBox;
me->nextSensorBox = me->lastSensorBox;
me->lastSensorBox = boxTemp;
if (me->nextSensorIsTerminal || me->lastSensorIsTerminal){
char isTemp = me->nextSensorIsTerminal;
me->nextSensorIsTerminal = me->lastSensorIsTerminal;
me->lastSensorIsTerminal = isTemp;
}
// Reserve the track above train and future (covers case of init)
// Update prediction
updatePrediction(me);
int reserveStatus = reserveMoreTrack(me, 0, me->d[speed][ACCELERATE][MAX_VAL]); // moving
if (reserveStatus == RESERVE_FAIL) {
reroute(me);
}
} else {
//TrainDebug(me, "Set speed. %d %d", speed, me->trainNum);
msg[0] = (char)speed;
Putstr(me->com1, msg, 2);
if (speed == 0) {
int delayTime = stopTime + 500;
Reply(me->stopDelayer, (char*)&delayTime, 4);
}
}
if (speed > me->speed) {
me->speedDir = ACCELERATE;
} else if (speed < me->speed) {
me->speedDir = DECELERATE;
}
me->speed = speed;
} else {
//TrainDebug(me, "Reverse... %d ", me->speed);
DriverMsg delayMsg;
delayMsg.type = SET_SPEED;
delayMsg.timestamp = stopTime + 500;
if (me->speedAfterReverse == -1) {
delayMsg.data2 = (signed char)me->speed;
} else {
delayMsg.data2 = (signed char)me->speedAfterReverse;
}
//TrainDebug(me, "Using delayer: %d for %d", me->delayer, stopTime);
Reply(me->delayer, (char*)&delayMsg, sizeof(DriverMsg));
msg[0] = 0;
msg[1] = (char)me->trainNum;
Putstr(me->com1, msg, 2);
me->speed = 0;
me->speedDir = DECELERATE;
}
}
