From 47c7fb15182700fb403894f65beaf143a7fad6ab Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 1 Apr 2018 12:23:48 -0600 Subject: Tweaked how reproject / calibate interact --- src/survive_reproject.c | 230 ++++++++++++------------------------------------ 1 file changed, 55 insertions(+), 175 deletions(-) (limited to 'src/survive_reproject.c') diff --git a/src/survive_reproject.c b/src/survive_reproject.c index 845f30a..ee9704f 100644 --- a/src/survive_reproject.c +++ b/src/survive_reproject.c @@ -4,75 +4,6 @@ #include #include -static void survive_calibration_options_config_normalize( - survive_calibration_options_config *option) { - if (!option->enable[0]) - option->invert[0] = false; - if (!option->enable[1]) - option->invert[1] = false; - if (!option->enable[0] && !option->enable[1]) - option->swap = false; -} - -void survive_calibration_options_config_apply( - const survive_calibration_options_config *option, const FLT *input, - FLT *output) { - FLT tmp[2]; // In case they try to do in place - for (int i = 0; i < 2; i++) { - tmp[i] = option->enable[i] * (option->invert[i] ? -1 : 1) * - input[i ^ option->swap]; - } - for (int i = 0; i < 2; i++) { - output[i] = tmp[i]; - } -} - -survive_calibration_config -survive_calibration_config_create_from_idx(size_t v) { - survive_calibration_config config; - memset(&config, 0, sizeof(config)); - - bool *_this = (bool *)&config; - - for (size_t i = 0; i < sizeof(config); i++) { - _this[i] = (bool)(v & 1); - v = v >> 1; - } - - survive_calibration_options_config_normalize(&config.phase); - survive_calibration_options_config_normalize(&config.tilt); - survive_calibration_options_config_normalize(&config.curve); - survive_calibration_options_config_normalize(&config.gibMag); - - config.gibPhase.enable[0] = config.gibMag.enable[0]; - config.gibPhase.enable[1] = config.gibMag.enable[1]; - - survive_calibration_options_config_normalize(&config.gibPhase); - - if (!config.gibPhase.enable[0] && !config.gibPhase.enable[1]) - config.gibUseSin = false; - - return config; -} - -size_t -survive_calibration_config_index(const survive_calibration_config *config) { - bool *_this = (bool *)config; - size_t v = 0; - for (size_t i = 0; i < sizeof(*config); i++) { - v = (v | _this[sizeof(*config) - i - 1]); - v = v << 1; - } - v = v >> 1; - return v; -} - -static FLT gibf(bool useSin, FLT v) { - if (useSin) - return sin(v); - return cos(v); -} - void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const survive_calibration_config *config, const SurvivePose *pose, const FLT *pt, FLT *out) { LinmathQuat invq; @@ -89,116 +20,43 @@ void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const surv FLT x = -t_pt[0] / -t_pt[2]; FLT y = t_pt[1] / -t_pt[2]; - double ang_x = atan(x); - double ang_y = atan(y); - - double phase[2]; - survive_calibration_options_config_apply(&config->phase, bsd->fcal.phase, phase); - double tilt[2]; - survive_calibration_options_config_apply(&config->tilt, bsd->fcal.tilt, tilt); - double curve[2]; - survive_calibration_options_config_apply(&config->curve, bsd->fcal.curve, curve); - double gibPhase[2]; - survive_calibration_options_config_apply(&config->gibPhase, bsd->fcal.gibpha, gibPhase); - double gibMag[2]; - survive_calibration_options_config_apply(&config->gibMag, bsd->fcal.gibmag, gibMag); - - out[0] = ang_x + phase[0] + (tilt[0]) * ang_y + curve[0] * ang_y * ang_y + - gibf(config->gibUseSin, gibPhase[0] + ang_x) * gibMag[0]; - out[1] = ang_y + phase[1] + (tilt[1]) * ang_x + curve[1] * ang_x * ang_x + - gibf(config->gibUseSin, gibPhase[1] + ang_y) * gibMag[1]; -} -void survive_reproject_from_pose_with_config(const SurviveContext *ctx, const survive_calibration_config *config, - int lighthouse, const SurvivePose *pose, const FLT *pt, FLT *out) { - const BaseStationData *bsd = &ctx->bsd[lighthouse]; - survive_reproject_from_pose_with_bsd(bsd, config, pose, pt, out); -} - -void survive_reproject_with_config(const SurviveContext *ctx, const survive_calibration_config *config, int lighthouse, - const FLT *point3d, FLT *out) { - survive_reproject_from_pose_with_config(ctx, config, lighthouse, &ctx->bsd[lighthouse].Pose, point3d, out); -} - -void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, - const SurvivePose *pose, FLT *pt, FLT *out) { - survive_reproject_from_pose_with_config(ctx, survive_calibration_default_config(ctx), lighthouse, pose, pt, out); -} + double ang[] = {atan(x), atan(y)}; -void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, - FLT *out) { - survive_reproject_from_pose(ctx, lighthouse, &ctx->bsd[lighthouse].Pose, - point3d, out); -} + const FLT *phase = bsd->fcal.phase; + const FLT *curve = bsd->fcal.curve; + const FLT *tilt = bsd->fcal.tilt; + const FLT *gibPhase = bsd->fcal.gibpha; + const FLT *gibMag = bsd->fcal.gibmag; + enum SurviveCalFlag f = config->use_flag; -const survive_calibration_config *survive_calibration_default_config(const SurviveContext *_ctx) { - SurviveContext *ctx = (SurviveContext *)_ctx; - if (ctx->calibration_config == 0) { - size_t idx = survive_configi(ctx, "default-cal-conf", SC_GET, 0); - ctx->calibration_config = malloc(sizeof(survive_calibration_config)); - memset(ctx->calibration_config, 0, sizeof(survive_calibration_config)); - *ctx->calibration_config = survive_calibration_config_create_from_idx(idx); - } - return ctx->calibration_config; -} + for (int axis = 0; axis < 2; axis++) { + int opp_axis = axis == 0 ? 1 : 0; -size_t survive_calibration_config_max_idx() { - survive_calibration_config cfg; - memset(&cfg, 0x1, sizeof(survive_calibration_config)); - return survive_calibration_config_index(&cfg); -} + out[axis] = ang[axis]; -static void survive_calibration_options_config_fprint(FILE *file, const survive_calibration_options_config *self) { - fprintf(file, "\t"); - if (!self->enable[0] && !self->enable[1]) { - fprintf(file, "disabled"); - return; + if (f & SVCal_Phase) + out[axis] -= config->phase_scale * phase[axis]; + if (f & SVCal_Tilt) + out[axis] -= (config->tilt_scale * tilt[axis]) * ang[opp_axis]; + if (f & SVCal_Curve) + out[axis] -= config->curve_scale * curve[axis] * ang[opp_axis] * ang[opp_axis]; + if (f & SVCal_Gib) + out[axis] -= config->gib_scale * sin(gibPhase[axis] + ang[axis]) * gibMag[axis]; } - fprintf(file, "swap: %d\n", self->swap); - for (int i = 0; i < 2; i++) { - if (self->enable[i]) { - fprintf(file, "\tinvert[%d]: %d", i, self->invert[i]); - } else { - fprintf(file, "\t%d: disabled", i); - } - } } -void survive_calibration_config_fprint(FILE *file, const survive_calibration_config *self) { - fprintf(file, "Index: %ld\n", survive_calibration_config_index(self)); - - fprintf(file, "Phase: \n"); - survive_calibration_options_config_fprint(file, &self->phase); - fprintf(file, "\n"); - - fprintf(file, "Tilt: \n"); - survive_calibration_options_config_fprint(file, &self->tilt); - fprintf(file, "\n"); - - fprintf(file, "Curve: \n"); - survive_calibration_options_config_fprint(file, &self->curve); - fprintf(file, "\n"); - - fprintf(file, "gibPhase: \n"); - survive_calibration_options_config_fprint(file, &self->gibPhase); - fprintf(file, "\n"); - - fprintf(file, "gibMag: \n"); - survive_calibration_options_config_fprint(file, &self->gibMag); - fprintf(file, "\n"); - - fprintf(file, "gibUseSin: %d\n", self->gibUseSin); -} - -void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, enum SurviveCalFlag f, const FLT *in, - FLT *out) { +void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, struct survive_calibration_config *config, + const FLT *in, FLT *out) { const BaseStationCal *cal = &ctx->bsd[lh].fcal; - out[0] = in[0] + cal->phase[0]; - out[1] = in[1] + cal->phase[1]; - - FLT tilt_scale = 10; - FLT curve_scale = 10000; - FLT gib_scale = 10; + out[0] = in[0] + config->phase_scale * cal->phase[0]; + out[1] = in[1] + config->phase_scale * cal->phase[1]; + + enum SurviveCalFlag f = config->use_flag; + FLT phase_scale = config->phase_scale; + FLT tilt_scale = config->tilt_scale; + FLT curve_scale = config->curve_scale; + FLT gib_scale = config->gib_scale; const int iterations = 4; for (int i = 0; i < iterations; i++) { FLT last_out[2] = {out[0], out[1]}; @@ -207,17 +65,39 @@ void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, enum Sur int oj = j == 0 ? 1 : 0; out[j] = in[j]; if (!last_iteration || (f & SVCal_Phase)) - out[j] += (cal->phase[j]); + out[j] += phase_scale * cal->phase[j]; if (!last_iteration || (f & SVCal_Tilt)) - out[j] += tan(cal->tilt[j] / tilt_scale) * last_out[oj]; + out[j] += (tilt_scale * cal->tilt[j]) * last_out[oj]; if (!last_iteration || (f & SVCal_Curve)) - out[j] += (cal->curve[j] / curve_scale) * last_out[oj] * last_out[oj]; + out[j] += (cal->curve[j] * curve_scale) * last_out[oj] * last_out[oj]; if (!last_iteration || (f & SVCal_Gib)) - out[j] += cos(cal->gibpha[j] + last_out[j]) * cal->gibmag[1] / gib_scale; + out[j] += sin(cal->gibpha[j] + last_out[j]) * cal->gibmag[j] * gib_scale; } } } +void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, const SurvivePose *pose, FLT *pt, + FLT *out) { + survive_reproject_from_pose_with_bsd(&ctx->bsd[lighthouse], &ctx->calibration_config, pose, pt, out); +} + +void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, FLT *out) { + survive_reproject_from_pose(ctx, lighthouse, &ctx->bsd[lighthouse].Pose, point3d, out); +} + +survive_calibration_config survive_calibration_config_ctor() { + return (survive_calibration_config){.use_flag = SVCal_All, + .phase_scale = 1., + .tilt_scale = 1. / 10000., + .curve_scale = 1. / 1000., + .gib_scale = -1. / 10.}; +} + void survive_apply_bsd_calibration(SurviveContext *ctx, int lh, const FLT *in, FLT *out) { - survive_apply_bsd_calibration_by_flag(ctx, lh, ctx->calibration_flag, in, out); + survive_apply_bsd_calibration_by_flag(ctx, lh, &ctx->calibration_config, in, out); +} + +void survive_reproject_from_pose_with_config(const SurviveContext *ctx, struct survive_calibration_config *config, + int lighthouse, const SurvivePose *pose, FLT *point3d, FLT *out) { + return survive_reproject_from_pose_with_bsd(&ctx->bsd[lighthouse], config, pose, point3d, out); } -- cgit v1.2.3