Upixel UP-T1-001-Plus support and optical flow position hold - #14922
Conversation
|
Do you want to test this code? You can flash it directly from the Betaflight App:
WARNING: It may be unstable. Use only for testing! |
|
Note Reviews pausedIt looks like this branch is under active development. To avoid overwhelming you with review comments due to an influx of new commits, CodeRabbit has automatically paused this review. You can configure this behavior by changing the Use the following commands to manage reviews:
Use the checkboxes below for quick actions:
WalkthroughAdds a UART UPT1 rangefinder driver (optionally providing optical-flow), an optical-flow-based position estimator, optical-flow position-hold/autopilot paths, and rangefinder altitude integration into altitude fusion and CLI/configuration. Changes
Sequence Diagram(s)sequenceDiagram
participant OF as OpticalFlow (UPT1)
participant RF as Rangefinder (UPT1)
participant Est as Position Estimator
participant AP as Autopilot Controller
participant GPS as GPS
OF->>Est: flow_x, flow_y, quality, integration_time
RF->>Est: distance_cm, health
Est->>Est: validate quality & altitude, convert flow→velocity, integrate→position
Est->>AP: publish position, trust, source=OPTICALFLOW
AP->>AP: PID (P on vel, I on pos, II slow integral, D on accel)
AP->>Mixer: output body angles
alt Optical-flow invalid/low-trust
GPS->>AP: GPS position
AP->>Est: setActivePositionSource(GPS)
AP->>AP: reset optical-flow state, use GPS PID path
end
sequenceDiagram
participant UPT1 as UPT1 Rangefinder
participant AltF as Altitude Fusion
participant Baro as Barometer
participant GPSAlt as GPS Altitude
participant Nav as Navigation
UPT1->>AltF: distance_cm, health
AltF->>AltF: apply min/max gates and offset handling
AltF->>AltF: select ALTITUDE_SOURCE (RANGEFINDER_ONLY / PREFER / DEFAULT)
AltF->>Nav: provide selected altitude estimate
AltF->>Baro: fallback when configured
AltF->>GPSAlt: fallback when configured
Estimated code review effort🎯 4 (Complex) | ⏱️ ~60 minutes Possibly related PRs
Suggested labels
Suggested reviewers
🚥 Pre-merge checks | ✅ 2 | ❌ 1❌ Failed checks (1 warning)
✅ Passed checks (2 passed)
✏️ Tip: You can configure your own custom pre-merge checks in the settings. ✨ Finishing Touches🧪 Generate unit tests (beta)
Thanks for using CodeRabbit! It's free for OSS, and your support helps us grow. If you like it, consider giving us a shout-out. Comment |
There was a problem hiding this comment.
Actionable comments posted: 8
Caution
Some comments are outside the diff and can’t be posted inline due to platform limitations.
⚠️ Outside diff range comments (1)
src/main/flight/autopilot_multirotor.c (1)
380-519:⚠️ Potential issue | 🟠 MajorReset GPS EF PID state on optical‑flow → GPS transition.
On a source switch,
previousDistance/previousVelocity/integralmay be stale, causing D/A spikes. Resetting EF PID state on transition keeps the GPS handoff smooth.🛠️ Suggested reset on transition
if (lastActiveSource == POSITION_SOURCE_OPTICALFLOW) { // Transitioning from optical flow to GPS // Set GPS target to current GPS position to prevent jump ap.targetLocation = gpsSol.llh; + for (axisEF_e efAxisIdx = LON; efAxisIdx <= LAT; efAxisIdx++) { + efPidAxis_t *efAxis = &ap.efAxis[efAxisIdx]; + efAxis->previousDistance = 0.0f; + efAxis->previousVelocity = 0.0f; + efAxis->integral = 0.0f; + efAxis->isStopping = true; + } }
🤖 Fix all issues with AI agents
In `@src/main/drivers/rangefinder/rangefinder_upt1.c`:
- Around line 209-223: The out-of-range branch currently leaves hasUPT1RFNewData
false so callers see RANGEFINDER_NO_NEW_DATA; update the else branch for the
UPT1 range processing to mark the frame as new data by setting hasUPT1RFNewData
= true and (when USE_OPTICALFLOW is defined) also set upt1RFDistanceMm and
upt1RFTimestampUs = micros() so the rangefinder path treats out-of-range frames
as valid updates (do the same change for the other occurrence around the second
block that mirrors lines 266-271).
- Around line 195-206: The conversion of optical-flow samples is wrong: use
integration_time to compute dt in seconds and convert flow_x/flow_y (which are
radians*10000) to angular rate (rad/s) before storing in
upt1OpticalflowSensorData.flowRate.x and .y; compute float dt_s =
(float)integration_time / 1e6, check dt_s > 0, then set flowRate.x =
(float)flow_x / 10000.0f / dt_s and similarly for flowRate.y; also correct the
nearby comment that currently says "radians*100" to "radians*10000" and ensure
any DEBUG_SET or cast lines still reflect the signed flow_x/flow_y values.
In `@src/main/flight/autopilot_multirotor.c`:
- Around line 185-188: The pt2FilterGain call for flowDGain is passing the
inverse time step (1.0f / FLOW_DATA_INTERVAL) as the dt argument, producing a
huge dT; change the second argument to pass the actual time step
FLOW_DATA_INTERVAL (seconds) so compute flowDGain = pt2FilterGain(0.25f /
FLOW_DATA_INTERVAL, FLOW_DATA_INTERVAL) and leave the subsequent
pt2FilterInit(&flowDLpf[X], flowDGain) and pt2FilterInit(&flowDLpf[Y],
flowDGain) calls unchanged.
In `@src/main/flight/position_estimator.c`:
- Around line 149-157: The calculation for opticalFlowPosition.trust can divide
by zero when minQuality (posHoldConfig()->opticalflowQualityMin) equals 50;
modify the branch that computes trust to guard the denominator: compute float
denom = 50.0f - minQuality and if denom <= 0.0f set opticalFlowPosition.trust to
(quality >= 50 ? 1.0f : 0.0f), otherwise compute trust = (quality - minQuality)
/ denom; ensure the result is clamped to [0.0f, 1.0f]. This change touches the
block using flow->quality, minQuality and opticalFlowPosition.trust.
- Line 62: opticalFlowPositionOffset is assigned in positionEstimatorInit and
resetOpticalFlowPosition but never used; either remove the dead variable and its
assignments (delete opticalFlowPositionOffset and any reset calls) or apply it
to the position computation by subtracting/adding opticalFlowPositionOffset
where the optical flow-derived position or integration is performed (e.g., in
the function that integrates optical flow into the estimated position or returns
computed optical-flow position estimates). Update references in
positionEstimatorInit and resetOpticalFlowPosition accordingly and ensure
tests/consumers of the estimator reflect the change.
- Around line 129-144: The code currently integrates body-frame velocityBF (from
flow->processedFlowRates) directly into opticalFlowPosition.position (documented
as earth-frame), which is incorrect; compute the earth-frame velocity by
rotating velocityBF by the current vehicle yaw/heading (use the heading/yaw
source used elsewhere in state estimation), e.g., apply a 2D rotation using
cos(heading) and sin(heading) to produce velocityEF (earth-frame) and then
integrate velocityEF into opticalFlowPosition.position using the existing dt
logic (keep deltaTimeUs, cmp32, and sanity check). Ensure you reference and use
the same heading variable/field used by other estimators so heading changes are
correctly accounted for before integration.
In `@src/main/flight/position.c`:
- Around line 199-227: The altitudeAvailable flag currently only considers
GPS/Baro and thus stays false when ALTITUDE_SOURCE_RANGEFINDER_ONLY is selected;
update the availability logic (the code that sets altitudeAvailable) to treat a
valid rangefinder (haveRangefinderAlt) as satisfying altitude availability when
positionConfig()->altitude_source is ALTITUDE_SOURCE_RANGEFINDER_ONLY,
ALTITUDE_SOURCE_RANGEFINDER_PREFER, or ALTITUDE_SOURCE_DEFAULT (as appropriate),
ensuring the same constants used in the fusion block
(ALTITUDE_SOURCE_RANGEFINDER_ONLY, ALTITUDE_SOURCE_RANGEFINDER_PREFER,
haveRangefinderAlt) are checked and altitudeAvailable is set true so
rangefinder-based altitude enables altitude‑dependent features.
In `@src/main/sensors/opticalflow.c`:
- Around line 110-116: The OPTICALFLOW_UPT1 case calls
rescheduleTask(TASK_POSHOLD, ...) unconditionally but TASK_POSHOLD is only
defined when USE_POSITION_HOLD is enabled; to fix, wrap the
rescheduleTask(TASK_POSHOLD, TASK_PERIOD_MS(dev->delayMs)) call in an `#ifdef`
USE_POSITION_HOLD (or equivalent feature macro) inside the OPTICALFLOW_UPT1 case
(near upt1OpticalflowDetect and opticalflowHardware assignment) so builds
without USE_POSITION_HOLD won't reference the undefined TASK_POSHOLD enum.
There was a problem hiding this comment.
Actionable comments posted: 1
🤖 Fix all issues with AI agents
In `@src/main/drivers/rangefinder/rangefinder_upt1.c`:
- Around line 264-275: The function rangefinderUPT1GetDistance references
hasUPT1RFNewData (and returns upt1Value), but hasUPT1RFNewData is only declared
inside `#ifdef` USE_OPTICALFLOW which causes build failures when
USE_RANGEFINDER_UPT1 is enabled without USE_OPTICALFLOW; fix by moving the
declaration of hasUPT1RFNewData (and any related uptime/upt1Value state if also
guarded) out of the USE_OPTICALFLOW guard so it is always defined for the UPT1
driver, keeping any optical-flow-specific usage guarded but ensuring
rangefinderUPT1GetDistance can compile independently.
🧹 Nitpick comments (3)
src/main/sensors/opticalflow.c (1)
87-88:cosRotAngleandsinRotAngleshould bestatic.These file-scope variables are only used within this translation unit but lack the
staticqualifier, giving them external linkage. This risks name collisions at link time.Suggested fix
-float cosRotAngle = 1.0f; -float sinRotAngle = 0.0f; +static float cosRotAngle = 1.0f; +static float sinRotAngle = 0.0f;src/main/drivers/rangefinder/rangefinder_upt1.c (2)
132-141: Init command is sent only once, butupt1_byte_countwon't reset on re-init.
upt1_byte_countis astaticlocal, so ifrangefinderUPT1Initresetsupt1FrameStateback toUPT1_FRAME_WAIT_RESET, the byte counter is already past the threshold and the init command will be re-sent on the very first byte of the nextUpdatecall — which happens to work, but is fragile and accidental.Consider resetting the counter in
rangefinderUPT1Init(e.g., make it file-scoped) so the startup sequence is properly repeatable.
225-251: Optical flow quality not degraded when rangefinder is out of max range.When
distanceMm > UPT1_RANGE_MAX,upt1Valueis set toRANGEFINDER_OUT_OF_RANGEbut the optical flow path (lines 228–243) still accepts the data at face value. At high altitudes where the rangefinder can't measure distance, optical flow velocity estimates become unreliable because height-to-velocity scaling is unknown.If the sensor's own
confidencefield naturally drops at altitude this may be fine in practice, but it's worth verifying. Consider mirroring the min-range check with a max-range check:if (distanceMm < UPT1_OPTICALFLOW_MIN_RANGE || distanceMm > UPT1_RANGE_MAX) { upt1OpticalflowSensorData.quality = OPTICALFLOW_OUT_OF_RANGE; }
There was a problem hiding this comment.
Actionable comments posted: 1
🤖 Fix all issues with AI agents
In `@src/main/drivers/rangefinder/rangefinder_upt1.c`:
- Around line 238-242: When integration_time == 0 the code currently skips
updating upt1OpticalflowSensorData.flowRate and leaves stale values; update the
branch so that when dt_s <= 0 you explicitly zero the flowRate fields
(upt1OpticalflowSensorData.flowRate.x and .y) and/or mark the measurement
invalid (e.g., set upt1OpticalflowSensorData.quality = 0) so consumers don’t see
stale data; modify the block around integration_time/dt_s handling where
flow_x/flow_y are converted to flowRate to include this explicit reset on dt_s
<= 0.
🧹 Nitpick comments (1)
src/main/drivers/rangefinder/rangefinder_upt1.c (1)
202-213: RedundantDEBUG_SET(DEBUG_LIDAR_TF, 0, distanceMm)at line 212.Index 0 is already set at line 202 (unconditionally), then overwritten with the same value at line 212 inside the in-range branch. The duplicate write at line 212 can be removed.
There was a problem hiding this comment.
Actionable comments posted: 2
🤖 Fix all issues with AI agents
In `@src/main/flight/autopilot_multirotor.c`:
- Around line 380-388: When detecting the transition from optical-flow to GPS
(the branch where lastActiveSource == POSITION_SOURCE_OPTICALFLOW and you set
ap.targetLocation = gpsSol.llh), also reset the estimator/filter history to
avoid a derivative spike: set efAxis->previousDistance and
efAxis->previousVelocity to zero for both axes (the efAxis instances used in the
GPS velocity calculation) so the next velocity calc ((axisDistance -
efAxis->previousDistance) * gpsDataFreq) does not produce a large transient;
update the same transition block that sets ap.targetLocation to perform these
resets for the relevant efAxis objects.
- Around line 270-305: The optical flow validity check uses stale data because
updateOpticalFlowPosition() is called only after useOpticalFlow is set; move the
call to updateOpticalFlowPosition() earlier so the validity checks use fresh
data: call updateOpticalFlowPosition() before obtaining flowPos (or immediately
after getOpticalFlowPosition() but before checking flowPos->isValid and
isOpticalFlowPositionValid()), then perform the (posSource, sensors,
flowPos->isValid, isOpticalFlowPositionValid()) gate and only set
useOpticalFlow/currentSource and run the transition reset logic
(resetOpticalFlowPosition, zero positionErrorIntegral and previousAxisVelocity)
after the updated validity is confirmed; keep lastActiveSource handling as-is
but ensure it compares against the freshly-determined currentSource.
🧹 Nitpick comments (4)
src/main/flight/autopilot_multirotor.c (2)
54-54: MacroPOSITION_OF_II_SCALElacks parentheses around the expression.`#define` POSITION_OF_II_SCALE 0.25f * POSITION_OF_I_SCALEWithout parentheses, this expands unsafely if ever used in a context where operator precedence matters (e.g.,
x / POSITION_OF_II_SCALE). Currently it's only used in a simple multiplication at line 169, so it's not a bug today, but it's a latent hazard.Suggested fix
-#define POSITION_OF_II_SCALE 0.25f * POSITION_OF_I_SCALE // Slow integral is 25% of I gain +#define POSITION_OF_II_SCALE (0.25f * POSITION_OF_I_SCALE) // Slow integral is 25% of I gain
372-379: Fragileelseacross#endifboundary.The
elseat line 375 binds to theif (gpsHasNewData(...))at line 379 across the#endifpreprocessor boundary. This works but is very easy to break with future edits. A stray brace or added statement between lines 376 and 379 would silently disconnect the fallback.Consider using a local
boolflag set in the#ifdefblock and testing it before the GPS path, to make the flow explicit and preprocessor-safe.src/main/flight/position_estimator.c (1)
219-226:updateOpticalFlowTargetByAxisuses magic numbers for axis selection.Consider using named constants (e.g.,
LON/LATorX/Yfrom axis enums) instead of0and implicitelsefor the axis parameter, for consistency with the rest of the codebase.src/main/flight/position_estimator.h (1)
51-51: Consider returningconst positionEstimate_t*fromgetOpticalFlowPosition.The current declaration returns a mutable pointer to internal static state. All callers in
autopilot_multirotor.conly read from it. Returningconst positionEstimate_t*would prevent accidental mutation and better express the API contract.
There was a problem hiding this comment.
Actionable comments posted: 2
🤖 Fix all issues with AI agents
In `@src/main/drivers/rangefinder/rangefinder_upt1.c`:
- Around line 238-243: The quality field is being reset to 0 when
integration_time (dt_s) is non-positive but then immediately overwritten by
upt1OpticalflowSensorData.quality = confidence; — move that assignment into the
branch where dt_s > 0 (the same branch that computes flowRate and confidence) so
that when dt_s <= 0 the code keeps upt1OpticalflowSensorData.flowRate.x/y == 0
and upt1OpticalflowSensorData.quality == 0; ensure the assignment to
upt1OpticalflowSensorData.quality = confidence occurs only alongside the valid
flow calculations.
In `@src/main/flight/position.c`:
- Around line 202-227: The altitude-source debug writes
(DEBUG_SET(DEBUG_ALTITUDE, 3, ...)) in the altitude-selection block (the
branches that check haveRangefinderAlt, useZeroedGpsAltitude, haveBaroAlt and
use positionConfig()->altitude_prefer_baro) are being clobbered by the vario
write DEBUG_SET(DEBUG_ALTITUDE, 3, estimatedVario); change the altitude-source
writes to a different unused debug slot or a dedicated debug key (e.g.
DEBUG_SET(DEBUG_ALTITUDE, N, ...) where N != 3 or introduce DEBUG_ALT_SOURCE) so
the altitude source indicator is not overwritten, and update any
consumers/labels accordingly to read the new slot or key instead of slot 3.
🧹 Nitpick comments (3)
src/main/flight/autopilot_multirotor.c (1)
48-61: Parenthesize compound#defineto prevent macro expansion surprises.
POSITION_OF_II_SCALEexpands to0.25f * 0.0007fwithout parentheses. If it's ever used in a context like1.0f / POSITION_OF_II_SCALE, operator precedence will silently produce a wrong result.Suggested fix
-#define POSITION_OF_II_SCALE 0.25f * POSITION_OF_I_SCALE // Slow integral is 25% of I gain +#define POSITION_OF_II_SCALE (0.25f * POSITION_OF_I_SCALE) // Slow integral is 25% of I gainsrc/main/flight/position.c (1)
131-139: Rangefinder minimum altitude guard uses magic number10.The lower bound
rangefinderAltCm > 10(i.e., >10 cm) is reasonable but hardcoded. The upper bound usespositionConfig()->rangefinder_max_range_cm, so the lower bound should also be documented or configurable for consistency. Minor since 10 cm is a safe noise floor.src/main/drivers/rangefinder/rangefinder_upt1.c (1)
203-218: Redundant DEBUG_SET at line 203 — always overwritten at line 213 or 217.
DEBUG_SET(DEBUG_LIDAR_TF, 0, distanceMm)at line 203 is unconditionally followed by the same slot being written at line 213 (in-range) or line 217 (out-of-range), both writing the samedistanceMmvalue. The first write is dead code.Remove redundant debug write
- DEBUG_SET(DEBUG_LIDAR_TF, 0, distanceMm); DEBUG_SET(DEBUG_LIDAR_TF, 1, confidence);
|
Note that the accelerometer should be trimmed (see stick commands) so that in angle mode the quad hovers without drift. Failure to do this requires the position hold code to compensate which results in the I term having to work harder than it should and may result in some wobbles when position hold is activated. |
|
Alt and pos hold. IMG_5152.mov |
haslinghuis
left a comment
There was a problem hiding this comment.
In follow up we should look into
- replacing the PID controller with a Lead-Lag compensator to improve the system's phase margin and transient response without the high-frequency noise sensitivity inherent in pure derivative action.
- a truly general solution would require more robust EKF (Extended Kalman Filter) integration.
There was a problem hiding this comment.
Actionable comments posted: 4
🧹 Nitpick comments (2)
src/main/flight/position_estimator.c (1)
78-165:updateOpticalFlowPosition()is called twice per task cycle — once fromsensorsOk()and once frompositionControl().Both
sensorsOk()(pos_hold_multirotor.c:76) andpositionControl()(autopilot_multirotor.c:295) callupdateOpticalFlowPosition()within the same task cycle. BecauselastUpdateUsis updated on each call, the second call'sdeltaTimeUsis near-zero, making the duplicate integration negligible. Not a correctness issue, but consider guarding with a "already updated this cycle" flag to avoid redundant sensor reads and trust computations.🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/main/flight/position_estimator.c` around lines 78 - 165, updateOpticalFlowPosition() is invoked twice per task cycle (from sensorsOk() and positionControl()), causing redundant sensor reads and trust computations and updating opticalFlowPosition.lastUpdateUs so the second call does almost no integration; add a per-cycle guard (e.g., a boolean flag like opticalFlowPosition.updatedThisCycle) checked at the start of updateOpticalFlowPosition() and set at the end to skip duplicate work, and clear that flag once per loop before sensorsOk()/positionControl() run (or in the task tick) so updateOpticalFlowPosition(), lastUpdateUs, and trust calculations only run once per cycle.src/main/flight/pos_hold_multirotor.c (1)
70-111: Side effect:updateOpticalFlowPosition()called inside a query function.
sensorsOk()is semantically a query (returns bool), but callingupdateOpticalFlowPosition()on line 76 makes it update estimator state as a side effect. This is called every cycle from the main loop (line 157), coupling position estimation updates with sensor health checks. IfsensorsOk()is ever called from a different context (e.g., OSD, telemetry), it would unintentionally drive the estimator. Consider hoisting theupdateOpticalFlowPosition()call toupdatePosHold()before thesensorsOk()call.♻️ Suggested restructuring
if (posHold.isEnabled && posHold.isControlOk) { +#ifdef USE_OPTICALFLOW + updateOpticalFlowPosition(); +#endif posHold.areSensorsOk = sensorsOk();And remove the
updateOpticalFlowPosition()call fromsensorsOk().🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/main/flight/pos_hold_multirotor.c` around lines 70 - 111, sensorsOk() currently has a side effect because it calls updateOpticalFlowPosition(); move that call out of sensorsOk() and into updatePosHold() immediately before sensorsOk() is invoked so the estimator update happens only in the positional update path, then remove updateOpticalFlowPosition() from sensorsOk(); keep the existing optical-flow checks (getOpticalFlowPosition(), isOpticalFlowPositionValid(), setActivePositionSource(POSITION_SOURCE_OPTICALFLOW), and the POSHOLD_SOURCE_OPTICALFLOW_ONLY short-circuit) unchanged so logic and return values remain identical after the refactor.
🤖 Prompt for all review comments with AI agents
Verify each finding against the current code and only fix it if needed.
Inline comments:
In `@src/main/drivers/rangefinder/rangefinder_lidarmt.c`:
- Around line 163-169: The code currently divides pkt->motionX and pkt->motionY
by dt_s computed from micros() - opticalflowSensorData.timeStampUs, which can be
zero/negative and yield Inf/NaN; add a guard: compute integration_time = curTime
- opticalflowSensorData.timeStampUs and if integration_time <= 0 then set
opticalflowSensorData.flowRate.x and .y to 0.0f (and still update
opticalflowSensorData.timeStampUs = curTime), otherwise compute dt_s and assign
flowRate as now; reference micros(), integration_time, dt_s, pkt->motionX,
pkt->motionY, and opticalflowSensorData.flowRate.x/.y.
- Around line 172-177: The DEBUG_SET calls are using DEBUG_LIDAR_TF (slots 0–5)
which collides with other rangefinder drivers and logs
opticalflowSensorData.quality before range/staleness checks override it; change
the debug mode used in those DEBUG_SET calls from DEBUG_LIDAR_TF to the
sensor-specific mode DEBUG_OPTICALFLOW (or request a new optical-flow telemetry
mode) and move the DEBUG_SET that logs opticalflowSensorData.quality to after
the range/staleness checks that may set OPTICALFLOW_OUT_OF_RANGE or
OPTICALFLOW_HARDWARE_FAILURE so the debug slot reflects the value actually used
by downstream estimators; keep the other debug fields
(latestRangefinderData->distanceMm, pkt->motionX/pkt->motionY parts) in the same
order but under DEBUG_OPTICALFLOW.
In `@src/main/flight/autopilot_multirotor.c`:
- Around line 270-317: The static locals positionErrorIntegral and
previousAxisVelocity in positionControl() (positionSource_e lastActiveSource)
retain state across POS HOLD disable/enable because lastActiveSource remains
POSITION_SOURCE_OPTICALFLOW; reset lastActiveSource to POSITION_SOURCE_NONE when
POS HOLD is disabled so the transition block runs on re-enable. Fix by either
(a) making lastActiveSource non-static and stored in a resettable state (e.g.,
add it to autopilotState_t and clear it in resetPositionControl()), or (b) add
an accessor to set/reset the internal lastActiveSource (e.g.,
resetPositionControl() or a new resetPositionSource() function) and call that
when POS HOLD is turned off, or (c) change the transition check to consult
getActivePositionSource() instead of the local static; implement one of these so
positionErrorIntegral and previousAxisVelocity are zeroed on each POS HOLD
enable.
In `@src/main/flight/position.c`:
- Around line 205-230: When ALTITUDE_SOURCE_RANGEFINDER_ONLY is selected but
haveRangefinderAlt is false, the code currently falls through and leaves
zeroedAltitudeCm set from GPS; change the logic to explicitly handle that case:
after the `#ifdef` USE_RANGEFINDER rangefinder branch, add an else-if for
(altSource == ALTITUDE_SOURCE_RANGEFINDER_ONLY && !haveRangefinderAlt) that
clears or invalidates the altitude (e.g., set a “haveZeroedAltitude”/“haveAlt”
flag false or set zeroedAltitudeCm to a clear sentinel) and avoid falling back
to GPS/Baro; reference ALTITUDE_SOURCE_RANGEFINDER_ONLY, haveRangefinderAlt, and
zeroedAltitudeCm when making the change so the behavior matches the intended
“rangefinder-only” semantics.
---
Nitpick comments:
In `@src/main/flight/pos_hold_multirotor.c`:
- Around line 70-111: sensorsOk() currently has a side effect because it calls
updateOpticalFlowPosition(); move that call out of sensorsOk() and into
updatePosHold() immediately before sensorsOk() is invoked so the estimator
update happens only in the positional update path, then remove
updateOpticalFlowPosition() from sensorsOk(); keep the existing optical-flow
checks (getOpticalFlowPosition(), isOpticalFlowPositionValid(),
setActivePositionSource(POSITION_SOURCE_OPTICALFLOW), and the
POSHOLD_SOURCE_OPTICALFLOW_ONLY short-circuit) unchanged so logic and return
values remain identical after the refactor.
In `@src/main/flight/position_estimator.c`:
- Around line 78-165: updateOpticalFlowPosition() is invoked twice per task
cycle (from sensorsOk() and positionControl()), causing redundant sensor reads
and trust computations and updating opticalFlowPosition.lastUpdateUs so the
second call does almost no integration; add a per-cycle guard (e.g., a boolean
flag like opticalFlowPosition.updatedThisCycle) checked at the start of
updateOpticalFlowPosition() and set at the end to skip duplicate work, and clear
that flag once per loop before sensorsOk()/positionControl() run (or in the task
tick) so updateOpticalFlowPosition(), lastUpdateUs, and trust calculations only
run once per cycle.
There was a problem hiding this comment.
Actionable comments posted: 1
🧹 Nitpick comments (3)
src/main/flight/pos_hold_multirotor.c (1)
144-158: Good cleanup on POS HOLD exit, butupdateOpticalFlowPosition()runs unconditionally even after sensors fail.The exit path correctly zeroes autopilot angles and releases stick control (lines 145–151).
However, in the active loop (lines 155–164):
updateOpticalFlowPosition()at line 157 runs regardless ofposHold.isControlOk. IfpositionControl()returnedfalseon the previous cycle (sanity check failure),isControlOkisfalseand lines 159–163 are skipped — but the position estimator keeps integrating. This means the estimated position will drift away from reality during the failure state, and if control were ever re-enabled, there would be a jump. In practice this may be benign since the mode is effectively dead onceisControlOkgoes false, but it's worth noting.🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/main/flight/pos_hold_multirotor.c` around lines 144 - 158, The optical-flow position update is currently called unconditionally (updateOpticalFlowPosition()) even when posHold.isControlOk is false, allowing the estimator to integrate and drift during sensor/control failures; change the logic so updateOpticalFlowPosition() is only invoked when both posHold.isEnabled and posHold.isControlOk are true (i.e., move or wrap the call inside the same conditional that checks posHold.isControlOk), ensuring the update is guarded under the same control-ok check (respecting the USE_OPTICALFLOW macro).src/main/flight/autopilot_multirotor.c (1)
393-400:else if+ bareifcreates a subtle control-flow coupling.Lines 393–400 form an
else if (OPTICALFLOW_ONLY) { return false; } elsefollowed by a bareif (gpsHasNewData(...)). Theelseat line 396 is empty — it connects to theifat line 400 only because there's no block. This works but is fragile: if someone later adds a statement between lines 396 and 400, it would break the intendedelse→ifchain. Consider wrapping the GPS block in the else explicitly.Suggested clarification
} else if (posSource == POSHOLD_SOURCE_OPTICALFLOW_ONLY) { // Optical flow required but not available return false; - } else -#endif // USE_OPTICALFLOW - - // Fall back to GPS position control - if (gpsHasNewData(&gpsStamp)) { + } else { +#endif // USE_OPTICALFLOW + + // Fall back to GPS position control + if (gpsHasNewData(&gpsStamp)) {Though this changes the
#ifdefstructure, the intent might be better served by a different approach depending on maintainer preference.🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/main/flight/autopilot_multirotor.c` around lines 393 - 400, The control flow uses an `else` that is effectively empty after the POSHOLD_SOURCE_OPTICALFLOW_ONLY block and then a bare `if (gpsHasNewData(&gpsStamp))`, which couples the `else` to the following `if` fragily; update the code around the POSHOLD_SOURCE_OPTICALFLOW_ONLY branch so the GPS fallback is explicitly inside the corresponding `else` block (e.g., replace the bare `if (gpsHasNewData(&gpsStamp))` with `} else { if (gpsHasNewData(&gpsStamp)) { ... } }` or otherwise wrap the GPS handling in an explicit `else`), touching the POSHOLD_SOURCE_OPTICALFLOW_ONLY branch and the gpsHasNewData(&gpsStamp) block to ensure the intended pairing is clear and robust.src/main/flight/position.c (1)
202-233: Altitude source fusion logic is well-structured; minor note onRANGEFINDER_ONLYfallback.The priority cascade (rangefinder → GPS/Baro mix → Baro-only) and the explicit
RANGEFINDER_ONLYno-fallback path at lines 212–215 are clear. WhenRANGEFINDER_ONLYis selected and the rangefinder becomes temporarily unavailable,zeroedAltitudeCmholds its last value (from the LPF at line 236) rather than being explicitly invalidated. This is probably acceptable for brief dropouts, but on a prolonged rangefinder outage the stale altitude could mislead altitude-dependent features. Worth a comment in code or documentation if intended as hold-last-value behavior.🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/main/flight/position.c` around lines 202 - 233, When ALTITUDE_SOURCE_RANGEFINDER_ONLY is selected but haveRangefinderAlt is false, the code currently leaves zeroedAltitudeCm as the last LPF value; update the branch handling altSource == ALTITUDE_SOURCE_RANGEFINDER_ONLY (and the surrounding rangefinder check using haveRangefinderAlt and altSource) to explicitly mark the altitude as invalid instead of silently holding the last value—e.g., set zeroedAltitudeCm to a sentinel (NaN) or flip an existing/added validity flag (e.g., haveValidZeroedAlt) so downstream code knows the altitude is unavailable; if you prefer to keep the hold-last-value behavior, add a clear comment near the ALTITUDE_SOURCE_RANGEFINDER_ONLY branch documenting that prolonged rangefinder outages will retain the last LPF altitude and advising consumers to check validity.
🤖 Prompt for all review comments with AI agents
Verify each finding against the current code and only fix it if needed.
Inline comments:
In `@src/main/sensors/opticalflow.c`:
- Around line 190-214: The MT driver fills motionX/motionY which are later used
as processed.x/processed.y and treated as rad/s (see DEGREES_TO_RADIANS usage
and position_estimator.c velocityBF.x = flow->processedFlowRates.x *
altitudeCm); confirm the MT datasheet units for motionX/motionY and either (a)
apply the appropriate scaling conversion to produce radians per second (matching
UPT1's scaling) before assigning to processed.x/processed.y, or (b) if they
already are rad/s, add an explicit comment next to the motionX/motionY
assignment documenting the units and any fixed-point scale so future readers
know no conversion is required. Ensure you update the code paths referencing
motionX/motionY (e.g., where processed.x/processed.y are set and where gyro
subtraction uses DEGREES_TO_RADIANS) so the units are consistent end-to-end.
---
Duplicate comments:
In `@src/main/drivers/rangefinder/rangefinder_lidarmt.c`:
- Around line 184-189: The MT optical flow driver is clobbering shared
DEBUG_LIDAR_TF slots (used by rangefinder_lidartf and rangefinder_upt1) and logs
opticalflowSensorData.quality before range/staleness checks; change the
DEBUG_SET calls that currently use DEBUG_LIDAR_TF (slots 0–5) to use
DEBUG_OPTICALFLOW or a new dedicated debug mode constant, and move the quality
debug write (opticalflowSensorData.quality) to after the staleness/range checks
around latestRangefinderData so the logged quality reflects the validated state;
update references to DEBUG_SET, DEBUG_LIDAR_TF, DEBUG_OPTICALFLOW,
latestRangefinderData, opticalflowSensorData and pkt accordingly.
---
Nitpick comments:
In `@src/main/flight/autopilot_multirotor.c`:
- Around line 393-400: The control flow uses an `else` that is effectively empty
after the POSHOLD_SOURCE_OPTICALFLOW_ONLY block and then a bare `if
(gpsHasNewData(&gpsStamp))`, which couples the `else` to the following `if`
fragily; update the code around the POSHOLD_SOURCE_OPTICALFLOW_ONLY branch so
the GPS fallback is explicitly inside the corresponding `else` block (e.g.,
replace the bare `if (gpsHasNewData(&gpsStamp))` with `} else { if
(gpsHasNewData(&gpsStamp)) { ... } }` or otherwise wrap the GPS handling in an
explicit `else`), touching the POSHOLD_SOURCE_OPTICALFLOW_ONLY branch and the
gpsHasNewData(&gpsStamp) block to ensure the intended pairing is clear and
robust.
In `@src/main/flight/pos_hold_multirotor.c`:
- Around line 144-158: The optical-flow position update is currently called
unconditionally (updateOpticalFlowPosition()) even when posHold.isControlOk is
false, allowing the estimator to integrate and drift during sensor/control
failures; change the logic so updateOpticalFlowPosition() is only invoked when
both posHold.isEnabled and posHold.isControlOk are true (i.e., move or wrap the
call inside the same conditional that checks posHold.isControlOk), ensuring the
update is guarded under the same control-ok check (respecting the
USE_OPTICALFLOW macro).
In `@src/main/flight/position.c`:
- Around line 202-233: When ALTITUDE_SOURCE_RANGEFINDER_ONLY is selected but
haveRangefinderAlt is false, the code currently leaves zeroedAltitudeCm as the
last LPF value; update the branch handling altSource ==
ALTITUDE_SOURCE_RANGEFINDER_ONLY (and the surrounding rangefinder check using
haveRangefinderAlt and altSource) to explicitly mark the altitude as invalid
instead of silently holding the last value—e.g., set zeroedAltitudeCm to a
sentinel (NaN) or flip an existing/added validity flag (e.g.,
haveValidZeroedAlt) so downstream code knows the altitude is unavailable; if you
prefer to keep the hold-last-value behavior, add a clear comment near the
ALTITUDE_SOURCE_RANGEFINDER_ONLY branch documenting that prolonged rangefinder
outages will retain the last LPF altitude and advising consumers to check
validity.
There was a problem hiding this comment.
Actionable comments posted: 1
🤖 Prompt for all review comments with AI agents
Verify each finding against the current code and only fix it if needed.
Inline comments:
In `@src/main/flight/position.c`:
- Around line 207-233: The code stops falling back to GPS/Baro when altSource ==
ALTITUDE_SOURCE_RANGEFINDER_PREFER and haveRangefinderAlt is false; update the
conditional checks that decide GPS/Baro fusion so they include
ALTITUDE_SOURCE_RANGEFINDER_PREFER where appropriate: add
ALTITUDE_SOURCE_RANGEFINDER_PREFER to the condition that currently reads
(altSource == ALTITUDE_SOURCE_DEFAULT || altSource == ALTITUDE_SOURCE_GPS_ONLY)
used for the useZeroedGpsAltitude/GPS mix, and also add
ALTITUDE_SOURCE_RANGEFINDER_PREFER to the condition that reads (altSource ==
ALTITUDE_SOURCE_DEFAULT || altSource == ALTITUDE_SOURCE_BARO_ONLY) used for
baro-only fallback; ensure the inner pDOP-weighted GPS/Baro blend (the branch
using gpsTrust and zeroedAltitudeCm/baroAltCm) also accepts
ALTITUDE_SOURCE_RANGEFINDER_PREFER so the quality-weighted fallback matches
DEFAULT behavior (references: altSource, ALTITUDE_SOURCE_RANGEFINDER_PREFER,
haveRangefinderAlt, useZeroedGpsAltitude, haveBaroAlt, gpsTrust,
zeroedAltitudeCm, baroAltCm).
There was a problem hiding this comment.
Actionable comments posted: 2
🤖 Prompt for all review comments with AI agents
Verify each finding against the current code and only fix it if needed.
Inline comments:
In `@src/main/flight/position.c`:
- Around line 134-141: The rangefinder cutoff is too abrupt: when
rangefinderAltCm crosses positionConfig()->rangefinder_max_range_cm
haveRangefinderAlt flips and zeroedAltitudeCm can step; modify the logic around
the block that sets haveRangefinderAlt (using rangefinderAltCm,
rangefinderGetLatestAltitude(), and positionConfig()->rangefinder_max_range_cm)
to implement hysteresis (e.g., only clear haveRangefinderAlt when > max_range
and only set it when < max_range - hysteresis_cm) and/or apply a short weighted
blend between the rangefinder altitude and the fallback altitude during the
handoff so the pt2 filter and altitude controller see a smooth transition.
Ensure the same change is applied to the other analogous block referenced (lines
~207-214).
- Around line 157-161: The zeroing logic only sets rangefinderAltOffsetCm when
haveRangefinderAlt is true (which requires rangefinderAltCm >=
RANGEFINDER_MIN_ALT), causing offsets to never be captured for low-clearance
vehicles; modify the disarmed zeroing block inside the USE_RANGEFINDER section
so it uses a separate, lower ground threshold (or simply checks for a valid
non-negative reading) instead of haveRangefinderAlt/RANGEFINDER_MIN_ALT.
Specifically, change the condition that sets rangefinderAltOffsetCm =
rangefinderAltCm to allow rangefinderAltCm >= 0 (or a new
RANGEFINDER_GROUND_MIN_CM constant) while leaving the in-flight
haveRangefinderAlt logic and RANGEFINDER_MIN_ALT gate intact.
There was a problem hiding this comment.
Actionable comments posted: 1
🧹 Nitpick comments (1)
src/main/flight/autopilot_multirotor.c (1)
354-358: Flow D filter cutoff not updated with actual data rate.The
flowDLpfPT2 filter gain is computed once at init with the nominal 50 Hz rate (FLOW_DATA_INTERVAL_DEFAULT), but the actual call interval is measured dynamically inflowDataInterval. If the real rate deviates from 50 Hz, the effective filter cutoff shifts. The GPS path demonstrates the correct pattern—updating the filter gain each cycle viapt1FilterUpdateCutoff.Suggested approach
+ // Update flow D filter gain with actual data rate + const float flowDGain = pt2FilterGain(0.25f / flowDataInterval, flowDataInterval); + flowDLpf[axis].k = flowDGain; + const float delta = (axisVelocity - previousAxisVelocity[axis]) * flowDataFreq;Alternatively, only update the gain when
flowDataIntervaldeviates meaningfully from the nominal value.🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/main/flight/autopilot_multirotor.c` around lines 354 - 358, The PT2 D-term low-pass (flowDLpf) is initialized with FLOW_DATA_INTERVAL_DEFAULT but not updated when the measured flowDataInterval changes, so the effective cutoff shifts; inside the D-term block where delta is computed and pt2FilterApply(&flowDLpf[axis], delta) is called (the lines calculating delta, previousAxisVelocity[], pidD and pidSum.v[axis]), update the filter gain each cycle using the measured flowDataInterval (similar to the GPS path's pt1FilterUpdateCutoff approach) — call the appropriate PT2 gain-update routine with the new sample interval (or use pt1FilterUpdateCutoff if only that helper exists) before applying pt2FilterApply, optionally throttling updates to only when flowDataInterval meaningfully deviates from FLOW_DATA_INTERVAL_DEFAULT.
🤖 Prompt for all review comments with AI agents
Verify each finding against the current code and only fix it if needed.
Inline comments:
In `@src/main/flight/autopilot_multirotor.c`:
- Around line 68-69: The static OF-only variables positionOFPidCoeffs and
flowDLpf (and any code that initializes them) are declared/initialized
unconditionally but only used under `#ifdef` USE_OPTICALFLOW; move their
declarations and all initialization code for positionOFPidCoeffs and flowDLpf
inside the existing `#ifdef` USE_OPTICALFLOW / `#endif` so they only exist for OF
builds, and remove the unconditional originals; update any related
initialization sites referenced (the blocks that set positionOFPidCoeffs fields
and flowDLpf) to be inside that same `#ifdef` to avoid wasting RAM on non-OF
builds.
---
Nitpick comments:
In `@src/main/flight/autopilot_multirotor.c`:
- Around line 354-358: The PT2 D-term low-pass (flowDLpf) is initialized with
FLOW_DATA_INTERVAL_DEFAULT but not updated when the measured flowDataInterval
changes, so the effective cutoff shifts; inside the D-term block where delta is
computed and pt2FilterApply(&flowDLpf[axis], delta) is called (the lines
calculating delta, previousAxisVelocity[], pidD and pidSum.v[axis]), update the
filter gain each cycle using the measured flowDataInterval (similar to the GPS
path's pt1FilterUpdateCutoff approach) — call the appropriate PT2 gain-update
routine with the new sample interval (or use pt1FilterUpdateCutoff if only that
helper exists) before applying pt2FilterApply, optionally throttling updates to
only when flowDataInterval meaningfully deviates from
FLOW_DATA_INTERVAL_DEFAULT.
PosHoldReaction.mp4 |
|
A regression impacting stopping behaviour after stick control during position hold, introduced by the introduction of the Kalman filter has been fixed. Altitude hold bounce has been addressed (filter delay on D was causing oscillation). Below is a Caddx Protos diff which works well with this commit. |
5367517 to
4bd6e77
Compare
|
Note that in building this test setup with both MTF02P and UTP1 I encountered the issue that connecting to UART4 prevented the bootloader from operating correctly, and I have to unplug in order to reflash. This occurs with both optical flow sensors, so be careful where you connect them.
For reference below are the diff files for each config. The UP-T1-001 seems much better than the MTF02P, at least under LED lighting, at holding position. BTFL_cli_BFPV85-UPT1_20260329_133036_HDZERO_AIO15.txt |
|
Don't merge for a moment; I'm doing som restructuring of the altitude hold code. |
|
Altitude hold now uses an inner velocity PID and outer altitude PID. Hold is much improved. |
…betaflight#14922) * Upixel UP-T1-001-Plus support and optical flow position hold
…betaflight#14922) * Upixel UP-T1-001-Plus support and optical flow position hold
…betaflight#14922) * Upixel UP-T1-001-Plus support and optical flow position hold
|
Hey Steve this looks awesome and I'd like to replicate this with my cinawhoop but I think I need some more details about your beta whoop setup. shoot me a message on Discord? LeadFingers#9794 |
On this 5" build, which I've had the chance to tune today, the ap_position setting worked well at half the defaults (tuned for a Caddx Protos). Interestingly the D term was still fine at the default. |
|
@SteveCEvans
During my flight tests, the performance of both ALTHOLD and POSHOLD has not been as stable or consistent as I had hoped. I understand that the results may depend on the configuration, sensor installation, flight conditions, and tuning, so I would appreciate your advice when you have a moment. I was wondering that:
I would be happy to provide my configuration, logs, videos, or any other test information that might be helpful.If it would be more convenient, please feel free to message me on Discord, @chrischou_05519. |
This PR provides support for the
UP-T1-001-Plusoptical flow and TOF altimeter used in the Caddx Protos.To test, please use the
CADDX_PROTOS_F4target and use the config below:BTFL_cli_CADDXPROTOS_20260214_201831_CADDX_PROTOS_F4.txt
With this config the
POS HOLDflight mode is enabled by switching the left rocker switch to the up position on the Caddx transmitter. Note that you cannot arm whilst in this mode, but once armed you can switch to this mode to take off, or of course you can switch to it mid-flight.The position hold code has currently only been tested with the Caddx Protos. There is some code in
opticalflow.cwhich compensated for rotation as detected by the gyro so that such rotation is not reported as movement of the optical flow sensor. I have yet to establish if this will work for other sensors, so this may need to change.A new debug mode,
AUTOPILOT_PID, has been created to report the PID and resulting commanded angle on each of roll and pitch axis.Note that this PR supersedes #14884 which is closed in deference to this.
@ctzsnooze I'd really appreciate your review of this code. It's largely inspired by your position estimation code, and I hope I've not messed with your GPS code! This should transition smoothly between optical flow position sensing and GPS when the quality of the optical flow reading drops.
The PID loop implemented in
position_estimator.cdrives thePterm with the velocity reported by the optical flow sensor, and theIterm with the distance from the target position. I've added an additionalIIterm which is the integral of the that distance, as without it there was either drift or I induced oscillations.I'm submitting this PR now to trigger the code rabbit review (and any human one's it's blessed with!) and to give other Protos owners the chance to try it and report their experiences. For now please don't merge which I check other optical flow sensors.
Summary by CodeRabbit
New Features
Bug Fixes