Odometry is lowkey wack

[nicohmje]

Current Odometry calculation:

• The slotted wheel on the driveshaft sends a pulse every 1/16th rotations. The gap between pulses is measured by the STM32 and the speed is computed, knowing that one rotation of the driveshaft is 79.748 mm of travel distance.
• The data is transmitted over SPI to the Raspberry Pi at 150Hz.
• The Raspberry Pi publishes the STM32 data to the stm32_sensors topic via the stm32_publisher node.
• The fork_publisher node converts the speed from mm/s to m/s and publishes it again to the raw_fork_data topic
• The odom_fork_imu.py node integrates the speed combined with the yaw reported by the IMU to get the position.

Issues encountered

The odometry is plain wrong. Experimental tests have shown that it is, for some reason, halved. Triple checking the formulas has not highlighted a reason for this missing factor, but adding it corrects a big part of the error.

The fork does not report signed speed. It is thus non trivial to determine if the car is going forwards or backwards. Current heuristic is that the car only moves under its own power, so it knows what direction it is supposed to be going. This fails when the car is set to reverse but has not come to a stop yet, resulting in a commanded reverse, but an effective forwards.

The orientation would jump to 0 when CRC failed, but this has been fixed in commit 0340f1c .

Possible fixes

Odometry could be integrated on the STM32 for better performance.

Car direction could be determined by analysing the linear acceleration (if speed == 0, then lin acc x < 0, then speed > 0 then we're probably going backwards).

Odometry could also be inferred from the wheel angle, with an UKF to do sensor fusion with the IMU heading.

It is unclear how big of an impact this has, but would still be great to fix.