Commit Graph
30515 Commits
Author SHA1 Message Date
Marco Hauswirth d0775f2e40 fix(ekf2): limit mag heading change on the innovation that is applied 2026-11-17 16:46:55 +01:00
Marco Hauswirth 9e2629ee24 refactor(ekf2): name the accumulating fusion helpers after what they do with the state 2026-11-17 16:46:55 +01:00
Marco Hauswirth 77c72664e6 refactor(ekf2): compute pos/vel innovations once 2026-11-17 16:46:55 +01:00
Marco Hauswirth 14ca6d176e test(ekf2): check sequential fusion against the batch update
Sequential fusion of a vector observation must equal the vector Kalman
update at the linearisation point; the batch update has no processing
order. The test builds a prior with correlated velocity, attitude and
terrain, feeds the optical flow fusion two 2.5 sigma innovations that
both pass the gate, and compares state and covariance with the batch
update. A second test does the same through measurementUpdate() in both
processing orders.

On main the second flow axis is rejected after the first one moved the
state (test ratio 3.0), the state ends 2.5 away from the batch update
and the covariance 0.22; with an uncorrelated prior, recomputing the
second innovation from the already corrected state alone moves the
state by 0.045.
2026-11-17 16:46:55 +01:00
Marco Hauswirth 5510811f66 feat(ekf2): edit sequential fusion residuals at the a priori state
The whitepaper warns that relinearizing between the scalars of one
vector measurement pushes the following residuals outside the edit
thresholds, so which of them survive depends on the processing
order.
2026-11-17 16:46:55 +01:00
Marco Hauswirth ee0b7c6976 feat(ekf2): make remaining sequential fusions order-invariant 2026-11-17 16:46:55 +01:00
Marco Hauswirth a3f3b861e3 feat(ekf2): make sequential fusion order-invariant (NASA Whitepaper 2025 Algorithm 4.1) 2026-11-17 16:46:55 +01:00
Silvan Fuhrer 6967a283d5 feat(commander): remove implicit RTL on GCS loss after mission finished (#29722)
With manual control disabled (or Mission excluded via COM_RCL_EXCEPT) and
NAV_DLL_ACT=0, losing the GCS link after the mission finished silently
triggered a Return. This overrides the explicit choice to disable the
data link loss failsafe, is not configurable and is not documented.
Users who want a reaction to data link loss should set NAV_DLL_ACT.

Assisted-by: Claude:claude-opus-5-5

Signed-off-by: Silvan <silvan@auterion.com>
2026-11-17 16:14:40 +01:00
Andrea Bernasconi 67f5c57e9a feat(GNSS): Introduce kinematic consistency check (#29660)
* feat(gnss): check that the position change matches the velocity

A receiver computes its position from the code (time of flight) and its
velocity from the Doppler shift. Multipath and other degradation hit the
two differently, so a degraded receiver's position change stops matching
its velocity, often before its reported accuracy shows it. The existing
drift checks compare the two only at rest on the ground.

The new strict check filters the position change rate minus the mean
velocity over about 1 s and fails above GNSS_REQ_POSVEL, also in flight,
so the selection prefers another receiver that passes. It is off by
default (GNSS_CHECK bit 12) until the threshold is tuned on logs; the
filtered value is logged in sensors_status_gnss either way.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Andrea Bernasconi <andrea.bernasconi@auterion.com>

* fix(gnss): skip the position step at a fix type change in the consistency check

When a receiver starts or stops applying differential or RTK corrections,
its position steps by up to a few meters in one epoch while the Doppler
velocity is unaffected. That is a new solution, not degradation, and it
shouldn't demote a receiver that just reached RTK fixed. On a 10 Hz log
with DGPS dropping in and out, these steps drove the filtered value to
4.5 m/s; without them it peaks at 0.8 m/s, during a real vertical drift.

Only the interval spanning the change is skipped and the filter keeps its
state, so a receiver that keeps switching isn't blind for longer.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Andrea Bernasconi <andrea.bernasconi@auterion.com>

* refactor(gnss): Reduce check description

* fix(GNSS): Make the feature enabled by default but with more margin

* fix(gnss): restrict the position/velocity consistency check to horizontal

Receivers that don't report the vertical velocity failed the check on any
climb. Merge the consistency tests into fewer cases.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Andrea Bernasconi <andrea.bernasconi@auterion.com>

* refactor(gnss): mark the position/velocity consistency check as horizontal

The check only compares the horizontal position change with the horizontal
velocity. Name the parameter, flags and status field after that, so that a
vertical counterpart can be added later as GNSS_REQ_VPOSVEL / CHECK_VPOSVEL.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Andrea Bernasconi <andrea.bernasconi@auterion.com>

* refactor(gnss): rename the position/velocity check to kinematic consistency

HPOSVEL read like a check on position and velocity separately, next to
HDRIFT and HSPEED. "Kinematic consistency" names the property being
tested and leaves room for a vertical counterpart (VKIN). Messages,
flag comments and the parameter's long description keep the plain
wording, since "kinematic" alone can be read as RTK.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Andrea Bernasconi <andrea.bernasconi@auterion.com>

---------

Signed-off-by: Andrea Bernasconi <andrea.bernasconi@auterion.com>
2026-11-17 10:38:26 +01:00
Jotheesh KummathiandJacob Dahl 0937553e1c fix(uavcan): correct GNSS timestamp conversion (#29803)
* fix(uavcan): correct GNSS timestamp conversion

Fix GPS and TAI leap-second offsets in DroneCAN GNSS timestamps.

Assisted-by: OpenAI:Codex

* refactor(uavcan): keep the GNSS UTC conversion in gnss.cpp

The fix is a units change in one switch. A shared header, a gtest and a test-build step that compiles the whole uavcan DSDL namespace cost more to maintain than they protect.

Assisted-by: Claude:claude-opus-5-5

---------

Co-authored-by: Jacob Dahl <dahl.jakejacob@gmail.com>
2026-11-16 18:02:34 -07:00
Julian Oes d995af2184 fix(commander/mavlink): DO_SET_HOME improvements (#29786)
* fix(commander): accept only 0 or 1 as DO_SET_HOME param1

MAV_CMD_DO_SET_HOME param1 is a MAV_BOOL: 1 uses the current position,
0 the specified location, and any other value is invalid. Any value
above 0.5 was taken as "use current", so an invalid value silently set
home to the current position instead of being denied.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Julian Oes <julian@oes.ch>

* fix(mavlink): honour the COMMAND_INT frame for DO_SET_HOME

The frame of a COMMAND_INT was only used to scale x/y and then dropped,
and Commander takes the DO_SET_HOME altitude as AMSL. A relative
altitude, as sent by MAVProxy, therefore set home to that many metres
above mean sea level, and local frames gave meaningless coordinates.

A global altitude is passed on as before, a relative altitude is
converted to AMSL using the current home altitude, and other frames
are rejected with MAV_RESULT_COMMAND_UNSUPPORTED_MAV_FRAME. The frame
does not matter when the current position is used.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Julian Oes <julian@oes.ch>

* fix(commander): allow DO_SET_HOME with COM_HOME_EN disabled

COM_HOME_EN is documented as setting home automatically, but disabling
it also denied MAV_CMD_DO_SET_HOME, leaving the vehicle without any way
to get a home. Disabling it now only stops home from being set
automatically, and home can still be set manually.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Julian Oes <julian@oes.ch>

---------

Signed-off-by: Julian Oes <julian@oes.ch>
2026-11-16 18:00:37 -07:00
mumumuandSummermu 8638fa397e fix(control_allocator): reject negative tilt indices (#29797)
CA_ROTORn_TILT is declared as an enum (0..4), but parameter ranges are
not enforced by param_set(). ActuatorEffectivenessRotors::updateParams()
stores tilt_param - 1 directly, and both consumers only skip the sentinel
value -1. Any value below -1 therefore indexes _params[] and _torque[]
with a negative index, which crashes the control allocator on the next
parameter update.

Treat every negative index as "not tilted" in updateTorqueSign() and
updateAxisFromTilts().

Test: main SITL, CA_AIRFRAME=8 + CA_ROTOR0_TILT=-100000 (SIGSEGV in
ActuatorEffectivenessTilts::updateTorqueSign)


Assisted-by: deepseek

Signed-off-by: Summermu <23356722+Summermu@users.noreply.github.com>
Co-authored-by: Summermu <23356722+Summermu@users.noreply.github.com>
2026-11-16 16:44:56 -07:00
Julian Oes d694bf0d53 fix(mavlink): reset the log entry on LOG_ERASE (#29831)
LOG_ERASE closed the open log file but kept the entry and the log count,
so a following LOG_REQUEST_DATA for the same id went to SendingData
without a file and crashed in ftell(nullptr). A failed fopen() and a
failed fseek() left the handler in the same state.

Reported by @KANG982.

Assisted-by: Claude:claude-opus-5-5

Signed-off-by: Julian Oes <julian@oes.ch>
2026-11-16 16:41:42 -07:00
jeroldcamacho 32c68d75c8 fix(rc): bound ST24 write index against shared rc_decode_buf union (#29693)
RCInput's protocol auto-scan rotates between RC decoders without
resetting their static state, and all decoders alias the same global
rc_decode_buf union. If a sibling decoder overwrites the union's
length byte while ST24 is mid-frame, st24_decode()'s GOT_TYPE write
index can never resynchronize and walks off the end of st24_data.

Bound the GOT_TYPE write and GOT_DATA CRC read against the buffer's
real size, resyncing to UNSYNCED instead of writing out of bounds.
Add a regression test reproducing the cross-protocol clobber.
2026-11-17 12:36:29 +13:00
Anil Kircaliali f6562b1075 fix(commander): mark adsb warning text as event duplicate (#29829) 2026-11-16 15:28:06 -07:00
Hamish WilleeandJulian Oes 4d755a614f fix(mavlink): Make MAV_CMD_SET_ACTUATOR scale properly in missions (#27336)
* fix(mavlink): scale DO_SET_ACTUATOR params 5/6 in mission items

MISSION_ITEM_INT carries params 5/6 in the int32 x/y fields, so the actuator
5 and 6 values (-1..1) only arrived as -1, 0 or 1, and an unset (INT32_MAX)
value drove the actuator to saturation instead of leaving it unchanged.

Scale them by 1e7 like COMMAND_INT does, map unset to NaN, and encode them
the same way on download.

Co-authored-by: Hamish Willee <hamishwillee@gmail.com>
Signed-off-by: Julian Oes <julian@oes.ch>
Assisted-by: Claude:claude-opus-5-5

* docs(navigator): list all supported mission commands

Co-authored-by: Hamish Willee <hamishwillee@gmail.com>
Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Julian Oes <julian@oes.ch>

* fix(mavlink): keep ARM_DISARM and AUTOTUNE params on global-frame download

A global-frame mission item zeroes param1-4 before filling in the
command-specific ones, and COMPONENT_ARM_DISARM and DO_AUTOTUNE_ENABLE
had no case there. An uploaded "arm" (param1 = 1) therefore downloaded
as "disarm" (param1 = 0).

Signed-off-by: Julian Oes <julian@oes.ch>
Assisted-by: Claude:claude-opus-5-5

* fix(mavlink): ignore unset COMMAND_INT x and y individually

COMMAND_INT x/y were only treated as unset when both were INT32_MAX. A
DO_SET_ACTUATOR setting only actuator 5 (y = INT32_MAX) therefore set
actuator 6 to 214.7, saturating it, instead of leaving it unchanged.

Map each of x and y to NaN on its own when unset, which also covers
INT32_MIN from a NaN converted to int.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Julian Oes <julian@oes.ch>
Co-authored-by: Julian Oes <julian@oes.ch>
2026-11-17 10:45:39 +13:00
Jonas Perolini 722ebc9635 refactor(mavlink): reduce mission-handler nesting (#29791)
* refactor(mavlink): reduce mission-handler nesting

Signed-off-by: jonas <jonspero@me.com>

* refactor(mavlink): drop negation in mission request list busy check

Assisted-by: Claude:claude-opus-5-5

---------

Signed-off-by: jonas <jonspero@me.com>
2026-11-16 12:04:40 -07:00
blar 7e4b18611a fix(rover): hold a straight line when reversing in Position mode (#29773)
The differential and Ackermann position controllers measured the
heading error against the bearing to the target, also when driving in
reverse, where the rover has to point away from that bearing. A rover
that was lined up to reverse therefore had a heading error of 180 deg.
The differential rover spot turned because of it, and since the yaw
setpoint is only turned around while the speed setpoint is negative, it
kept alternating between turning towards the bearing and away from it.
The Ackermann rover applied the RO_SPEED_RED speed reduction for that
error. Turn the yaw setpoint around before the heading error is
calculated.

Position mode stored the bearing of its desired track when the track
hold started. Moving the stick from forwards to reverse without resting
at zero kept that bearing, so the target stayed in front of the rover,
which turned around to back up towards it. Store the heading of the
track instead and take the bearing from the sign of the stick, so the
rover reverses along the line it drove forwards on.

Assisted-by: Claude:claude-fable-5-2

Signed-off-by: blar-tw <blar.huang@gmail.com>
2026-11-16 12:17:04 -05:00
Silvan FuhrerandMahima Yoga 18f0524791 fix(commander): continue fixed-wing mission takeoff without position (#29719)
* fix(commander): continue fixed-wing mission takeoff without position

A fixed-wing vehicle losing its position estimate during Takeoff mode keeps
climbing wings-level (FW_POSCTRL_MODE_AUTO_TAKEOFF_NO_NAV), because fixed-wing
Takeoff mode does not require a position. During the takeoff item of a
mission, the mode fallback instead checked the Mission requirements and
escalated to Descend.

Check the mode fallback against the Takeoff mode requirements while a
fixed-wing vehicle flies the takeoff item of a mission, so both behave the
same. Once the takeoff item is done, the mission requirements apply again.

Assisted-by: Claude:claude-opus-5-5

* feat(commander): add FW mission takeoff state to failsafe simulator

The failsafe state now distinguishes the takeoff item of a fixed-wing
mission, which only needs the Takeoff mode requirements. Expose it in the
simulator so this fallback can be tried out there too.

Assisted-by: Claude:claude-opus-5-5

* Update docs/en/flight_modes_fw/mission.md

Co-authored-by: Mahima Yoga <mahima@auterion.com>

* Update docs/en/flight_modes_fw/mission.md

Co-authored-by: Mahima Yoga <mahima@auterion.com>

---------

Co-authored-by: Mahima Yoga <mahima@auterion.com>
2026-11-16 11:49:11 +01:00
Christian Rauch 169b2bc88d fix(system_identification): reset _y_lpf (#29805)
SystemIdentification::reset calls _u_lpf.reset(0.f); twice. This is very likely a typo, where the intention was to call _y_lpf.reset(0.f); instead.
2026-11-16 11:25:12 +01:00
Mathéo Taillandier 336eafd0c5 fix(drivers): fix wrong RPM in UAVCAN ESC telemetry (#29580) 2026-11-16 08:55:04 +01:00
Andrea BernasconiandHamish Willee db94ca49db feat(failure_injection): add wrong, garbage and drift IMU failures (#29601)
* feat(failure_injection): add wrong, garbage and drift injection for gyro and accel

Corrupt the addressed IMU's samples on all three axes: wrong adds a
constant bias (SYS_FAIL_{G,A}_BIAS), garbage uniform noise within
+-SYS_FAIL_{G,A}_NOISE and drift a bias growing by SYS_FAIL_{G,A}_DRIFT
per second since injection. Applied to both the plain and FIFO driver paths.

* refactor(failure_injection): handle IMU failures in process_imu()

Mirror process_gnss(): resolve the mode once, run the generic Off / Stuck
first and only then apply the Wrong / Garbage / Drift offset, so the Stuck
cache keeps the uncorrupted sample and a later Stuck never replays a
drifted one. Drop the second process() on the integrated FIFO report, which
cached the already-corrupted report. The offset is still computed before
Off so drift_start is reset while the sensor is off.

* docs(failure_injection): align IMU failure docs with code

The Syntax section did not list drift and described garbage as
uninitialised-memory output, while gyro/accel garbage adds bounded noise
to the real measurement. Also match the new parameter descriptions to
the wording of the existing SYS_FAIL_* parameters.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Hamish Willee <hamishwillee@gmail.com>

* fix(failure_injection): add drift RC mode and clarify bias param text

* refactor(drivers): Drop extra comment

* test(mavsdk_tests): add SITL test for IMU failure injection

Fly the SIH quad through gyro and accel bias, noise and drift with the
default SYS_FAIL_{G,A}_* magnitudes and check it holds altitude.

Holding altitude alone also passes when the injection never reaches the
sensor data, so the test also checks that the IMU noise level jumps
while noise is injected and returns once cleared. Bias and drift are
not checked in the data: the rate controller and the estimator react
to them and partly hide them in flight.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Andrea Bernasconi <andrea.bernasconi@auterion.com>

* test(mavsdk_tests): inject IMU failures through MAVSDK 4.2.0

MAVSDK-Python 4.2.0 adds FailureType.DRIFT (mavlink/MAVSDK#3211), so the
IMU failure test no longer needs to send a raw MAV_CMD_INJECT_FAILURE
with its own copies of the unit and type values. Use the tester's
inject_failure() like the motor failure tests, and bump the mavsdk pin
so the new type is available.

Assisted-by: Claude:claude-fable-5-2
Signed-off-by: Andrea Bernasconi <andrea.bernasconi@auterion.com>

---------

Signed-off-by: Hamish Willee <hamishwillee@gmail.com>
Signed-off-by: Andrea Bernasconi <andrea.bernasconi@auterion.com>
Co-authored-by: Hamish Willee <hamishwillee@gmail.com>
2026-11-13 11:24:05 +01:00
Michael Fritsche e8b3274ab0 feat(msg): rename rover throttle setpoint to surface_vehicle_thrust_setpoint (#29777)
* feat(msg): rename rover throttle setpoint to surface_vehicle_*

The throttle setpoint was the last rover topic shared by rovers and boats
without the surface_vehicle_* prefix decided in #29524. Rename
RoverThrottleSetpoint to SurfaceVehicleThrottleSetpoint and the
TYPE_ROVER_THROTTLE_* setpoint types to TYPE_SURFACE_VEHICLE_THROTTLE_*.
The type values are unchanged.

The topic rename breaks ROS 2 users anyway, so also shorten the fields of
the throttle and speed setpoints to x and y: the message name already says
what the value is and [@frame Body] states the frame.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Michael Fritsche <frimi999@gmail.com>

* fix(zenoh): bring surface_vehicle rename to the zenoh dds topics

* fix(concept): rename rover throttle setpoint to surface_vehicle_* in navigation terminology

* fix(tools): update px4-ros2-interface-lib pin

* docs(msg): describe sideways fields as omnidirectional, not mecanum

The surface vehicle messages are shared by rovers and boats, and the lateral
axis applies to any vehicle that can move sideways, not only mecanum rovers.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Michael Fritsche <frimi999@gmail.com>

* docs(ros2): use the surface vehicle setpoint type names of the interface lib

The pinned px4-ros2-interface-lib names the rover setpoint types
SurfaceVehicle*SetpointType. The table and the example reference still used
the old Rover*SetpointType class names, which do not exist in that version.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Michael Fritsche <frimi999@gmail.com>

* feat(msg)!: rename surface vehicle throttle setpoint to thrust setpoint

The setpoint is a normalized force command along the body x and y axes,
which is a thrust, not a throttle. Throttle stays the name for the motor
command the actuator control derives from it.

surface_vehicle_throttle_setpoint is now surface_vehicle_thrust_setpoint
(SurfaceVehicleThrustSetpoint), and SetpointConfig
TYPE_SURFACE_VEHICLE_THROTTLE_* are now TYPE_SURFACE_VEHICLE_THRUST_*
(same values). The px4-ros2-interface-lib pin follows the matching rename
there.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Michael Fritsche <frimi999@gmail.com>

* fix(tools): update px4-ros2-interface-lib pin

* fix(commander): do not enable velocity control for surface vehicle thrust setpoints

The SurfaceVehicleThrust* setpoint types enabled the velocity flag, taken
over from the original interface lib configuration. Nothing in these types
uses velocity, and offboard mode maps the same combinations without it.

The flag kept the rover speed controller running: switching from a
SurfaceVehicleSpeed* to a SurfaceVehicleThrust* type sets the same flags,
so the controllers were not reset and a stale speed setpoint kept
overriding the commanded thrust. It also made external modes that only
command thrust require a local position estimate.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Michael Fritsche <frimi999@gmail.com>

* docs(msg): apply review suggestions for the surface vehicle setpoints

Add long descriptions to the thrust and speed setpoints and the speed
status, name modules instead of C++ classes, and use single spaces before
field comments. Rename the speed status PID integrals from pid_throttle_*
to pid_thrust_*, since the speed controller drives the thrust setpoint.

Update the docs and the rover control structure diagram for the thrust
setpoint and omnidirectional vehicles, note the Rover*SetpointType names of
PX4 v1.17 and v1.18, and add the ROS 2 release note.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Michael Fritsche <frimi999@gmail.com>

---------

Signed-off-by: Michael Fritsche <frimi999@gmail.com>
2026-11-13 10:05:26 +01:00
Matthias Grob 8e94784e1d refactor(flight_mode_manager): remove unused code
ManualVelocitySmoothingXY has had no user since
FlightTaskManualPositionSmoothVel was removed. Its position lock test
after a reset moves to the still used ManualVelocitySmoothingZ.

Also remove members and methods of the flight tasks that are never
used: the acceleration setpoint feedback and the activation timestamp
were only written, updateVelocityControllerFeedback(), errorToString()
and StickAccelerationXY::getVelocityConstraint() were never called,
and _accelerationValid(), setGearAccordingToSwitch() and
_updateYawCorrection() were declared without a definition.
2026-11-12 19:56:01 +01:00
Matthias Grob 85228f2111 fix(motion_planning): plan trajectories before integrating them 2026-11-12 19:56:01 +01:00
Matthias Grob c339b0a8c8 refactor(simulator_sih): keep terrain argument parsing out of NuttX builds (#29778)
Select the terrain model directly in sih.hpp, dropping the extra
Terrain.hpp header. Only build load_terrain() and the -t usage for POSIX
because NuttX has no DEM to load, and make TerrainFlat::load() non-virtual
so it is no longer linked there. This saves 368 bytes of flash on
px4_fmu-v6c_default.

SITL builds without the DEM libraries still refuse to start when given a
terrain file.
2026-11-12 13:54:44 +01:00
Jonas PeroliniandMatthias Grob ef0990ae36 fix(vtol_att_control)!: don't treat a commanded quadchute as a failure (#29626)
* fix(vtol_att_control): do not treat a commanded quad-chute as a fixed-wing failure

An operator can command a quad-chute (MAV_CMD_DO_VTOL_TRANSITION with
param2 = 1) when nothing has failed, e.g. to stop fixed-wing flight for
detect and avoid. It set fixed_wing_system_failure like a detected
failure, so COM_QC_ACT ran (Return by default) and arming was blocked
until reboot.

A commanded quad-chute now only forces the immediate switch to
multicopter, through a one-shot latch. The multicopter transition
command keeps the vehicle there until the next front transition.

The destructor now also deletes the VTOL type, which leaked.

Assisted-by: Claude:claude-opus-5-5

* refactor(vtol_att_control): track quad-chute by reason

Only a detected quad-chute is published as fixed_wing_system_failure.

* fix(vtol_att_control): no pusher assist after a commanded quad-chute

A further quad-chute is ignored while one is active.

* fix(vtol_att_control): reset commanded quadchute on disarm

* feat(vtol_att_control): keep pusher assist after a commanded quadchute

---------

Co-authored-by: Matthias Grob <maetugr@gmail.com>
2026-11-12 12:31:23 +01:00
cb00217286 feat(sih): add optional support for non-flat terrain for MC, FW and VTOL (#29619)
* feat(simulator_sih): add a GeoTIFF terrain elevation model for SITL

SIH only simulates flat ground. This adds the loader for a terrain
elevation model (DEM) in a GeoTIFF file, the base for non-flat terrain
in SITL.

The DEM is read with libtiff and libgeotiff. A DEM in a projected CRS is
resampled once at load time with PROJ onto a WGS84 lat/lon grid, so a
lookup in the simulation loop is a plain bilinear interpolation. It is
built only for px4_sitl and only when the three libraries are found, so
hardware builds and SITL without them are unchanged.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Beniamino Pozzan <beniamino.pozzan@auterion.com>

* feat(simulator_sih): use the terrain DEM for ground contact and the rangefinder

With PX4_SIH_TERRAIN set to a GeoTIFF DEM, SIH in SITL places the vehicle
on the terrain at start, lands it on the terrain, ray-casts the downward
distance sensor against it, and publishes it as the groundtruth
terrain_alt. Outside the DEM the ground stays flat at SIH_LOC_H0.

Terrain is only used for multicopters, VTOLs and fixed-wing; other
vehicle types keep flat ground. The DEM is loaded in task_spawn() so a
bad file fails the start command, and because the SIH task stack is too
small for libtiff and PROJ.

The groundtruth terrain_alt now holds the WGS84 terrain altitude, as the
message documents, instead of the height above the origin.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Beniamino Pozzan <beniamino.pozzan@auterion.com>

* build(setup): install the SIH terrain dependencies with the simulation tools

Install libtiff, libgeotiff and PROJ so SITL builds SIH with terrain
elevation (DEM) support. They are optional: without them SIH builds as
before.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Beniamino Pozzan <beniamino.pozzan@auterion.com>

* docs(sim_sih): document terrain elevation from a GeoTIFF DEM

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Beniamino Pozzan <beniamino.pozzan@auterion.com>

* docs(sim_sih): fix and clarify the terrain elevation section

Address review findings on the new terrain section: list the sample
types the loader actually reads, note that the prebuilt px4 package and
px4io/px4-sitl container are built without terrain support, and say
when SIH_LOC_H0 still sets the start altitude.

Also add a version badge, link GeoTIFF, EPSG and GDAL, mention the
dist_bottom groundtruth field, and correct the GLO-30 coverage.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Hamish Willee <hamishwillee@gmail.com>

* fix: cast to double for MAC builds

Signed-off-by: Beniamino Pozzan <beniamino.pozzan@auterion.com>

* chore: implement TerrainBase interface

Assisted-by: Claude:claude-opus-5-5

Signed-off-by: Beniamino Pozzan <beniamino.pozzan@auterion.com>

* chore: improve isolation of terrain logic from sih core

Signed-off-by: Beniamino Pozzan <beniamino.pozzan@auterion.com>

* refactor(simulator_sih): move DEM handling into the terrain classes

SIH should only use the terrain, not manage it. TerrainFlat becomes the
base class, since flat ground is what every terrain model builds on, and
TerrainDem falls back to it until a DEM is loaded. Terrain.hpp picks the
model at compile time, so SIH holds the terrain by value with no #ifdef,
new/delete or pointer handover, and NuttX builds only get the flat model.

Tracking whether the vehicle is inside the DEM, and the matching
warnings and status, move from SIH into TerrainDem.

The DEM is still loaded in task_spawn() because the SIH task stack is too
small for libtiff and PROJ, so the terrain is a static member that the
instance uses directly instead of moving it.

A terrain file with an unsupported vehicle type now fails the start like
a bad file instead of silently using flat ground.

---------

Signed-off-by: Beniamino Pozzan <beniamino.pozzan@auterion.com>
Signed-off-by: Hamish Willee <hamishwillee@gmail.com>
Co-authored-by: Hamish Willee <hamishwillee@gmail.com>
Co-authored-by: Matthias Grob <maetugr@gmail.com>
2026-11-12 16:49:11 +11:00
Hamish Willee 5a533b77d1 fix(mavlink): accept DO_SET_ROI_WPNEXT_OFFSET pitch/roll/yaw offsets (#29758)
The param validation table allowed only param1 for
MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET, but params 5-7 carry the pitch, roll and
yaw offsets that navigator copies into vehicle_roi and the gimbal applies.

As a result, every QGroundControl Structure Scan upload was rejected
(INVALID_PARAM5, or INVALID_PARAM7 with a zero gimbal pitch, because QGC
sends a 90 degree yaw offset), and the command form was denied.

Assisted-by: Claude:claude-opus-5-5

Signed-off-by: Hamish Willee <hamishwillee@gmail.com>
2026-11-12 13:49:55 +13:00
Jacob Dahl e192036345 fix(commander): ack calibration cancels once and stop promptly (#29514)
* feat(simulator_sih): add orient command to pose the vehicle for calibration

Accel and mag calibration need the vehicle held still on each side and
rotated, which SIH could not do, so neither could be tested in
simulation. `simulator_sih orient` sets the attitude kinematically,
keeping the IMU and the simulated mag consistent with it, and `orient
all` runs every side in turn so a calibration started from a GCS
completes on its own.

Assisted-by: Claude:claude-opus-5-5

* fix(commander): hand a calibration cancel to the running calibration

A cancel got TEMPORARILY_REJECTED from Commander and ACCEPTED from the
calibration, in either order. The calibration polled for it with a
fresh vehicle_command subscription that only saw the newest command,
filtered it by a per-side start time, and did not poll while waiting
for the vehicle to be still or while measuring a side. Baro and level
calibration never polled. A cancel could be lost or take until the
vehicle was held still.

Commander now hands the cancel to the running calibration, which acks
it once when it stops, or rejects it if it ends without stopping.
Every calibration loop checks for it, and accel calibration no longer
reports a failure after a cancel.

Fixes #29424

Assisted-by: Claude:claude-opus-5-5

* test(mavsdk_tests): run accel and mag calibration and cancels on SIH

Covers a full accel and mag calibration, and a cancel in each place it
used to be missed or delayed, checking it gets exactly one ack.

Assisted-by: Claude:claude-opus-5-5

* perf(simulator_sih): build the orient command for SITL only

Commander denies sensor calibration when SIH runs on flight controller
hardware, so orient was about 1.8 KB of unusable code on every NuttX
board that builds SIH.

Assisted-by: Claude:claude-opus-5-5

* fix(commander): open calibration cancels before the worker thread starts

Commander handles one command per loop and the worker thread runs at
low priority, so a cancel queued right behind the start could be
rejected before the worker opened it, and the calibration kept running.

Assisted-by: Claude:claude-opus-5-5

* fix(commander): stop reporting a cancelled gyro calibration as failed

A cancel broke out of the retry loop into the failure report, as accel
calibration did before.

Assisted-by: Claude:claude-opus-5-5

* test(mavsdk_tests): cover gyro cancel and remove two calibration races

A retried start is rejected once the calibration runs, and that late
ack could be counted against the cancel, so the start is sent once.
The baro cancel landed 1 s into a 3 s calibration, about 200 ms of
host time at speed factor 10; it is now sent right after the start.

Assisted-by: Claude:claude-opus-5-5

* style(mavsdk_tests): wrap the calibration start assert for flake8

The 104-character line failed the Python CI checks.

Assisted-by: Claude:claude-opus-5-5

* fix(commander): require param7 zero for a calibration cancel

An ESC calibration (param7 = 1) or baro temperature calibration
(param7 = 3) sent while a calibration ran matched the cancel test. It
stopped the running calibration and was acked ACCEPTED, though the
requested calibration never started. `commander calibrate esc` did the
same.

Assisted-by: Claude:claude-fable-5-2
2026-11-11 14:38:01 -07:00
g945643frankieandFarhang 56ebfa63c8 feat(drivers/imu): add ICM-56686 driver and enable it on GA1 (#28677)
feat(drivers/imu): add ICM-56686 driver and enable GA1 alternate IMUs

Add an InvenSense ICM-56686 SPI FIFO driver (WHO_AM_I 0x08, DEVTYPE
0x39, DS-000563 v1.1 register map, 6400 Hz FIFO). It publishes the
16-bit FIFO fields at full scale (as in #28134) and passes
config.external to the publishers (as in #28352).

GA1 (accton-godwit):
- Each internal SPI socket can hold an ICM-42688P, ICM-45686,
  ICM-56686 or LSM6DSV32X/LSM6DSK320X. Probe them in an if/fi chain
  and stop at the first one found. SPI2 is probed first so the heater
  binds to the heated instance.
- The LSM6DSV now uses the same rotation as the Invensense parts on
  each socket (-R 6 on SPI2, -R 14 on SPI1), following #27625.
- The ICM-42688P and ICM-45686 on SPI2 are polled instead of using
  DRDY, because only INT2 is routed to the MCU there. This removes
  the missed-DRDY counts.
- Drop six drivers that nothing starts at boot (BMP388, MS5611,
  BMM150, MMC5983MA, MCP23009, landing target estimator).
  Flash use: 98.67% -> 97.52%.

Tested on GA1 hardware with a v1.17-based build: SPI2 polling and the
driver cleanup. Not yet tested on hardware: the ICM-56686 decode
change and the LSM6DSV rotation.

Assisted-by: Grok:grok-4.6
Assisted-by: Kiro:claude-opus-5.5
Signed-off-by: g945643frankie <g945643@oz.nthu.edu.tw>
Co-authored-by: Farhang <46557204+farhangnaderi@users.noreply.github.com>
2026-11-10 23:03:32 -05:00
Jacob Dahl 91a4c21ca8 refactor(drivers): share the start/stop/status dispatch of I2C/SPI drivers (#29556)
90 driver main functions repeated the same three strcmp() checks and
calls. I2CSPIDriver<T>::module_main() does it once; drivers with extra
verbs keep their own dispatch.

Assisted-by: Claude:claude-opus-5-5
2026-11-10 15:58:59 -07:00
Jonas Perolini c1d5757020 feat(navigator): skip automatic front transitions after a quadchute (#29625) 2026-11-10 18:45:59 +01:00
Jonas Perolini 4f7e35a06a fix(navigator)!: reject a DO_JUMP that targets a DO_JUMP before a waypoint (#29566)
Assisted-by: Claude:claude-opus-5-5
2026-11-10 18:45:08 +01:00
Michael Fritsche 240d6c36cc feat(rover): make the manual mode stick mapping configurable (#29640)
* feat(rover): make the manual mode stick mapping configurable

Assisted-by: Claude:claude-opus-5-5

* fix(rover_mecanum): apply the stick mapping to manual parking sideways driving

Manual Parking kept driving sideways with the roll stick, which is the
turn stick in the default mapping. Switching from another manual mode
into Manual Parking while turning made the rover drive sideways instead.
The tank-style sides only use the vertical axes and the sideways
assignment is always a horizontal axis, so it can follow RO_STICK_MAP.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Michael Fritsche <frimi999@gmail.com>

* docs(rover_control): shorten the RO_STICK_MAP description

The option labels already list which stick does what, and parameter
descriptions ship in ROMFS on boards with flash to spare.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Michael Fritsche <frimi999@gmail.com>

* docs(rover): mention RO_STICK_MAP in the rover basic setup

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Michael Fritsche <frimi999@gmail.com>

* docs(rover): describe motions instead of stick positions in the tuning guides

The stick positions depend on RO_STICK_MAP, so name the motion to drive
and link to the stick mapping instead.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Michael Fritsche <frimi999@gmail.com>

* docs(rover): update the rover manual controls image to comply with the new stick mapping

* docs(rover): fix CI by re-adding rover_manual_controls.png

---------

Signed-off-by: Michael Fritsche <frimi999@gmail.com>
2026-11-10 16:48:54 +01:00
e03e075003 feat(rover): add high level encoder support (#28848)
* feat(rover): add high level encoder support

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

* fix(rover): add gz mixing interface to output encoder position

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

* fix(rover): add wheel encoder to logged topics, high rate sensors

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

* fix(rover): fix wheel encoder division by zero and stream iteration

Corrected Roboclaw driver to initialize counts_per_rev, fixed wheel encoder stream to iterate over multiple wheels, and added trapezoidal integration to GZMixingInterfaceWheel. Removed unused RO_WHEEL_RAD from position controllers and restricted the wheel encoder stream to rover targets.

Assisted-by: Qwen-Code

Co-authored-by: Qwen-Coder <qwen-coder@alibabacloud.com>
Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

* fix(rover): adjust wheel radius default for SITL

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

* Update src/lib/rover_control/rovercontrol_params.yaml

Co-authored-by: Michael Fritsche <64464652+Frigiii@users.noreply.github.com>

* Update src/lib/rover_control/rovercontrol_params.yaml

Co-authored-by: Michael Fritsche <64464652+Frigiii@users.noreply.github.com>

* Update msg/WheelEncoders.msg

Co-authored-by: Michael Fritsche <64464652+Frigiii@users.noreply.github.com>

* chore(rover): fixes to decimal and constrained flash mavlink message

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

* chore(gz): update encoder counts reference

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

* chore(rover): fix to outdated encoder pos reference

* fix(rover): update uorb message to account for 4 wheel encoders

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

* Update msg/WheelEncoders.msg

Co-authored-by: Michael Fritsche <64464652+Frigiii@users.noreply.github.com>

* fix(roboclaw): add in encoder position, counts per rev. Msg desc update

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

* chore(wheel_encoders): add description to top of msg file for docs

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

* chore(sim): fixes to initialize wheel pos m, rename wheel_encoder to wheel_distance

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

* Update src/lib/rover_control/rovercontrol_params.yaml

Co-authored-by: Michael Fritsche <64464652+Frigiii@users.noreply.github.com>

* Update src/modules/mavlink/streams/WHEEL_DISTANCE.hpp

Co-authored-by: Michael Fritsche <64464652+Frigiii@users.noreply.github.com>

* Update src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp

Co-authored-by: Michael Fritsche <64464652+Frigiii@users.noreply.github.com>

* Update src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp

Co-authored-by: Michael Fritsche <64464652+Frigiii@users.noreply.github.com>

* chore(rover): update wheel encoder msg, fixes to gz bridge

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

* chore(rover): Apply batched suggestions from code review

Co-authored-by: Michael Fritsche <64464652+Frigiii@users.noreply.github.com>

---------

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>
Co-authored-by: Qwen-Coder <qwen-coder@alibabacloud.com>
Co-authored-by: Michael Fritsche <64464652+Frigiii@users.noreply.github.com>
2026-11-10 08:54:19 -05:00
Jonas Perolini 908f99ad32 refactor(navigator): trim the Return destination selection stack (#29669)
setRtlTypeAndDestination() kept room in its frame for a
battery_status_s, used only by the RTL_TYPE 6 battery loop, and for a
copy of the global position, also while it looked up rally points and
VTOL landing approaches. getVtolLandApproachesNearLocation() returned a
different object on its early exit, so GCC did not apply NRVO and its
208 B land_approaches_s result was on the stack twice.

Read the batteries in a separate function, take the global position by
reference and return one named result. On fmu-v6c the two frames drop
from 336 B to 176 B and from 272 B to 80 B.

Assisted-by: Claude:claude-opus-5-5

Signed-off-by: jonas <jonspero@me.com>
2026-11-10 10:58:16 +01:00
440179383a feat(SIH): Added optical flow sensor continues on #28270 (#29676)
* feat(sih): model the distance sensor range limits, quality and noise

The simulated distance sensor reported a noise free reading with no signal
quality, and saturated only against the upper range limit. Readings below the
minimum range were passed through unchanged, which is not what a range finder
does.

Report the raw saturated value with a zero quality outside of the measurement
range, derate the quality over the last 20% of the range, and add configurable
Gaussian noise (SIH_DISTSNSR_NOI). The quality is decided on the true distance
so that a measurement sitting at a range limit does not flicker in and out of
validity.

The sensor is now enabled with SIH_DISTSNSR_EN instead of by an out of band
value of the override parameter, and the ground distance computation moves into
a helper.

* feat(sih): add optical flow simulation

Simulate a downward facing optical flow sensor in SIH so that flow aided
navigation can be exercised without a rendered camera image. The flow rate is
the sum of the body rates and of the ground relative velocity scaled by the
distance to the ground, integrated over one publication interval and published
on sensor_optical_flow with configurable rate (SIH_OF_RATE) and noise
(SIH_OF_NOISE).

The sensor reports the integrated flow only: the flow module completes the
sample with the vehicle gyro and the downward facing distance sensor, which
therefore has to be enabled as well. Quality drops to zero above
SIH_OF_H_MAX or when the flow rate exceeds what the sensor can correlate.

The topic is advertised before the simulated clock starts, because the sensors
module brings up its flow pipeline on the first appearance of the topic, and
publishing during startup deadlocks the lockstep clock this thread advances.

Add the quadx_flow and standard_vtol_flow models with matching airframes:
SYS_AUTOSTART 10046 flies the multicopter on flow only with no GNSS, 10047
fuses the flow next to GNSS on a VTOL.

* prune configs

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* more config cleanup

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* added EKF2_HGT_REF to vtol flow config

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* feat(simulation): added tests for optical flow for both quadx and stanard vtol
modes and enable them in CI

- for the test we monitor error on ground truth position and velocity

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* feat(docs): document addition of optical flow in SIH

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* fix(mavsdk_tests): unsubscribe flow ground truth comparison on destruction

The ground truth comparison callbacks capture `this` and touch members of
AutopilotTesterFlow. If a REQUIRE fails between start and stop, the tester
is destroyed during unwinding while the subscriptions are still active,
so a callback could run on the destroyed mutex and sample buffers.

Make stop idempotent and call it from the destructor, and stop any running
comparison before starting a new one.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: RomanBapst <bapstroman@gmail.com>

* tests for python

* fix(mavsdk_tests): start the VTOL flow loiter already on its radius

The loiter was centered on the vehicle's position right after the
transition, so it started at the center and had to fly straight out to the commanded
radius before the lateral guidance curved onto the track.

Assisted-by: Claude:claude-sonnet-5

* fix(mavsdk_tests): resend the flow test EKF origin until it is taken

The SITL startup script starts ekf2 after MAVLink. When MAVSDK connected
first, the single SET_GPS_GLOBAL_ORIGIN was published before ekf2 existed
and MAVSDK's RUN_PREARM_CHECKS followed it. Ekf2 then subscribed, saw
only the newest vehicle command and never got an origin. The Coverage
build starts slowly enough to hit this every time.

Assisted-by: Claude:claude-opus-5-5

* ci(sitl_tests): run the VTOL flow test without coverage

As a Coverage job it built with coverage on every PR and uploaded the
sitl-vtol flag from PRs, which replaced main's carried-forward VTOL
coverage with the flow-only flight. The quadx_flow entry also sat
between the Coverage comment and the standard_vtol entry it describes.

Assisted-by: Claude:claude-opus-5-5

* fix(mavsdk_tests): sum the flow test heading sweep with its sign

Adding up the absolute heading changes counted yaw oscillation toward the
360 degrees, so the wait could end before a full orbit was flown.

Assisted-by: Claude:claude-opus-5-5

* refactor(airframes): keep only the non-default params in the SIH flow airframes

SIH_OF_H_MAX, SIH_DISTSNSR_EN and EKF2_OF_CTRL were set to their defaults
and SENS_EN_GPSSIM is already set by the quadx base airframe. The quadx
forced EKF2_RNG_CTRL to 2 while the VTOL used the conditional default for
no stated reason, so both now use the default.

Assisted-by: Claude:claude-opus-5-5

* docs(sim_sih): mark the SIH flow airframes as new in PX4 v2.0

release/1.18 has branched, so these airframes first ship in v2.0, not
v1.17.

Assisted-by: Claude:claude-opus-5-5

---------

Signed-off-by: RomanBapst <bapstroman@gmail.com>
Co-authored-by: RomanBapst <bapstroman@gmail.com>
Co-authored-by: Jacob Dahl <dahl.jakejacob@gmail.com>
2026-11-09 11:13:16 -07:00
Matthias Grob 7e34e87d3a fix(mavlink): keep ADS-B receiver reporting online when it saturates on traffic stream (#29385)
Some ADS-B receivers, e.g. the pingRX Pro, give their traffic reports
priority over HEARTBEAT when there is a lot of traffic. The heartbeat then
times out and the traffic avoidance system shows as lost, which can trigger
the traffic_avoidance_unhealthy failsafe, while traffic is still
arriving.

ADSB_VEHICLE messages from the component that announced itself as
MAV_TYPE_ADSB now also reset the ADS-B liveness timeout. At least one
heartbeat is still needed to identify the receiver. Traffic from other
systems or components, such as a GCS forwarding traffic, does not keep it
online.
2026-11-09 17:03:41 +01:00
Michael Fritsche d34142bc4e refactor(rover): run each rover controller on its own trigger topic (#29630)
* refactor(rover): run each rover controller on its own trigger topic

The rover modules ran the whole controller cascade from the gyro callback,
so the position and speed controllers ran several times per EKF output on
stale estimates. Each controller is now a work item triggered by the topic
it consumes (local position, attitude, angular velocity, steering
setpoint), and the module only generates setpoints and enables the
controllers the active control mode needs. All work items share the
rate_ctrl queue, so enabling and resetting never overlaps a controller run.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Michael Fritsche <frimi999@gmail.com>

* fix(rover): register controller callbacks top-down

Position and speed control share the vehicle_local_position trigger and
uORB schedules callbacks in registration order. Speed was registered
first, so it always consumed the previous cycle's position-control
setpoint, adding one EKF period of latency.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Michael Fritsche <frimi999@gmail.com>

* docs(rover): move controller update descriptions back to their functions

The init() and setEnabled() declarations were inserted between the
update*Control() doc comments and their functions, leaving two stacked
@brief blocks.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Michael Fritsche <frimi999@gmail.com>

* fix(rover): widen the dt clamp of the EKF-triggered controllers

Attitude and speed control now run on vehicle_attitude and
vehicle_local_position instead of the gyro. Their period is the EKF2
output period, nominally exactly the old 10 ms upper bound, so jitter or
a larger EKF2_PREDICT_US clipped dt and slowed the slew-rate limiters and
PID integrators below real time. Allow up to 50 ms.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Michael Fritsche <frimi999@gmail.com>

* docs(rover): correct the attitude controller dt comment

vehicle_attitude is published on every EKF2 output predictor update at
IMU_INTEG_RATE, not at the EKF2_PREDICT_US filter update rate.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Michael Fritsche <frimi999@gmail.com>

* fix(rover): restart the controller dt when a controller is re-enabled

A controller keeps its last run timestamp while it is disabled, so the
first run after a mode switch computed dt over the whole disabled time
and hit the upper clamp. With the speed and attitude clamps at 50 ms that
step was six EKF periods long for the slew limiters and integrators.
Restart the timestamp on enabling, so the first dt is the lower bound.

SIH rover (10045), position -> manual -> position while driving, speed
PID integral after the first re-enabled run: dt 50 ms before, 1 ms after
(normal runs 8 ms).

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Michael Fritsche <frimi999@gmail.com>

* fix(rover): pace the actuator controllers by the gyro

The actuator controllers ran only when a steering setpoint arrived. A
mode change resets the rate controller's setpoint to NaN, so the next
cycle published no steering setpoint and therefore no actuator setpoint.
SIH lockstep waits for an actuator output every step and froze there:
switching to stabilized or offboard velocity stalled the simulation.
Trigger the actuator controllers on vehicle_angular_velocity, registered
after the rate controller, so they publish every cycle with the steering
setpoint of the same cycle.

SIH rover (10045), manual/acro/stab/position/offboard velocity cycle, 3 runs
each: before all 3 runs froze (offboard switch in all, stab switch in one), after no
lockstep timeouts, actuator outputs every cycle (max gap 8 ms) and >= 99.7 %
of them use the steering setpoint of the same cycle.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Michael Fritsche <frimi999@gmail.com>

* perf(rover): only wake the enabled controllers

All controllers stayed registered on their trigger topics, so in every
mode and while disarmed the disabled ones were scheduled at up to gyro
rate just to return. Register a controller's callback when it gets
enabled and unregister it when it gets disabled. A newly enabled
controller runs once right away, because it registered after the trigger
of the current cycle fired; without that the lockstep simulation misses
the cycle's actuator output and stalls. When an upper controller is
enabled while the lower one sharing its trigger topic keeps running
(position/speed on vehicle_local_position, rate/actuator on
vehicle_angular_velocity), the lower one is re-registered behind it to
keep the cascade order.

SIH rover (10045), 3 runs each:
- work_queue status: disarmed only the module runs, manual adds the
  actuator controller, acro rate + actuator, stab + attitude, position all
  five, offboard velocity all but position; no lockstep timeouts.
- Value-based order check (servo command vs steering setpoint, speed
  controller setpoint vs position controller setpoint): every consumer
  uses the setpoint of the same cycle after manual -> acro, manual ->
  offboard velocity and offboard velocity -> Go-To. With the
  re-registration disabled the actuator controller used the previous
  cycle's steering setpoint after acro / offboard, and the speed
  controller the previous cycle's speed setpoint in Go-To (153 of 153).
- Re-enable probe: first dt after position -> manual -> position is still
  1-2 ms.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Michael Fritsche <frimi999@gmail.com>

* fix(rover): assign the rover control workitems to nav_and_controllers

* fix(rover): retry a failed controller callback registration

setEnabled() marked a controller enabled even when registering its
trigger callback failed. The next call then saw no change and never
retried, so the controller ran once from ScheduleNow() and then stayed
idle with a single error. A controller now only counts as enabled once
its callback is registered, so the next module cycle retries. A failed
re-registration for the cascade order clears it the same way. The
modules report a failure once instead of every cycle.

SIH rover (10045), attitude controller registration forced to fail for
the first 1500 attempts: the controller stays off while it fails and
starts at 250 Hz on the first successful retry, one error per failure
episode. Mode cycle without the fault unchanged.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Michael Fritsche <frimi999@gmail.com>

* refactor(rover): share the controller trigger logic in a base class

All 15 rover controllers carried the same copy of init(), setEnabled(),
enabled(), reregisterTrigger() and the enabled check in Run(). Copies of
the code that decides when a controller runs and in which order can
drift apart, so RoverControllerWorkItem in lib/rover_control now owns
the trigger subscription, the work queue and the enable state. A
controller only implements its step and, if it has a time step, the
restart on enabling.

The trigger only wakes the controller, so the controllers read their
inputs through plain subscriptions again. The actuator controllers no
longer drain the gyro topic: callbacks are scheduled on new
publications, not on unread data.

A functional test covers the contract the modules rely on: no runs
while disabled, one run right away on enabling, a run on every trigger,
registration order for controllers sharing a trigger and re-registering
to restore the cascade order.

SIH rover (10045), manual/acro/stab/position cycle: each mode wakes the
same controllers at the same rates as before, actuator setpoints at
250 Hz, no errors.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Michael Fritsche <frimi999@gmail.com>

* fix(rover): drop the protected destructor of the controller base class

clang-tidy (cppcoreguidelines-virtual-class-destructor) rejects a
destructor that is both protected and virtual. WorkItem already has a
public virtual destructor, so the implicit one is enough.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Michael Fritsche <frimi999@gmail.com>

---------

Signed-off-by: Michael Fritsche <frimi999@gmail.com>
2026-11-09 08:23:12 -05:00
Sofian ElmotiemandSofian Elmotiem 92b18d17f3 fix(msg): escape IDL keywords in uORB field names for idlc (#29696)
A uORB field named after an IDL keyword, such as `sequence` in several
MAVLink-M messages, made CycloneDDS idlc fail with a syntax error on any
build with CONFIG_LIB_CDRSTREAM, such as the Zenoh targets. Escaping the
name in the shared IDL is not enough: rosidl_parser, which computes the
RIHS01 type hash, rejects escaped identifiers.

Generate a second IDL copy for idlc with every field and constant name
escaped by a leading underscore. idlc strips the underscore, so the
generated C member names are unchanged. The hash step keeps reading the
plain IDL, so RIHS01 hashes and Zenoh key expressions are unchanged too.

Fixes #29600

Signed-off-by: Sofian Elmotiem <191787501+SofianElmotiem@users.noreply.github.com>
Co-authored-by: Sofian Elmotiem <191787501+SofianElmotiem@users.noreply.github.com>
2026-11-09 13:17:42 +00:00
Jonas Perolini ca6b8c8e03 fix(navigator): keep the mission-landing Return altitude current (#29684)
Assisted-by: Claude:claude-opus-5-5
2026-11-09 09:45:51 +01:00
Saibernard ded6314ffd fix(vtol): restart the tailsitter front transition mid back transition (#29608)
A tailsitter that got a fixed-wing request while still transitioning
back to hover switched straight to fixed-wing mode, even with the nose
part way up and the airspeed below the transition speed. The standard
VTOL had this removed in 2016 because it can result in an unsafe flying
state, and restarts the front transition instead. Do the same for the
tailsitter.

Signed-off-by: Saibernard Yogendran <bernie97@seas.upenn.edu>
2026-11-09 09:09:43 +01:00
Saibernard cdf9cfb28b fix(vtol): restart the tiltrotor front transition mid back transition (#29609)
* fix(vtol): restart the tiltrotor front transition mid back transition

A tiltrotor that got a fixed-wing request while still transitioning
back to hover switched straight to fixed-wing mode, which commands full
forward tilt in one step and starts the throttle blend from the hover
thrust on the front rotors, even with the rotors already part way to
vertical and the motors off. In Gazebo that can pitch the aircraft up
50 degrees and stall it. Keep the instant switch while the rotors are
still forward of VT_TILT_TRANS and otherwise restart the front
transition from the current tilt, as the standard VTOL does. The first
front transition phase sets the pitch weight and ramps the throttle
weight back up, since the back transition zeroes both while the rotors
rotate.

Signed-off-by: Saibernard Yogendran <bernie97@seas.upenn.edu>
2026-11-09 09:09:31 +01:00
jc-works d83b4e7df9 fix(airspeed_selector): skip validation checks during landing
Navigator publishes the active setpoint through position_setpoint_triplet.
Subscribing to the standalone dependency left the setpoint at its default
POSITION value, so the existing LAND exclusion was never applied and the
load-factor check could latch the airspeed sensor as invalid during landing.

Consume the current triplet setpoint so the intended exclusion takes effect
without changing existing failure-latching behavior.

Assisted-by: Codex:gpt-5
Signed-off-by: jc-works <jc-works@users.noreply.github.com>
2026-11-09 09:02:06 +01:00
Jonas Perolini defc7844a7 fix(navigator): move to the mission landing at altitude after the Return climb (#29688)
Assisted-by: Claude:claude-opus-5-5
2026-11-09 08:45:58 +01:00
f9088053ab fix(zenoh): px4_sitl_zenoh build on macOS (#29538)
zenoh is now part of px4_sitl_default, so the macOS SITL build compiles
it and fails in two places:

- msg: zenoh_topics was a static library with only a header as its
  source. macOS ar rejects the resulting empty archive ("no archive
  members specified"). It only orders header generation, so make it a
  custom target.
- zenoh: POSIX_COMPATIBLE makes zenoh-pico define ZENOH_LINUX ahead of
  its own Darwin detection, which pulls in the Linux-only getrandom().
  Set it only on non-Apple platforms. Linux is unaffected, and NuttX
  (CMAKE_SYSTEM_NAME Generic) keeps it.

Also mark the uORB_Zenoh_Subscriber overrides, consistent with the
publisher.

Signed-off-by: Beniamino Pozzan <beniamino.pozzan@gmail.com>
Co-authored-by: Farhang <46557204+farhangnaderi@users.noreply.github.com>
Co-authored-by: Beniamino Pozzan <beniamino.pozzan@gmail.com>
2026-11-08 22:15:08 -05:00
Jonas Perolini caf75ad3c1 fix(mavlink): report missing Return time estimates as unknown (#29685)
Assisted-by: Claude:claude-opus-5-5
2026-11-08 16:18:35 +01:00
Jonas Perolini 5dc8ab048f refactor(navigator): collapse Return's executor switches (#29683)
Assisted-by: Claude:claude-opus-5-5
2026-11-08 16:17:22 +01:00
treston 14a7aec820 fix(tunes): return defined note fields for rests and repeated tunes (#29132)
Tunes::get_next_note() returned Continue from the N0 rest and from the
end of a repeating tune without writing frequency and duration. The
MAVLink tune publisher and the tune_control command keep uninitialised
locals for them, publish them as they are and schedule the next note
from now + duration + silence, so a tune such as "N0" or "MBC" played
an indeterminate note after an indeterminate delay.

Zero frequency and duration on the N0 rest as P and R already do, and at
the end of the string so a repeating tune restarts through a zero-length
rest. Add a gtest for both paths.
2026-11-07 23:08:57 +13:00