KHRN_FMEM_T *khrn_fmem_init(uint32_t render_state_n)
{
uint32_t *block;
KHRN_NMEM_GROUP_T temp_nmem_group;
KHRN_FMEM_T *fmem;
khrn_nmem_group_init(&temp_nmem_group, true);
block = (uint32_t *)khrn_nmem_group_alloc_master(&temp_nmem_group);
if (!block) return NULL;
fmem = (KHRN_FMEM_T *)block;
fmem->nmem_group = temp_nmem_group;
block += sizeof(KHRN_FMEM_T)/4;
fmem->nmem_entered = false;
fmem->bin_begin = (uint8_t *)block;
fmem->bin_end = NULL;
fmem->render_begin = NULL;
fmem->cle_pos = fmem->bin_begin;
fmem->last_cle_pos = NULL;
fmem->junk_pos = block + ((KHRN_NMEM_GROUP_BLOCK_SIZE-sizeof(KHRN_FMEM_T))/4);
fmem->fix_start = (KHRN_FMEM_FIX_T *)alloc_junk(fmem, sizeof(KHRN_FMEM_FIX_T), 4);
vcos_assert(fmem->fix_start); /* Should be enough room in initial block that this didn't fail */
fmem->fix_end = fmem->fix_start;
fmem->fix_end->count = 0;
fmem->fix_end->next = NULL;
tweak_init(fmem, &fmem->special);
tweak_init(fmem, &fmem->interlock);
tweak_init(fmem, &fmem->ramp);
fmem->render_state_n = render_state_n;
return fmem;
}
int main(int argc, char** args) {
int c;
dl* xys = dl_new(16);
dl* radecs = dl_new(16);
dl* otherradecs = dl_new(16);
double* xy;
double* xyz;
int i, N;
tan_t tan, tan2, tan3;
int W=0, H=0;
double crpix[] = { HUGE_VAL, HUGE_VAL };
int loglvl = LOG_MSG;
FILE* logstream = stderr;
int order = 1;
while ((c = getopt(argc, args, OPTIONS)) != -1) {
switch (c) {
case 'v':
loglvl++;
break;
case 'h':
exit(0);
case 'o':
order = atoi(optarg);
break;
case 'W':
W = atoi(optarg);
break;
case 'H':
H = atoi(optarg);
break;
case 'X':
crpix[0] = atof(optarg);
break;
case 'Y':
crpix[1] = atof(optarg);
break;
}
}
if (optind != argc) {
exit(-1);
}
log_init(loglvl);
log_to(logstream);
errors_log_to(logstream);
if (W == 0 || H == 0) {
logerr("Need -W, -H\n");
exit(-1);
}
if (crpix[0] == HUGE_VAL)
crpix[0] = W/2.0;
if (crpix[1] == HUGE_VAL)
crpix[1] = H/2.0;
while (1) {
double x,y,ra,dec;
if (fscanf(stdin, "%lf %lf %lf %lf\n", &x, &y, &ra, &dec) < 4)
break;
if (x == -1 && y == -1) {
dl_append(otherradecs, ra);
dl_append(otherradecs, dec);
} else {
dl_append(xys, x);
dl_append(xys, y);
dl_append(radecs, ra);
dl_append(radecs, dec);
}
}
logmsg("Read %i x,y,ra,dec tuples\n", dl_size(xys)/2);
N = dl_size(xys)/2;
xy = dl_to_array(xys);
xyz = malloc(3 * N * sizeof(double));
for (i=0; i<N; i++)
radecdeg2xyzarr(dl_get(radecs, 2*i), dl_get(radecs, 2*i+1), xyz + i*3);
dl_free(xys);
dl_free(radecs);
fit_tan_wcs(xyz, xy, N, &tan, NULL);
tan.imagew = W;
tan.imageh = H;
logmsg("Computed TAN WCS:\n");
tan_print_to(&tan, logstream);
sip_t* sip;
{
tweak_t* t = tweak_new();
starxy_t* sxy = starxy_new(N, FALSE, FALSE);
il* imginds = il_new(256);
il* refinds = il_new(256);
for (i=0; i<N; i++) {
starxy_set_x(sxy, i, xy[2*i+0]);
starxy_set_y(sxy, i, xy[2*i+1]);
}
tweak_init(t);
tweak_push_ref_xyz(t, xyz, N);
tweak_push_image_xy(t, sxy);
for (i=0; i<N; i++) {
il_append(imginds, i);
il_append(refinds, i);
}
// unweighted; no dist2s
tweak_push_correspondence_indices(t, imginds, refinds, NULL, NULL);
tweak_push_wcs_tan(t, &tan);
t->sip->a_order = t->sip->b_order = t->sip->ap_order = t->sip->bp_order = order;
for (i=0; i<10; i++) {
// go to TWEAK_HAS_LINEAR_CD -> do_sip_tweak
// t->image has the indices of corresponding image stars
// t->ref has the indices of corresponding catalog stars
tweak_go_to(t, TWEAK_HAS_LINEAR_CD);
logmsg("\n");
sip_print(t->sip);
t->state &= ~TWEAK_HAS_LINEAR_CD;
}
tan_write_to_file(&t->sip->wcstan, "kt1.wcs");
sip = t->sip;
}
for (i=0; i<dl_size(otherradecs)/2; i++) {
double ra, dec, x,y;
ra = dl_get(otherradecs, 2*i);
dec = dl_get(otherradecs, 2*i+1);
if (!sip_radec2pixelxy(sip, ra, dec, &x, &y)) {
logerr("Not in tangent plane: %g,%g\n", ra, dec);
exit(-1);
//continue;
}
printf("%g %g\n", x, y);
}
/*
blind_wcs_move_tangent_point(xyz, xy, N, crpix, &tan, &tan2);
blind_wcs_move_tangent_point(xyz, xy, N, crpix, &tan2, &tan3);
logmsg("Moved tangent point to (%g,%g):\n", crpix[0], crpix[1]);
tan_print_to(&tan3, logstream);
tan_write_to_file(&tan, "kt1.wcs");
tan_write_to_file(&tan3, "kt2.wcs");
*/
dl_free(otherradecs);
free(xy);
free(xyz);
return 0;
}
int main(int argc, char **argv)
{
//=== OBJECTS ===//
// Create Hubo_Control object
Hubo_Control hubo;
redirectSigs();
// std::cout << "Daemonizing as impedanceCtrl\n";
// Hubo_Control hubo("impedanceCtrl");
//=== LOCAL VARIABLES ===//
int i=0, imax=40;
double dt, ptime;
double rHandCurrent=0;
double lHandCurrent=0;
double handCurrentStep=0.25;
bool print=true;
ptime = hubo.getTime(); // set initial time for dt(0)
std::cout << "Executing control loop ...\n";
tweak_init();
char c;
while(!daemon_sig_quit)
{
// get latest state info for Hubo
hubo.update();
dt = hubo.getTime() - ptime;
ptime = hubo.getTime();
// if new data is available...
if(dt > 0)
{
if ( read(STDIN_FILENO, &c, 1) == 1) {
switch (c) {
case 'a':
lHandCurrent += handCurrentStep;
if(lHandCurrent > 1.0) lHandCurrent = 1.0;
break;
case 'z':
lHandCurrent -= handCurrentStep;
if (lHandCurrent < -1.0) lHandCurrent = -1.0;
break;
case 's':
rHandCurrent += handCurrentStep;
if(rHandCurrent > 1.0) rHandCurrent = 1.0;
break;
case 'x':
rHandCurrent -= handCurrentStep;
if (rHandCurrent < -1.0) rHandCurrent = -1.0;
break;
}
}
hubo.passJointAngle(LF1, lHandCurrent);
hubo.passJointAngle(LF2, lHandCurrent);
hubo.passJointAngle(LF3, lHandCurrent);
hubo.passJointAngle(LF4, lHandCurrent);
hubo.passJointAngle(LF5, lHandCurrent);
hubo.passJointAngle(RF1, rHandCurrent);
hubo.passJointAngle(RF2, rHandCurrent);
hubo.passJointAngle(RF3, rHandCurrent);
hubo.passJointAngle(RF4, rHandCurrent);
hubo.passJointAngle(RF5, rHandCurrent);
hubo.sendControls();
// print output every imax cycles
if( i>=imax && print==true )
{
std::cout //<< "\033[2J"
<< "lHandCurrent: " << lHandCurrent
<< "\nrHandCurrent: " << rHandCurrent
<< "\nc: " << c
<< std::endl;
}
if(i>=imax) i=0; i++;
}
}
tty_reset(STDIN_FILENO);
return 0;
}
