first commit

dev/imu.cpp

@@ -0,0 +1,67 @@
+#include "i2c.h"
+
+#include "imu.h"
+
+static const unsigned char IMU_Addr = 0x6A; // ACC GYR
+
+static inline short Rev16(short v)
+{
+  asm("REV16 %1, %0" : "=r" (v) : "r" (v)); // v = v<<8 | v>>8;
+  return v;
+}
+//------------------------------------------------------------------------------
+
+static inline void IMU_SetReg(unsigned char Reg, unsigned char Value)
+{
+  unsigned char reg[2];
+  reg[0]=Reg; reg[1]=Value;
+  I2C1_Write(IMU_Addr, reg, 2);
+  I2C1_Stop();
+}
+//------------------------------------------------------------------------------
+
+static inline unsigned char IMU_GetReg(unsigned char Reg)
+{
+  I2C1_Write(IMU_Addr, Reg);
+  I2C1_Read(IMU_Addr, &Reg, 1);
+  I2C1_Stop();
+  return Reg;
+}
+//------------------------------------------------------------------------------
+
+void IMU_Init() // 
+{ 
+  I2C1_Init();
+  
+  for(int a=0; a<100000; a++) { asm volatile("NOP"); }
+
+  unsigned char wai=IMU_GetReg(0x0F);
+  
+   IMU_SetReg(0x10, 0x38);
+  IMU_SetReg(0x11, 0x48);
+  IMU_SetReg(0x12, 0x06);
+  
+  for(int a=0; a<100000; a++) { asm volatile("NOP"); }
+}
+//------------------------------------------------------------------------------
+
+void IMU_Get(IMU_Data& Data)
+{
+  struct {short temp; short gx; short gy; short gz; short ax; short ay; short az; } data;
+
+  I2C1_Write(IMU_Addr, 0x20);
+  I2C1_Read(IMU_Addr, &data, sizeof(data));
+  I2C1_Stop();
+
+  Data.Temp  = 21+(((long)Rev16(data.temp))*128)/42735; // 21+(temperature / 333.87)
+  
+  Data.Acc.X = Rev16(data.ax);
+  Data.Acc.Y = Rev16(data.ay);
+  Data.Acc.Z = Rev16(data.az);
+  
+  Data.Gyr.X = Rev16(data.gx);
+  Data.Gyr.Y = Rev16(data.gy);
+  Data.Gyr.Z = Rev16(data.gz);
+  
+}
+//------------------------------------------------------------------------------