auto-calibration (bias correction)

[xiota]

Consider auto calibrating the gyro for airmouse functionality.

This is from my userspace implementation in LuaJit.

local BETA = 0.1  -- smaller = slower adaptation

local bias = {
  lock_count = 0,
  lock_window = 100,
  tolerance = 0,
  max_tolerance = 10,

  x = 0,
  y = 0,
  z = 0,
}

function update_bias(x, y, z)
  if bias.tolerance == 0 then
    bias.tolerance = (255 - state.battery)/60 + 0.1
  end

  if bias.lock_count <= bias.lock_window then
    bias.x = BETA * x + (1 - BETA) * bias.x
    bias.y = BETA * y + (1 - BETA) * bias.y
    bias.z = BETA * z + (1 - BETA) * bias.z

    local tol_ok = math.abs(x - bias.x) <= bias.tolerance
      and math.abs(y - bias.y) <= bias.tolerance
      and math.abs(z - bias.z) <= bias.tolerance
    if tol_ok then
      bias.lock_count = bias.lock_count + 1
      if bias.lock_count > bias.lock_window then
        print(string.format(
          "Bias locked: tol=%.2f bat=%.3f bias=(%.3f, %.3f, %.3f)",
          bias.tolerance, (state.battery/255), bias.x, bias.y, bias.z
        ))
      end
    else
      bias.lock_count = math.max(0, bias.lock_count - 1)
    end

      if bias.lock_count < bias.lock_window/5 then
        bias.tolerance = math.min(bias.tolerance + 0.01, bias.max_tolerance)
      end
  end
end