Reports which way the body is pointing, as the :ground joint's rotation.
The robot balances well enough to stay where it is put, but nothing stops it twisting: one wheel finding more grip than the other, a slope, a nudge. This measures that so the balance controller can drive the wheels differentially and take it back out.
Where the angle comes from
The gyroscope's reading is a rate in the chip's frame. Taken through the static
rotation from imu_link to base_link — read from the topology rather than
written down here — its component about the body's +Z is the yaw rate, and
integrating that is the heading.
Strictly the rate about the world's vertical is wz * cos(pitch) plus a term
in wx, and this is the small-pitch approximation of it. At the three degrees
a balancing robot actually holds, cos(pitch) is 0.9986, so the approximation
costs less than the gyro's own noise. It is the same trade
BB.NSK.Sensor.Lean makes about roll, for the same reason.
It drifts, and that is designed for rather than fixed
There is no magnetometer, so rotation about gravity is unobservable and nothing corrects the integral. The chip's bias runs at about 0.33 degrees a second, which is 20 degrees a minute — so this is not a compass and must not be treated as one.
What makes that survivable is that the controller holds a recent heading rather than an absolute one: its setpoint leaks towards whatever this reports, so a fast twist is corrected and a slow drift is followed and ignored. That is what disturbance rejection needs, and it is the only thing this sensor can honestly support. Driving in a straight line over a long run would need a heading that doesn't drift, and this is not one.
Wrapped to (-pi, pi] so it stays a heading rather than growing without bound
over a seven minute stand. Use difference/2 to compare two of them — plain
subtraction is wrong across the wrap.
What it asserts that it doesn't know
:ground is a :planar joint, so its configuration is a whole
BB.Math.Transform2D — x, y and theta together. Only theta is
measured. The other two are published as zero, which is what the joint already
read before this existed, and they remain unobservable without encoders. So
forward kinematics gets a real heading and a position that is still a
placeholder, and anything reasoning about where the robot is rather than
which way it faces is reading a number nobody measured.
Summary
Functions
The signed shortest angle from b to a, in radians.
The heading after integrating rate for dt seconds, wrapped.
Bring an angle into (-pi, pi].
The yaw rate in radians per second for a gyroscope reading, positive turning to the robot's left.
Functions
The signed shortest angle from b to a, in radians.
Both arguments are wrapped headings, so subtracting them directly is wrong by a
full turn whenever they straddle the wrap. Positive means a is to the left of
b.
iex> import BB.NSK.Sensor.Heading
iex> Float.round(difference(3.0, -3.0), 6)
-0.283185The heading after integrating rate for dt seconds, wrapped.
Rectangular integration, because the alternative is holding a previous rate to trapezoid against and at 100 Hz the difference is far below the drift that dominates this anyway.
Bring an angle into (-pi, pi].
@spec yaw_rate(BB.Math.Vec3.t(), BB.Math.Quaternion.t()) :: float()
The yaw rate in radians per second for a gyroscope reading, positive turning to the robot's left.
body_from_imu rotates a rate in the chip's frame into the body's, after which
yaw is the component about +Z — the same axis the right hand rule gives for a
robot whose +X is forwards and +Y is to its left.