fix(firmware): guard invalid servo position reads

[jun76]

fix(firmware): guard invalid servo position reads

Summary

Guard servo position reads before converting raw positions into StackChan motion angles.

ReadPos() can return an invalid value, such as -1, when the servo position read fails. Previously that value was converted directly into the current logical angle:

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

With the default yaw zero position, ReadPos() == -1 becomes a large negative yaw angle and is then clamped to the yaw limit. This is consistent with the attached reproduction clip, where the device suddenly turns left by about 90 degrees.

invalid_servo_pos.mp4

Because auto angle synchronization uses getCurrentAngle() as the animation start point, an invalid read can also make the next motion start from an incorrect extreme angle before smoothly animating back to the intended target.

This PR validates the raw servo position before using it. If the value is outside the configured raw position limits, the firmware ignores the invalid read and falls back to the last known logical servo angle.

The same validation is also applied when saving the current position as the zero calibration point, so a failed read does not overwrite the stored calibration value.

This PR only addresses firmware-side handling of invalid read results. It does not attempt to diagnose the hardware-side cause of the failed position read.

Changes

• Validate raw servo positions against the configured raw position limits.
• Ignore invalid ReadPos() results before converting them into motion angles.
• Fall back to the last known logical servo angle when the current raw position read is invalid.
• Skip zero calibration writes when the current raw position cannot be read safely.
• Add warning logs for ignored invalid position reads.

Testing

• Built the firmware in my downstream fork with this guard applied.
• Ran the firmware on a real StackChan device.
• Confirmed that invalid servo position reads are ignored instead of being converted into extreme motion angles.
• The sudden left turn of about 90 degrees shown in the attached reproduction clip has not reproduced so far after applying this change.