Improve sensor delta_angle calculation

[austin-bowen]

Description

Improve delta_angle calculation in Sensor::getVelocity() by:

1. Handling full_rotations integer wrap-arounds.
2. Preventing float precision loss issues from unbounded float growth.
3. Fewer float ops --> faster.

Math explanation:

// Current implementation
delta_angle = (full_rotations * 2pi + angle_prev) - (vel_full_rotations * 2pi + vel_angle_prev);
                           // ^ this op grows without bound; eventually not enough precision for angle_prev

// Proposed implementation
delta_angle = 2pi * (full_rotations - vel_full_rotations) + (angle_prev - vel_angle_prev);
               // ^ this op should always be small; the full_rotations delta will never be >1 under normal circumstances

Comparing Floating Point Ops (assuming other ops are basically free)

Code Path	Current Code	Proposed Code
Hot (rotations equal)	1 fsub	1 fsub
Cold (rotations differ)	7 total: 2 i2f, 2 fmul, 2 fadd, 1 fsub	4 total: 1 i2f, 1 fmul, 1 fadd, 1 fsub

Type of change

Please delete options that are not relevant.

• Bug fix (non-breaking change which fixes an issue)

How Has This Been Tested?

Basically just put a motor on velocity control mode at 5RPS for a long time.

Test Configuration/Setup:

• Hardware:
  • SimpleFOC Mini
  • 2804 gimbal motor
  • AS5600 magnetic encoder (using analog OUT)
  • All from this Amazon listing
• IDE: Arduino
• MCU package version: XIAO RP2040

[runger1101001]

This is a very good idea - will be included in the next release.

I’ve changed the branch from master to dev, as we only accept PRs to the dev branch.