Platform-Specific PX4 Bug - Incorrect Evaluation of NaN

[jmltt]

Hardware - VOXL2
Software version - up to SDK 1.4.0

I identified what could be a significant bug when attempting to implement offboard control via acceleration setpoints in px4 on voxl2. See this post for complete details:

https://discuss.px4.io/t/ros2-offboard-transitioning-from-position-to-acceleration-setpoints/44552/2

TLDR: acceleration control doesn't work because a constrain function in the MC position control code incorrectly clamps a value of NaN for vz (set in trajectory_setpoint.vz field from an external controller) to the downward velocity limit. This only occurs on VOXL2, not in px4_sitl built from the same firmware but running on my laptop.

I resolved the issue by wrapping the specific constrain function in a call to PX4_ISFINITE which just calls __builtin_isfinite(), but this raises the question of why is NaN being evaluated incorrectly within the constrain function and could checks on NaN be failing silently elsewhere in the control stack and causing other hard to track issues.

IDK if this is being caused by a compiler flag set specifically for voxl2, it's inherent to the QRB5165 hardware, or if this issue would apply to all ARM devices. Can someone take a look and provide some feedback?

[Eric Katzfey]

Can you provide a PX4 log of the scenario from a flight with VOXL2 and a log of the same (similar) scenario from SITL?

[Eric Katzfey]

I can replicate the math::constrain issue you are seeing so looking at the best way to resolve that.

[Eric Katzfey]

This is from the analysis done by Codex:

  For the original ternary helper:

  return (value < lower_bound) ? lower_bound : ((value > upper_bound) ? upper_bound : value);

  Hexagon codegen emits:

  p0 = sfcmp.uo(r2,r0)        // unordered check: lower vs value
  p1 = sfcmp.gt(r2,r0)        // lower > value
  r0 = sfmin(r0,r3)           // r0 = min(value, upper)
  p0 = and(p1,!p0)
  if (p0) r0 = lower
  jumpr r31

  The key instruction is:

  r0 = sfmin(r0,r3)

  where r0 is the input value and r3 is 1.0f.

  On this DSP, sfmin(NaN, 1.0f) returns 1.0f, so the upper-bound half of the clamp converts NaN into the
  upper bound. That matches:

  nan_math original_pattern=3f800000

  So the heart of the issue is now very specific: the compiler lowers the upper clamp branch into sfmin, and
  the Hexagon sfmin NaN behavior does not match the C++ comparison/ternary behavior PX4 expects.

[Eric Katzfey]

So, unfortunately, it does look like a compiler issue. Fixing the math::constrain function with an explicit NaN check is probably the most reasonable thing to do in the short term.

[Eric Katzfey]

This is the commit that I'm planning to propagate: https://github.com/modalai/px4-firmware/commit/c4fd40afc9f0c79a83a633023013f11b1185cf23

[Eric Katzfey]

Here is the PR for upstream PX4: https://github.com/PX4/PX4-Autopilot/pull/27931

[Eric Katzfey]

And the latest build of our fork with this fix is here: http://voxl-packages.modalai.com/dists/qrb5165/dev/binary-arm64/voxl-px4_1.14.0-2.0.146-202607161052_arm64.deb