Wäre es nicht einfacher, direkt die I2C-Methoden aus der RP6 Lib zu nehmen?
Beim Schreiben machst du das ja, aber beim Lesen noch nicht.

Kannst dir ja mal den Quellcode zur Ansteuerung der SRF02 bei meiner Remotrol ansehen.
Ansosnten solltest du zur Kalibrierung am besten nach draußen gehen.
Dort stören dann auch eventuelle Stahlarmierungen im Boden nicht mehr.
Außerdem solltest du natürlich den Kompass möglichst weit weg vom RP6 platzieren, 20 cm oder am besten noch mehr.