Astrobe Forum

Code: Select all

MODULE Accelerometer;
(* =========================================================================  
   Example ARM Oberon I2C Accelerometer Program  

   Description:
     Reads the accelerometer X, Y, Z and temperature values every 50ms and 
     displays the values whenever the change exceeds a specified threshold. 
   
   Target: 
     LPC17xx systems with a Bosch SBM380 3-axis Accelerometer 
     connected to the I2C2 bus

   Tested with:
     Olimex LPC1766-STK  
     
   Refs: 
     NXP LPC17xx User Manual UM10360
     Bosch SBM380 Datasheet
     Oberon for Cortex-M3 Microcontrollers
   
   (c) 2010-2012 CFB Software   
   http://www.astrobe.com  
   
   ========================================================================= *)

IMPORT MCU, I2C, SYSTEM, Main, Out, Timer;


PROCEDURE* DataToInt(lsb, msb: SYSTEM.BYTE): INTEGER;
(* The result is a 10-bit 2's complement integer:
     result:9:2 := msb:7:0, result:0:1 = lsb:7:6 
   First store the data as the most significant 10 bits of the result
   so that we can then use ASR to extend the sign bit while shifting the 
   data to the least significant 10 bits.
*)
  RETURN ASR(LSL(ORD(msb), 24) + LSL(ORD(lsb), 16), 22)
END DataToInt;


PROCEDURE* DataToDegs(data: SYSTEM.BYTE): INTEGER;
(* data = 0..255, result = -30 deg to 97.5 deg *)
  RETURN -30 + (ORD(data) DIV 2)
END DataToDegs;


PROCEDURE* Diff(prev, this: INTEGER): INTEGER;
  RETURN ABS(prev - this)
END Diff;


PROCEDURE OutData(label: ARRAY OF CHAR; data: INTEGER);
BEGIN
  Out.String(label); Out.Int(data, 5)
END OutData;


PROCEDURE Run();
CONST
  I2CFreq = 400000;
  I2CBus = 2;
  SBM380Addr = 038H;
  (* Minimum change to be reported *)
  Threshold = 10;
  (* 10-bit signed integer *)
  MaxXYZ = 511;
VAR
  data: ARRAY 8 OF SYSTEM.BYTE;
  status, thisX, thisY, thisZ, prevX, prevY, prevZ, degrees: INTEGER;
  dataAddr: CHAR;
BEGIN
  I2C.Init(I2CBus, I2CFreq);
  dataAddr := 02X;
  
  prevX := MaxXYZ + Threshold + 1; 
  prevY := MaxXYZ + Threshold + 1; 
  prevZ := MaxXYZ + Threshold + 1;
  
  WHILE TRUE DO 
    status := I2C.Read(SBM380Addr, dataAddr, data);
    IF status = I2C.OK THEN 
      
    (* Convert the raw data *)
      thisX := DataToInt(data[0], data[1]); 
      thisY := DataToInt(data[2], data[3]); 
      thisZ := DataToInt(data[4], data[5]); 
      degrees := DataToDegs(data[6]);
      
      (* Check if the change is enough to be reported *)
      IF (Diff(prevX, thisX) > Threshold)
      OR (Diff(prevY, thisY) > Threshold)
      OR (Diff(prevZ, thisZ) > Threshold) THEN 
        OutData("x:", thisX); 
        OutData(", y:", thisY); 
        OutData(", z:", thisZ); 
        OutData(", deg:", degrees);
        Out.Ln();
        prevX := thisX; prevY := thisY; prevZ := thisZ;
      END
      
    ELSE
      OutData("I2C Read Error: ", status);
      Out.Ln()
    END;
    Timer.MSecDelay(50)
  END
END Run;


BEGIN
  Run()
END Accelerometer.