This file implements the IMU task. It reads heading and yaw rate from the BNO055, unwraps the heading so it remains continuous across angle rollover, filters the yaw rate, and supports IMU calibration load/save behavior.
The task provides clean orientation information to the rest of the robot, especially the observer and the serial diagnostics.
| Name | Description |
|---|---|
imu | BNO055 driver object. |
psi | Output Share for measured heading. |
psiDot | Output Share for measured yaw rate. |
imuCalReq | Share used to request calibration actions. |
_unwrap | Enables continuous-angle heading unwrapping. |
_calib_file | Filename used to store calibration coefficients. |
_yaw_lim | Maximum allowed yaw-rate magnitude. |
_yaw_spike_dps | Spike threshold in degrees per second. |
_psi_prev, _psi_cont | Internal variables used to maintain continuous heading. |
_r_filt | Filtered yaw-rate state. |
| Method / Logic | Description |
|---|---|
| Heading unwrap | Maintains continuous heading rather than a wrapped 0–360 degree signal. |
| Calibration load | Attempts to load stored coefficients from file at startup or on request. |
| Calibration save | Saves updated coefficients when calibration is complete. |
| Spike rejection | Rejects unrealistic gyro spikes before publishing values. |
Heading unwrapping is especially important for control and estimation. If the robot turns through 359 degrees to 0 degrees, a raw wrapped signal would jump abruptly. This file prevents that issue by maintaining a continuous heading state.
This file turns raw IMU data into something usable by the observer and by the debugging interface. Without it, heading and yaw-rate information would be less reliable and more difficult to interpret.