Arduino C++: CMPS11 Kalibrierung (9D IMU)

[HaWe]

Hallo,
aus aktuellem Anlass habe ich mal ein Kalibrierungsprogramm für den CMPS11 (9D IMU) umgeschrieben und erweitert, um die Routinen (Start, Init, Messen, Speichern und Ende) sicher per Button (pin 2) und LED-Kontrolle (pin 13, built-in) steuern zu können. Hier der Code:



// CMPS11 calibration example
// description see below
//
// Button 2 press to start/finish calibration


#include <Wire.h>

#define ADDR_CMPS11 0x60



void setup(){
pinMode( 2, INPUT_PULLUP); // start button
pinMode(13, OUTPUT); // LED (built-in)

Wire.begin();
Serial.begin(115200);
calibrate();
}


void calibrate(){
Serial.println("Calibration Mode: start/finish: press button 2");

digitalWrite(13, LOW);
while(digitalRead(2));
if( !digitalRead(2)) {
digitalWrite(13, HIGH);
delay(100);
while( !digitalRead(2));
}
digitalWrite(13, LOW);
Serial.println("Starting...");
delay(1000);

Serial.println();
Serial.println("Begin: rotate into each direction (3D rotation)");
Serial.println("until LED stops flashing!");
Serial.println("Then fix it to the ground at either edge for 2sec each!");
Serial.println("finish: again press button 2");
delay(2000); //2 sec before starting

Wire.beginTransmission(ADDR_CMPS11);
Wire.write(0); //command register
Wire.write(0xF0);
Wire.endTransmission();
delay(25);

Wire.beginTransmission(ADDR_CMPS11);
Wire.write(0); //command register
Wire.write(0xF5);
Wire.endTransmission();
delay(25);

Wire.beginTransmission(ADDR_CMPS11);
Wire.write(0); //command register
Wire.write(0xF6); // horizontal: 0xf7 all: 0xf6
Wire.endTransmission();
delay(2000);

while(digitalRead(2));
if( !digitalRead(2)) {
digitalWrite(13, HIGH);
delay(100);
while( !digitalRead(2));
}
digitalWrite(13, LOW);

Serial.println("Finish...");
delay(500);

Wire.beginTransmission(ADDR_CMPS11);
Wire.write(0); //command register
Wire.write(0xF8);
Wire.endTransmission();

Serial.println("...done!");
}


void loop(){

}


/*
Documentation
http://www.robot-electronics.co.uk/htm/cmps11i2c.htm

Please do not do this until you have I2C communication fully working.
I would recommend evaluating the CMPS11 performance first before implementing this function.
Its purpose is to remove sensor gain and offset of both magnetometer and accelerometer
and achieves this by looking for maximum sensor outputs.
First of all you need to enter the calibration mode by sending a 3 byte sequence of
0xF0,0xF5 and then 0xF6
to the command register, these MUST be sent in 3 separate I2C frames,
you cannot send them all at once. There MUST be a minimum of 20ms between each I2C frame.
An I2C frame is [start sequence] [I2C address] [register address] [command byte] [stop sequence].
The LED will then extinguish and the CMPS11 should now be rotated in all directions in 3 dimensions,
if a new maximum for any of the sensors is detected then the LED will flash,
when you cannot get any further LED flashes in any direction then exit the calibration mode
with a command of
0xF8.
Please make sure that the CMPS11 is not located near to ferrous objects as this will distort
the magnetic field and induce errors in the reading.
While calibrating rotate the compass slowly.
Remember the axis of the magnetic field is unlikely to be horizontal, it dips into the earth
at an angle which varies depending on your location. At our offices in the UK it dips into the earth
at 67 degrees and that is the orientation each axis of the compass needs to be to find the maximums.
You need to find both positive and negative maximums for each axis so there are 6 points to calibrate.
The accelerometer is also calibrated at the same time, so the module should also be positioned horizontal,
inverted, and on all 4 sides to calibrate the 6 accelerometer points. Each accelerometer point needs
to be stable for 200mS for its reading to be used for calibration.
This delay is deliberate so that light taps to the module do not produces disruptive accelerometer readings
which would mess up the pitch and roll angles. There is no delay for the magnetic points.
The performance of the module is directly related to how well you perform calibration
so do this slowly and carefully.

To enter the horizontal calibration mode by sending a 3 byte sequence of
0xF0,0xF5 and then 0xF7
to the command register, these MUST be sent in 3 separate I2C frames,
you cannot send them all at once. There MUST be a minimum of 20ms between each I2C frame.
The LED will then extinguish and the CMPS11 should now be rotated in all directions on a horizontal plane,
if a new maximum for any of the sensors is detected then the LED will flash,
when you cannot get any further LED flashes in any direction
then exit the calibration mode with a command of 0xF8.
Please make sure that the CMPS11 is not located near to ferrous objects as this will distort the magnetic field
and induce errors in the reading.
While calibrating rotate the compass slowly.
Only the X and Y magnetometer axis are calibrated in this mode.


*/