Skip to content

Guard invalid ReadPos to avoid servo angle jump on bus read failure - #5

Open
GOB52 wants to merge 2 commits into
m5stack:mainfrom
GOB52:fix/servo-readpos-guard
Open

Guard invalid ReadPos to avoid servo angle jump on bus read failure#5
GOB52 wants to merge 2 commits into
m5stack:mainfrom
GOB52:fix/servo-readpos-guard

Conversation

@GOB52

Copy link
Copy Markdown

Summary

ScsServo::getCurrentAngle() can return a large, out-of-range angle when the
underlying serial bus read fails, causing the head to jerk to a physical
extreme. This PR validates the raw position before converting it to an angle and
falls back to the commanded angle on failure. It also prevents a failed read
from corrupting the stored zero (home) calibration.

Problem

_scs_bus.ReadPos() returns -1 when the half-duplex servo bus read fails
(no/unstable response). The previous code converted that value directly:

int angle = (current_pos - _zero_pos) * 5 * 10 / 16;

With current_pos == -1, this produces a large negative angle that clamps to
the angle limit, i.e. full deflection. Any caller that builds a relative
move on top of getCurrentAngle() (e.g. "look slightly left from the current
pose") then snaps the head hard to one side. On a StackChan this shows up as the
head violently slamming to the extreme when a bus read intermittently fails.

The same unchecked read in setCurrentAngleAsZero() would store -1 as the
zero position, permanently corrupting the home calibration.

Fix

  • Add is_raw_pos_valid() (checks _config.rawPosLimit) and a small
    raw_pos_to_angle() helper.
  • getCurrentAngle(): on an invalid raw position, log a warning and return the
    commanded (spring) angle, clamped to the angle limit, instead of a bogus
    extreme.
  • setCurrentAngleAsZero(): on an invalid raw position, keep the existing zero
    and skip the write, so a transient read failure cannot corrupt calibration.

No behavior change on a successful read.

Consistency with the StackChan firmware

This uses the same handling the official StackChan firmware applies to servo
positions: it validates the position against _config.rawPosLimit and falls
back to a safe value when the position is out of range. The firmware does this
for the stored zero position in get_zero_pos_from_nvs()
(firmware/main/hal/hal_servo.cpp):

if (nvs_zero_pos >= _config.rawPosLimit.x && nvs_zero_pos <= _config.rawPosLimit.y) {
_zero_pos = nvs_zero_pos; // accept
} else {
_zero_pos = _config.defaultZeroPos; // reject out-of-range, fall back
}

This PR applies that same rawPosLimit validation to the live ReadPos()
results in getCurrentAngle() / setCurrentAngleAsZero(), which are otherwise
used unguarded, so a transient bus failure cannot turn into a bogus angle or a
corrupted zero.

Testing

  • Before: intermittent bus read failures caused the head to slam to full
    deflection during relative moves (idle motion / head-pet gestures).
  • After: the head holds its commanded pose on a failed read; relative moves
    behave normally; home calibration is preserved across transient failures.

Sign up for freeto join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant

@GOB52