fix(failsafe): allow orbit on the first command after a failsafe

[elisaaferraraa]

Problem

After a failsafe occurs and is resolved, the first VEHICLE_CMD_DO_ORBIT (34) is
accepted but the vehicle switches to Hold/Loiter instead of Orbit; only a
second orbit command actually starts the orbit. (Image below).

The reason is that Failsafe::modifyUserIntendedMode() rewrites a requested ORBIT to
AUTO_LOITER when releasing control after an active failsafe.
That guard exists to stops a stale Orbit intention from auto-resuming, since the orbit task resets
its center to the current position and radius/velocity to defaults on
activation. But the check couldn't tell that auto-resume apart from an explicit DO_ORBIT the operator just sent: on the first command the previous cycle's action is still the failsafe action, so the command is
downgraded; on the second command the previous action is None, so it goes
through.

Solution

FailsafeBase::update() already computes a combined user_intended_mode_updated
signal. We make modifyUserIntendedMode() use it and apply the
ORBIT -> LOITER downgrade only when the orbit was not explicitly requested.
An explicit operator DO_ORBIT after a resolved failsafe now
enters Orbit on the first command, while a stale auto-resume is still
downgraded to Loiter.

Testing

• Unit tests (functional-failsafe_test, all pass) : two new cases: a fresh
  DO_ORBIT after a cleared failsafe enters ORBIT; a stale ORBIT
  intention auto-resuming after a transient failsafe is still downgraded to
  AUTO_LOITER.
• SITL (Gazebo gz_x500): took off, triggered a geofence Hold failsafe,
  cleared it, and sent a single DO_ORBIT. Log confirms nav_state goes
  directly Hold(4) → ORBIT(21) on the first command, with
  vehicle_status.failsafe active in the cycle immediately prior. (Image below)

[github-actions]

[error] google-readability-avoid-underscore-in-googletest-name [error]
avoid using "_" in test name "orbit_after_failsafe_explicit_command_allowed" according to Googletest FAQ

[github-actions]

[error] google-readability-avoid-underscore-in-googletest-name [error]
avoid using "_" in test name "orbit_autoresume_after_failsafe_downgraded_to_loiter" according to Googletest FAQ

[github-actions]

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 8 byte (0 %)]

    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%      +8  +0.0%      +8    .text
     +18%      +6   +18%      +6    Failsafe::modifyUserIntendedMode()
    +1.0%      +4  +1.0%      +4    EKFGSF_yaw::fuseVelocity()
    -0.8%      -2  -0.8%      -2    Failsafe::updateParamsImpl()
  +0.0%     +55  [ = ]       0    .debug_abbrev
  +0.0%     +77  [ = ]       0    .debug_info
  -0.0%     -20  [ = ]       0    .debug_line
  +0.0%     +13  [ = ]       0    .debug_loclists
  +0.0%      +1  [ = ]       0    .debug_rnglists
  +0.0%      +2  [ = ]       0    .debug_str
  +0.8%      +2  [ = ]       0    .shstrtab
  +0.0%      +2  [ = ]       0    .strtab
    +1.5%      +1  [ = ]       0    Failsafe::modifyUserIntendedMode()
    +1.7%      +1  [ = ]       0    FailsafeBase::modifyUserIntendedMode()
     +67%     +16  [ = ]       0    __hrt_call_enter_veneer
   -42.1%     -16  [ = ]       0    __sched_unlock_veneer
  -0.1%      -8  [ = ]       0    [Unmapped]
  +0.0%    +132  +0.0%      +8    TOTAL

px4_fmu-v6x [Total VM Diff: 0 byte (0 %)]

    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%     +55  [ = ]       0    .debug_abbrev
  +0.0%     +77  [ = ]       0    .debug_info
  -0.0%     -20  [ = ]       0    .debug_line
  +0.0%     +10  [ = ]       0    .debug_loclists
  +0.0%      +2  [ = ]       0    .debug_str
  +0.9%      +2  [ = ]       0    .shstrtab
  +0.0%      +2  [ = ]       0    .strtab
    +1.5%      +1  [ = ]       0    Failsafe::modifyUserIntendedMode()
    +1.7%      +1  [ = ]       0    FailsafeBase::modifyUserIntendedMode()
  +0.0%    +128  [ = ]       0    TOTAL

Updated: 2026-06-15T14:34:26