int turn(float heading){ float currentHeading = getHeading(fd); float deltaHeading = wrap180(heading - currentHeading); // deltaheading: negative means turn left, positive means turn right const int tolerance = 5; printf("deltaHeading: %f, currentHeading: %f, tolerance: %d\n", deltaHeading, currentHeading, tolerance); //while(abs((int) deltaHeading) > tolerance && keepGoing){ while(keepGoing && abs(deltaHeading) > tolerance){ if(deltaHeading > 0){ //turn right printf("**turning right**deltaHeading positive: %f, currentHeading: %f\n", deltaHeading, currentHeading); gpio_set_value(LEFT_FWD_GPIO, 1); gpio_set_value(LEFT_BCK_GPIO, 0); gpio_set_value(RIGHT_BCK_GPIO, 1); gpio_set_value(RIGHT_FWD_GPIO, 0); } else { // turn left printf("**turning left**deltaHeading negative: %f, currentHeading: %f\n", deltaHeading, currentHeading); gpio_set_value(LEFT_BCK_GPIO, 1); gpio_set_value(LEFT_FWD_GPIO, 0); gpio_set_value(RIGHT_FWD_GPIO, 1); gpio_set_value(RIGHT_BCK_GPIO, 0); } currentHeading = getHeading(fd); deltaHeading = wrap180(heading - currentHeading); } gpio_set_value(LEFT_FWD_GPIO, 0); gpio_set_value(LEFT_BCK_GPIO, 0); gpio_set_value(RIGHT_FWD_GPIO, 0); gpio_set_value(RIGHT_BCK_GPIO, 0); }
int track_find(const double t_eq, const double phi_eq, const double dt_min, const double dphi, size_t *idx, const track_workspace *w) { int s = GSL_FAILURE; size_t i; for (i = 0; i < w->n; ++i) { track_data *tptr = &(w->tracks[i]); double t_diff = fabs(t_eq - tptr->t_eq) / 60000.0; /* convert to minutes */ double phi_diff = wrap180(phi_eq - tptr->lon_eq); if (fabs(phi_diff) > dphi) continue; if (t_diff > dt_min) continue; *idx = i; s = GSL_SUCCESS; break; } return s; }
size_t track_flag_lon(const double lon_min, const double lon_max, size_t *ndata_flagged, satdata_mag *data, track_workspace *w) { size_t i; size_t nflagged = 0; /* number of points flagged */ size_t ntrack_flagged = 0; /* number of entire tracks flagged for UT */ if (data->n == 0) return 0; for (i = 0; i < w->n; ++i) { track_data *tptr = &(w->tracks[i]); double lon = wrap180(tptr->lon_eq); if (lon < lon_min || lon > lon_max) { nflagged += track_flag_track(i, TRACK_FLG_LONGITUDE, data, w); ++ntrack_flagged; } } fprintf(stderr, "track_flag_lon: flagged %zu/%zu (%.1f%%) tracks due to longitude\n", ntrack_flagged, w->n, (double) ntrack_flagged / (double) w->n * 100.0); if (ndata_flagged) *ndata_flagged = nflagged; return ntrack_flagged; } /* track_flag_lon() */
int magfit_print_rms(const int header, FILE *fp, const double lon0, const track_data *tptr, const satdata_mag *data, magfit_workspace *w) { size_t i; double rms[3] = { 0.0, 0.0, 0.0 }; size_t n = 0; if (header) { i = 1; fprintf(fp, "# Field %zu: timestamp of equator crossing\n", i++); fprintf(fp, "# Field %zu: dlon (degrees)\n", i++); fprintf(fp, "# Field %zu: X rms (nT)\n", i++); fprintf(fp, "# Field %zu: Y rms (nT)\n", i++); fprintf(fp, "# Field %zu: Z rms (nT)\n", i++); return 0; } for (i = 0; i < tptr->n; ++i) { size_t didx = i + tptr->start_idx; double r = data->altitude[didx] + data->R; double theta = M_PI / 2.0 - data->latitude[didx] * M_PI / 180.0; double phi = data->longitude[didx] * M_PI / 180.0; double B_model[3]; if (!SATDATA_AvailableData(data->flags[didx])) continue; /* use only low-latitude data */ if (fabs(data->qdlat[didx]) > MAGFIT_QDMAX) continue; /* compute model prediction */ magfit_eval_B(r, theta, phi, B_model, w); rms[0] += pow(tptr->Bx[i] - B_model[0], 2.0); rms[1] += pow(tptr->By[i] - B_model[1], 2.0); rms[2] += pow(tptr->Bz[i] - B_model[2], 2.0); ++n; } if (n > 0) { for (i = 0; i < 3; ++i) rms[i] = sqrt(rms[i] / (double)n); } fprintf(fp, "%ld %.4f %.2f %.2f %.2f\n", satdata_epoch2timet(tptr->t_eq), wrap180(tptr->lon_eq - lon0), rms[0], rms[1], rms[2]); fflush(fp); return 0; }
size_t docorr(const char *filename, const satdata_eef *eef1, const satdata_eef *eef2) { size_t i, j; FILE *fp; size_t cnt = 0; bin2d_workspace *bin_p; const double tmin = -50.0; const double tmax = 50.0; const double lonmin = -50.0; const double lonmax = 20.0; const size_t nt = 10; const size_t nlon = 5; fp = fopen(filename, "w"); if (!fp) { fprintf(stderr, "docorr: unable to open %s: %s\n", filename, strerror(errno)); return 0; } bin_p = bin2d_alloc(lonmin, lonmax, nlon, tmin, tmax, nt); i = 1; fprintf(fp, "# Field %zu: timestamp of satellite 1 (UT)\n", i++); fprintf(fp, "# Field %zu: delta t of equator crossings (min)\n", i++); fprintf(fp, "# Field %zu: delta longitude of equator crossings (degrees)\n", i++); fprintf(fp, "# Field %zu: peak J1 (A/m)\n", i++); fprintf(fp, "# Field %zu: peak J2 (A/m)\n", i++); for (i = 0; i < eef1->n; ++i) { double t1 = eef1->t[i]; size_t idx = eef_search(t1, eef2); double t2 = eef2->t[idx]; double dt_s = (t1 - t2) / 1000.0; /* convert to s */ double dt_m = dt_s / 60.0; /* convert to m */ double dlon = wrap180(eef1->longitude[i] - eef2->longitude[idx]); if (fabs(dt_s) > 3600.0) continue; if (dlon < lonmin || dlon > lonmax) continue; if (dt_m < tmin || dt_m > tmax) continue; bin2d_add_element_corr(dlon, dt_m, eef1->J[i], eef2->J[idx], bin_p); fprintf(fp, "%ld %f %f %f %f\n", satdata_epoch2timet(t1), dt_m, dlon, eef1->J[i], eef2->J[idx]); ++cnt; } fclose(fp); i = 1; printf("# Field %zu: delta longitude (degrees)\n", i++); printf("# Field %zu: delta t (minutes)\n", i++); printf("# Field %zu: correlation\n", i++); printf("# Field %zu: number of points in bin\n", i++); for (i = 0; i < nlon; ++i) { for (j = 0; j < nt; ++j) { double dt, dlon; size_t n; double r; bin2d_xyval(i, j, &dlon, &dt, bin_p); n = bin2d_n(dlon, dt, bin_p); r = bin2d_correlation(dlon, dt, bin_p); printf("%f %f %f %zu\n", dlon, dt, r, n); } printf("\n"); } bin2d_free(bin_p); return cnt; }
int track_print_stats_flag(const char *filename, const size_t flag, track_workspace *w) { int s = 0; size_t i; FILE *fp; satdata_mag *data = w->data; size_t nflagged; fp = fopen(filename, "w"); if (!fp) { fprintf(stderr, "track_print_stats: unable to open %s: %s\n", filename, strerror(errno)); return -1; } nflagged = track_nflagged(w); fprintf(fp, "# Total tracks: %zu\n", w->n); fprintf(fp, "# Total flagged: %zu\n", nflagged); fprintf(fp, "# Total unflagged: %zu\n", w->n - nflagged); fprintf(fp, "# Track selection flags: %zu\n", flag); i = 1; fprintf(fp, "# Field %zu: timestamp (UT seconds since 1970-01-01 00:00:00 UTC)\n", i++); fprintf(fp, "# Field %zu: longitude of equator crossing (degrees)\n", i++); fprintf(fp, "# Field %zu: local time of equator crossing (hours)\n", i++); fprintf(fp, "# Field %zu: track mean altitude (km)\n", i++); fprintf(fp, "# Field %zu: number of data points in track\n", i++); fprintf(fp, "# Field %zu: number of non-flagged data points in track\n", i++); fprintf(fp, "# Field %zu: satellite direction (+1 north -1 south)\n", i++); fprintf(fp, "# Field %zu: track X rms (nT)\n", i++); fprintf(fp, "# Field %zu: track Y rms (nT)\n", i++); fprintf(fp, "# Field %zu: track Z rms (nT)\n", i++); fprintf(fp, "# Field %zu: track scalar rms (nT)\n", i++); fprintf(fp, "# Field %zu: number of along-track data used for scalar rms\n", i++); fprintf(fp, "# Field %zu: number of along-track data used for vector rms\n", i++); fprintf(fp, "# Field %zu: track flagged due to rms\n", i++); for (i = 0; i < w->n; ++i) { track_data *tptr = &(w->tracks[i]); size_t nflag = track_data_nflagged(tptr, data); if (flag != 0xffffffff) { if (flag == 0 && tptr->flags != 0) continue; if (flag != 0 && !(tptr->flags & flag)) continue; } fprintf(fp, "%ld %10.4f %6.2f %6.2f %6zu %6zu %2d %10.4f %10.4f %10.4f %10.4f %6zu %6zu %d\n", satdata_epoch2timet(tptr->t_eq), wrap180(tptr->lon_eq), tptr->lt_eq, tptr->meanalt, tptr->n, tptr->n - nflag, tptr->satdir, tptr->rms[0], tptr->rms[1], tptr->rms[2], tptr->rms[3], tptr->nrms_scal, tptr->nrms_vec, (tptr->flags & TRACK_FLG_RMS) ? 1 : 0); } fclose(fp); return s; } /* track_print_stats_flag() */
int mfield_euler_print(const char *filename, const size_t sat_idx, const mfield_workspace *w) { FILE *fp; size_t i; double t0 = w->data_workspace_p->t0[sat_idx]; double t1 = w->data_workspace_p->t1[sat_idx]; double t, dt; double alpha_mean = 0.0, beta_mean = 0.0, gamma_mean = 0.0; size_t nmean = 0; fp = fopen(filename, "w"); if (!fp) { fprintf(stderr, "mfield_euler_print: unable to open %s: %s\n", filename, strerror(errno)); return -1; } i = 1; fprintf(fp, "# Euler angles for satellite %zu\n", sat_idx); fprintf(fp, "# Field %zu: timestamp (CDF_EPOCH)\n", i++); fprintf(fp, "# Field %zu: time (decimal years)\n", i++); fprintf(fp, "# Field %zu: alpha (degrees)\n", i++); fprintf(fp, "# Field %zu: beta (degrees)\n", i++); fprintf(fp, "# Field %zu: gamma (degrees)\n", i++); fprintf(fp, "# Field %zu: alpha - mean (arcseconds)\n", i++); fprintf(fp, "# Field %zu: beta - mean (arcseconds)\n", i++); fprintf(fp, "# Field %zu: gamma - mean (arcseconds)\n", i++); if (w->params.euler_period > 0.0) dt = w->params.euler_period * 86400000; /* convert to ms */ else dt = t1 - t0; /* compute means of Euler angles */ for (t = t0; t < t1; t += dt) { size_t euler_idx = mfield_euler_idx(sat_idx, t, w); double alpha = gsl_vector_get(w->c, euler_idx); double beta = gsl_vector_get(w->c, euler_idx + 1); double gamma = gsl_vector_get(w->c, euler_idx + 2); if (alpha == 0.0 || beta == 0.0 || gamma == 0.0) continue; alpha_mean += alpha; beta_mean += beta; gamma_mean += gamma; ++nmean; } if (nmean > 0) { alpha_mean /= (double) nmean; beta_mean /= (double) nmean; gamma_mean /= (double) nmean; } for (t = t0; t < t1; t += dt) { size_t euler_idx = mfield_euler_idx(sat_idx, t, w); double alpha = gsl_vector_get(w->c, euler_idx); double beta = gsl_vector_get(w->c, euler_idx + 1); double gamma = gsl_vector_get(w->c, euler_idx + 2); fprintf(fp, "%f %f %.10f %.10f %.10f %f %f %f\n", t, satdata_epoch2year(t), wrap180(alpha * 180.0 / M_PI), wrap180(beta * 180.0 / M_PI), wrap180(gamma * 180.0 / M_PI), (alpha - alpha_mean) * 180.0 / M_PI * 3600.0, (beta - beta_mean) * 180.0 / M_PI * 3600.0, (gamma - gamma_mean) * 180.0 / M_PI * 3600.0); } fclose(fp); return 0; }
int correlateJ(const char *data_file, const char *corr_file, current_data *data1, current_data *data2) { int s = 0; size_t i, j; bin2d_workspace *bin2d_p; bin_workspace *bin_p; const double phi_min = -5.0; const double phi_max = 25.0; const size_t nphi = 7; const double t_min = -60.0; const double t_max = 60.0; const size_t nt = 2; FILE *fp_data, *fp_corr; double *dp, *x, *y, *r; size_t n = 0; double sigma; fp_data = fopen(data_file, "w"); fp_corr = fopen(corr_file, "w"); bin2d_p = bin2d_alloc(phi_min, phi_max, nphi, t_min, t_max, nt); bin_p = bin_alloc(phi_min, phi_max, nphi); dp = malloc(data1->n * sizeof(double)); x = malloc(data1->n * sizeof(double)); y = malloc(data1->n * sizeof(double)); r = malloc(data1->n * sizeof(double)); i = 1; fprintf(fp_data, "# Field %zu: timestamp of satellite 1 crossing (UT)\n", i++); fprintf(fp_data, "# Field %zu: delta t (minutes)\n", i++); fprintf(fp_data, "# Field %zu: delta phi (degrees)\n", i++); fprintf(fp_data, "# Field %zu: satellite 1 peak current (mA/m)\n", i++); fprintf(fp_data, "# Field %zu: satellite 2 peak current (mA/m)\n", i++); for (i = 0; i < data1->n; ++i) { curr_profile *prof = &(data1->profiles[i]); curr_profile *prof2; time_t dt; double dphi, dt_min; size_t idx; double lt = get_localtime(prof->t, prof->phi * M_PI / 180.0); s = find_profile_t(prof->t, data2, &idx); if (s) continue; prof2 = &(data2->profiles[idx]); if (prof->npeak != 1 || prof2->npeak != 1) continue; #if 0 if (prof->peak_J > 20.0) continue; /* a couple outliers */ #endif #if 0 if (lt > 20.0) continue; #endif dt = prof2->t - prof->t; dphi = wrap180(prof2->phi - prof->phi); dt_min = (double)dt / 60.0; #if 0 bin2d_add_element_corr(dphi, dt_min, prof->peak_J, prof2->peak_J, bin2d_p); #endif if (fabs(dt_min) <= 30.0 && dphi >= phi_min && dphi <= phi_max) { /* store this point */ x[n] = prof->peak_J; y[n] = prof2->peak_J; dp[n] = dphi; ++n; fprintf(fp_data, "%ld %f %f %f %f\n", prof->t, dt_min, dphi, prof->peak_J, prof2->peak_J); } } /* robust fit straight line to (x,y) */ { curvefit_workspace *curvefit_p = curvefit_alloc(gsl_multifit_robust_bisquare, curvefit_poly, n, 2); curvefit(0, x, y, curvefit_p); curvefit_residuals(x, y, r, curvefit_p); curvefit_free(curvefit_p); const double alpha = 2.5; sigma = gsl_stats_sd(r, 1, n); fprintf(stderr, "sigma = %f\n", sigma); /* loop through again and add (J1,J2) points which aren't outliers */ for (i = 0; i < n; ++i) { if (fabs(r[i]) > alpha*sigma) continue; bin_add_element_corr(dp[i], x[i], y[i], bin_p); } } #if 0 i = 1; fprintf(fp_corr, "# Field %zu: delta longitude (degrees)\n", i++); fprintf(fp_corr, "# Field %zu: delta t (minutes)\n", i++); fprintf(fp_corr, "# Field %zu: correlation\n", i++); fprintf(fp_corr, "# Field %zu: number of points in bin\n", i++); for (i = 0; i < nphi; ++i) { for (j = 0; j < nt; ++j) { double dt, dlon; size_t n; double r; bin2d_xyval(i, j, &dlon, &dt, bin2d_p); n = bin2d_n(dlon, dt, bin2d_p); r = bin2d_correlation(dlon, dt, bin2d_p); fprintf(fp_corr, "%f %f %f %zu\n", dlon, dt, r, n); } fprintf(fp_corr, "\n"); } #else i = 1; fprintf(fp_corr, "# Field %zu: delta longitude (degrees)\n", i++); fprintf(fp_corr, "# Field %zu: correlation\n", i++); fprintf(fp_corr, "# Field %zu: number of points in bin\n", i++); for (i = 0; i < nphi; ++i) { double dlon; size_t n; double r; bin_xval(i, &dlon, bin_p); n = bin_n(dlon, bin_p); r = bin_correlation(dlon, bin_p); fprintf(fp_corr, "%f %f %zu\n", dlon, r, n); } #endif fclose(fp_data); fclose(fp_corr); free(x); free(y); free(r); return s; }
static void * secs2d_alloc(const void * params) { const magfit_parameters *mparams = (const magfit_parameters *) params; secs2d_state_t *state; const size_t flags = mparams->secs_flags; const double lat_spacing = mparams->lat_spacing2d; const double lat_max = mparams->lat_max; const double lat_min = mparams->lat_min; const size_t ntheta = (size_t) ((lat_max - lat_min) / lat_spacing + 1.0); const double dtheta = lat_spacing * M_PI / 180.0; const double theta_min = M_PI / 2.0 - lat_max * M_PI / 180.0; const double lon_spacing = mparams->lon_spacing; const double lon_max = mparams->lon_max; const double lon_min = mparams->lon_min; const double phi_min = lon_min * M_PI / 180.0; const double diff = wrap180(lon_max - lon_min); const size_t nphi = GSL_MAX((size_t) (fabs(diff) / lon_spacing + 1.0), 2); const double dphi = diff / (nphi - 1.0) * M_PI / 180.0; const size_t npoles = ntheta * nphi; size_t i; state = calloc(1, sizeof(secs2d_state_t)); if (!state) return 0; state->nmax = 30000; state->n = 0; state->p = 0; state->ntheta = ntheta; state->nphi = nphi; state->R = mparams->R; state->flags = flags; state->df_offset = 0; state->cf_offset = 0; state->dtheta = dtheta; if (flags & MAGFIT_SECS_FLG_FIT_DF) { state->df_offset = state->p; state->p += npoles; } if (flags & MAGFIT_SECS_FLG_FIT_CF) { state->cf_offset = state->p; state->p += npoles; } state->X = gsl_matrix_alloc(state->nmax, state->p); state->c = gsl_vector_alloc(state->p); state->rhs = gsl_vector_alloc(state->nmax); state->wts = gsl_vector_alloc(state->nmax); state->cov = gsl_matrix_alloc(state->p, state->p); state->multifit_p = gsl_multifit_linear_alloc(state->nmax, state->p); state->theta0 = malloc(ntheta * sizeof(double)); state->phi0 = malloc(nphi * sizeof(double)); /* fill in theta0 array */ for (i = 0; i < ntheta; ++i) { state->theta0[i] = theta_min + i * dtheta; } /* fill in phi0 array */ for (i = 0; i < nphi; ++i) { state->phi0[i] = phi_min + i * dphi; } fprintf(stderr, "secs2d_alloc: ntheta = %zu\n", state->ntheta); fprintf(stderr, "secs2d_alloc: nphi = %zu\n", state->nphi); fprintf(stderr, "secs2d_alloc: ncoeff = %zu\n", state->p); return state; }
size_t track_flag_rms(const char *outfile, const double thresh[4], size_t *ndata_flagged, satdata_mag *data, track_workspace *w) { FILE *fp; size_t i, j; size_t nflagged = 0; /* number of points flagged */ size_t ntrack_flagged = 0; /* number of entire tracks flagged for rms */ if (data->n == 0) return 0; fp = fopen(outfile, "w"); i = 1; fprintf(fp, "# Field %zu: timestamp of equator crossing (UT seconds since 1970-01-01 00:00:00 UTC)\n", i++); fprintf(fp, "# Field %zu: time of equator crossing (years)\n", i++); fprintf(fp, "# Field %zu: longitude of equator crossing (degrees)\n", i++); fprintf(fp, "# Field %zu: local time of equator crossing (hours)\n", i++); fprintf(fp, "# Field %zu: track mean altitude (km)\n", i++); fprintf(fp, "# Field %zu: number of data points in track\n", i++); fprintf(fp, "# Field %zu: track X rms (nT)\n", i++); fprintf(fp, "# Field %zu: track Y rms (nT)\n", i++); fprintf(fp, "# Field %zu: track Z rms (nT)\n", i++); fprintf(fp, "# Field %zu: track scalar rms (nT)\n", i++); fprintf(fp, "# Field %zu: satellite direction (+1 north -1 south)\n", i++); fprintf(fp, "# Field %zu: number of along-track data used for scalar rms\n", i++); fprintf(fp, "# Field %zu: number of along-track data used for vector rms\n", i++); fprintf(fp, "# Field %zu: flagged due to rms (1 or 0)\n", i++); for (i = 0; i < w->n; ++i) { track_data *tptr = &(w->tracks[i]); size_t sidx = tptr->start_idx; size_t eidx = tptr->end_idx; int s; int flag_rms = 0; /* compute rms of track residuals for low-latitudes for |qdlat| < QDLAT_MAX */ track_calc_rms(0.0, TRACK_QDLAT_MAX, tptr, &(tptr->nrms_scal), &(tptr->nrms_vec), tptr->rms, data); /* compute rms of track residuals for polar latitudes for |qdlat| > QDLAT_MAX */ track_calc_rms(TRACK_QDLAT_MAX, 100.0, tptr, &(tptr->nrmspol_scal), &(tptr->nrmspol_vec), tptr->rmspol, data); /* check if there are enough points for good rms estimate */ s = 0; if (thresh[3] > 0.0 && tptr->nrms_scal < TRACK_RMS_NDATA) s = -1; #if 0 /* don't check vector nrms since some CHAMP local-times have 0 vector * data but still have good scalar data */ if ((thresh[0] > 0.0 || thresh[1] > 0.0 || thresh[2] > 0.0) && tptr->nrms_vec < TRACK_RMS_NDATA) s = -1; #endif if (s != 0) { /* too few points for rms calculation */ #if TRACK_DEBUG fprintf(stderr, "track_flag_rms: failed rms, flagging pts [%zu, %zu]\n", sidx, eidx); #endif flag_rms = 1; } else { /* check rms against thresholds */ for (j = 0; j < 4; ++j) { if (thresh[j] > 0.0 && tptr->rms[j] > thresh[j]) flag_rms = 1; } if (!flag_rms) { /* * otherwise, look for individual outliers which weren't part of * the rms computation (ie: at the poles) */ nflagged += track_flag_outliers(sidx, eidx, 10.0*thresh[3], 10.0*thresh[2], data); } } if (flag_rms) { /* failed rms test */ nflagged += track_flag_track(i, TRACK_FLG_RMS, data, w); ++ntrack_flagged; } fprintf(fp, "%ld %f %10.4f %6.2f %6.2f %6zu %10.4f %10.4f %10.4f %10.4f %2d %6zu %6zu %d\n", satdata_epoch2timet(tptr->t_eq), satdata_epoch2year(tptr->t_eq), wrap180(tptr->lon_eq), tptr->lt_eq, tptr->meanalt, tptr->n, tptr->rms[0], tptr->rms[1], tptr->rms[2], tptr->rms[3], satdata_mag_satdir(sidx, data), tptr->nrms_scal, tptr->nrms_vec, flag_rms); fflush(fp); } fclose(fp); fprintf(stderr, "track_flag_rms: flagged %zu/%zu (%.1f%%) tracks due to rms\n", ntrack_flagged, w->n, (double) ntrack_flagged / (double) w->n * 100.0); fprintf(stderr, "track_flag_rms: rms data written to %s\n", outfile); if (ndata_flagged) return nflagged; return ntrack_flagged; } /* track_flag_rms() */