/// RM3100 development board tests using I2C #include // in ../ArdIDE/Java/hardware/arduino/avr/libraries/ A4 A5 pins SDA SCL Uno #define RMI2C_ADD 0x20 // 32 0b010000 with b_0 and b_1 to grnd /* cyc 50 100 200 400 800 LSB/muT 20 38 75 150 303 .37 cyc +1 ? nT/LSB 50 26.3 13.3 6.67 3.33 rate Hz 533 283 147 80 40 [circuit 180 kHz] 1-axis rate/3 noise nT 30 20 15 11 8 linearity .5% default */ /* #define GAIN_FAC 13.3333 #define CYCLES 200 #define CDELAY 4 */ /* #define GAIN_FAC 6.6667 #define CYCLES 400 #define CDELAY 10 #define NO_TO_AVE 64 */ // power of 2 preferred for speed 2^6 #define GAIN_FAC 3.33 #define CYCLES 800 #define CDELAY 24 // 22 gives some duplicates #define NO_TO_AVE 32 // power of 2 preferred for speed 2^5 /* calibration biases (see anal.f, minf.f) - garden data fit # 30 and 31 c -83.52 102.23-221.75 1.0000 0.9954 1.0250 0.0073 0.0019 0.0053 42.47 30 240 c -86.57 98.27-222.81 1.0000 0.9993 1.0256 0.0067 0.0015 0.0032 68.17 31 240 c compromise -85 100 -222 - -.0002 +.0253 .0070 .0017 .0042 /1024 -2 26 7 2 4 */ #define XOFFSET -85L #define YOFFSET 100L #define ZOFFSET -222L #define YSCALE -2L #define ZSCALE 26L #define XYMIX 7L #define YZMIX 2L #define ZXMIX 4L /* // null biases for checking #define XOFFSET 0L #define YOFFSET 0L #define ZOFFSET 0L #define YSCALE 0L #define ZSCALE 0L #define XYMIX 0L #define YZMIX 0L #define ZXMIX 0L */ /* interpolate x y z in time x0 y0 z0 twait | x1 y1 z1 x,y,z: 2.5+tw||.5 1.5+tw||1.5 0.5+tw||2.5 fractions to 256 : 3+twait 80*3+16 76*3+28 etc */ // #define TWAIT 16 // 24 used in anal.f - 22 poss 39:217 117:139 217:39 #define XFAC0 40L // 256-TWAIT /6 #define XFAC1 216L // 256-TWAIT *5/6 + TWAIT #define YFAC0 120L // 256-TWAIT /2 #define YFAC1 136L // 256-TWAIT /2 + TWAIT #define ZFAC0 216L // 256-TWAIT *5/6 + TWAIT #define ZFAC1 40L // 256-TWAIT /6 /* I2C: std 100 kbit/s fast 400 fast+ 1000 high 3400 (all max) CMM timing default 0x96 -> 27ms or 37 Hz issue that rock/roll of boat will bias: (ave H_comp)^2 =/= (ave (H_comp^2) and x,y,z t=/= */ long xv, yv, zv, xyz; long total, totalT; float totalF; long timemicros; int count; long xv0 = 0L; // so defined for interpolation later long yv0 = 0L; long zv0 = 0L; bool readStatus(byte i2Address){ // read status register [conflicts with read data] Wire.beginTransmission (i2Address); Wire.write(byte(0x34)); Wire.endTransmission (); delay(1); if( Wire.requestFrom(i2Address, 1) == 0 ) { Serial.println("readStatus error"); return false; } else { byte status = Wire.read(); status = (status >> 7) & 0x01; // need bit 7 if ( status == 0x01 ) { return true;} return false; } } void printStatus(byte i2Address){ // read and print status if (readStatus(i2Address) ) {Serial.println("status true"); } else { Serial.println("status false"); } } void readCycleReg(byte i2Address) { // read cycle registers Wire.beginTransmission (i2Address); Wire.write(byte(0x04)); // CR address - read Wire.endTransmission(); delay(1); if( Wire.requestFrom(i2Address, 6) == 0 ) { // request 6 bytes 3 * 2-byte Serial.println(" readCycleReg error"); } else { xv=Wire.read(); xv = xv << 8; xv |= Wire.read(); yv=Wire.read(); yv = yv << 8; yv |= Wire.read(); zv=Wire.read(); zv = zv << 8; zv |= Wire.read(); Serial.print (xv); Serial.print (" "); Serial.print (yv); Serial.print (" "); Serial.print (zv); Serial.println (" "); } } void writeCycleReg(byte i2Address, int count) { // write cycle registers byte to_reg[6]; Wire.beginTransmission (i2Address); Wire.write(byte(0x04)); // CR address - write if (count > 800) count=800; else if (count < 30) count=30; to_reg[0] = count >> 8; to_reg[1] = count; to_reg[2] = to_reg[0]; to_reg[3] = to_reg[1]; to_reg[4] = to_reg[0]; to_reg[5] = to_reg[1]; Wire.write(to_reg,6); // to send buffer Wire.endTransmission(); } void setup() { Serial.begin (115200); // Serial.begin (9600); Serial.println ("RM3100 test"); Wire.begin(); // 100 kHz default Wire.beginTransmission (RMI2C_ADD); if (Wire.endTransmission () == 0) { Serial.print ("Found address: "); /* Serial.print (i, DEC); Serial.print (" (0x"); Serial.print (i, HEX); Serial.println (")"); */ delay (1); } Serial.print ("Found "); Serial.println (" device."); writeCycleReg(RMI2C_ADD,CYCLES); readCycleReg(RMI2C_ADD); printStatus(RMI2C_ADD); Wire.beginTransmission (RMI2C_ADD); Wire.write(byte(0b00000000)); // POLL reg Wire.write(byte(0b01110000)); // POLL 111 means z y x Wire.endTransmission(); // initiates measurement: DRDY high when done } void loop() { // single measurement on all 3 axes x y z is north, east, down // asked for results -- process last result while waiting count = 0; totalT=0; while( true ){ Wire.beginTransmission (RMI2C_ADD); Wire.write(byte(0b00100100)); // 24 Hex will auto increment to 2C Wire.endTransmission(); Wire.requestFrom(RMI2C_ADD, 9); // request 9 bytes 3 * 3-byte 2-comp if (9 <= Wire.available()) { xv = Wire.read(); // receive highest byte (overwrites previous reading) xv = xv << 8; // shift high byte to be higher 8 bits xv |= Wire.read(); // receive low byte as lower 8 bits xv = xv << 8; // shift high bytes to be higher 16 bits xv |= Wire.read(); // receive low byte as lower 8 bits // C gives you an arithmetic shift to the right not a logic shift to the right. (for int) xv = xv << 8; xv = xv >> 8; yv = Wire.read(); yv = yv << 8; yv |= Wire.read(); yv = yv << 8; yv |= Wire.read(); yv = yv << 8; yv = yv >> 8; zv = Wire.read(); zv = zv << 8; zv |= Wire.read(); zv = zv << 8; zv |= Wire.read(); zv = zv << 8; zv = zv >> 8; /* to check bias corrections and interpolation: seems OK Serial.print(xv); Serial.print(" "); Serial.print(yv); Serial.print(" "); Serial.print(zv); Serial.print(" "); */ // calibration corrections components <~ 16000 (800 cycles) // care xv*XYMIX can be > 32768 so need long int (XYMIX defined with L) xv += XOFFSET; yv += YOFFSET; zv += ZOFFSET; yv += ( (yv * YSCALE ) >> 10 ); zv += ( (zv * ZSCALE ) >> 10 ); xv += ( ((yv * XYMIX) >> 10) + ((zv * ZXMIX ) >> 10) ); yv += ( ((xv * XYMIX) >> 10) + ((zv * YZMIX ) >> 10) ); zv += ( ((xv * ZXMIX) >> 10) + ((yv * YZMIX ) >> 10) ); // interpolate x y z in time x0 y0 z0 twait x1 y1 z1 // fractions to 256 xyz = ( XFAC0 * xv0 + XFAC1 * xv ) >> 8; xv0=xv; xv=xyz; xyz = ( YFAC0 * yv0 + YFAC1 * yv ) >> 8; yv0=yv; yv=xyz; xyz = ( ZFAC0 * zv0 + ZFAC1 * zv ) >> 8; zv0=zv; zv=xyz; /* Serial.print(millis()); // 200 and 800 25-30 ms fast serial Serial.print(" "); */ Serial.print(xv); Serial.print(" "); Serial.print(yv); Serial.print(" "); Serial.print(zv); Serial.print(" "); totalF = xv * xv+ yv * yv+ zv * zv; totalF = sqrt(totalF) * GAIN_FAC ; total = totalF; Serial.println(total); } delay(CDELAY); // gives different result each call - cycl=800 (22 OK 21 doesnt): 24-25ms / loop // cycl=400 10ms min (uses 13ms/loop) // cycl=200 4ms min (uses 6-7ms/loop) // so loop adds circa 2.5ms to delay - before averaging Wire.beginTransmission (RMI2C_ADD); Wire.write(byte(0b00000000)); // POLL reg Wire.write(byte(0b01110000)); // POLL 111 means z y x Wire.endTransmission(); // initiates measurement: DRDY high when done totalT = totalT + total; if( count == NO_TO_AVE - 1 ) { Serial.print(" ave="); Serial.println((totalT >> 5)); // 2^5 or 2^6 = NO_TO_AVE break; } count++; } }