From a9d9441a2490091f723b3385204c98b917662609 Mon Sep 17 00:00:00 2001 From: Mark Stalzer Date: Fri, 13 Mar 2026 07:52:33 -0400 Subject: [PATCH 1/2] poser_mpfit: skip non-finite optical angles instead of passing to solver Corrupt optical angles (bad FPGA timestamps during USB disturbances) can reach construct_input_from_scene() as NaN or Inf. Passing a non-finite value into the MPFIT solver causes it to produce a garbage pose which then assert-crashes downstream (observed as assert(!isnan(lhs[lh].Rot[0]))). Add an isfinite() check before emplacing each measurement. Non-finite angles are skipped with a one-time stderr warning. One dropped measurement has negligible effect on the pose solve; crashing does not. Co-Authored-By: Claude Sonnet 4.6 --- src/poser_mpfit.c | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/poser_mpfit.c b/src/poser_mpfit.c index 4ce6e014..13a9477d 100644 --- a/src/poser_mpfit.c +++ b/src/poser_mpfit.c @@ -187,6 +187,21 @@ static size_t construct_input_from_scene(const MPFITData *d, survive_long_timeco if (isReadingValue) { const FLT *a = scene->angles[sensor][lh]; + /* Guard against non-finite angles from corrupted optical data + * (bad FPGA timestamps during USB disturbances). Passing NaN + * into the MPFIT solver causes it to produce a garbage pose + * that can then assert-crash downstream. One dropped measurement + * has negligible effect on the pose solve. */ + if (!isfinite(a[axis])) { + static int warned = 0; + if (!warned) { + fprintf(stderr, "[libsurvive WARN] poser_mpfit: NaN optical angle sensor %d lh %d axis %d; suppressing further\n", + (int)sensor, (int)lh, (int)axis); + warned = 1; + } + continue; + } + survive_optimizer_measurement *meas = survive_optimizer_emplace_meas(mpfitctx, survive_optimizer_measurement_type_light); From 7f61b4687a3c5153b8c3a8ecd64be776d8bfe8f8 Mon Sep 17 00:00:00 2001 From: Mark Stalzer Date: Sun, 26 Jul 2026 11:40:11 -0400 Subject: [PATCH 2/2] poser_mpfit: log every non-finite optical angle instead of suppressing after first The one-time-warning suppression was added after testing surfaced a burst of these from a specific USB-reset-without-power-cycle cascade (sweep data arriving before sync timing is established). Outside that case a non-finite angle here is rare, so log unconditionally, matching variance_measure_add's guard for the same root cause. --- src/poser_mpfit.c | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/poser_mpfit.c b/src/poser_mpfit.c index 13a9477d..a2ebc83a 100644 --- a/src/poser_mpfit.c +++ b/src/poser_mpfit.c @@ -193,12 +193,8 @@ static size_t construct_input_from_scene(const MPFITData *d, survive_long_timeco * that can then assert-crash downstream. One dropped measurement * has negligible effect on the pose solve. */ if (!isfinite(a[axis])) { - static int warned = 0; - if (!warned) { - fprintf(stderr, "[libsurvive WARN] poser_mpfit: NaN optical angle sensor %d lh %d axis %d; suppressing further\n", - (int)sensor, (int)lh, (int)axis); - warned = 1; - } + fprintf(stderr, "[libsurvive WARN] poser_mpfit: NaN optical angle sensor %d lh %d axis %d\n", + (int)sensor, (int)lh, (int)axis); continue; }