Diff: JON_IMU/IMU.cpp

Revision:

0:0309bb86b703

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/JON_IMU/IMU.cpp	Thu Feb 23 22:59:51 2012 +0000
@@ -0,0 +1,89 @@
+#include "IMU.h"
+#include "mbed.h"
+
+IMU::IMU(PinName XGyro, PinName YAccel, PinName ZAccel, PinName AZ):
+        _XGyro(XGyro), _YAccel(YAccel), _ZAccel(ZAccel), _AZ(AZ) {
+    _AZ = 0;
+}
+
+void IMU::initialise(void) {
+    _AZ = 1;
+    wait_us(1500);
+    _AZ = 0;
+    wait(0.01); //requires 7ms to calibrate
+
+    samples = 100;
+    for (int i=0; i<100; i++)    reading[i]=_XGyro.read();
+    Median();
+
+    Gyro_offset = median;
+    t.start();
+}
+
+//****************************************************************************/
+// Median Filter
+//****************************************************************************/
+void IMU::Median(void)  {
+    //sort the value of the array
+    for (int i=0; i< samples-1; i++) {
+        for (int j=i+1; j< samples; j++) {
+            if (reading[i] > reading[j]) {
+
+                w = reading[i];
+                reading[i] = reading[j];
+                reading[j] = w;
+            }
+        }
+
+    }
+
+    //find the middle value of the sample
+    middle = (samples+1)/2;
+    median = reading[middle];
+}
+
+void IMU::update(void) {
+
+    Xrate = -(_XGyro.read() -Gyro_offset)*3.3/0.002; //Voltage - Gyro_offset / Sensitivity
+    GyroAngleX += Xrate*t.read();
+
+    //Accelerometer Y
+    samples = 60;
+    for (int i=0; i<samples; i++)    reading[i]=_YAccel.read();
+    Median(); //100 point Median Filter
+
+    Y = (median*3.3 - ZERO_G)/0.33; //Voltage - ZERO G bias (3.3/2) / Sensitivity ( + 0.03711048?)
+
+    //Accelerometer Z
+
+    samples = 60;
+    for (int i=0; i<samples; i++)    reading[i]=_ZAccel.read();
+    Median(); //100 point Median Filter
+
+    Z = (median*3.3 - ZERO_G)/0.33; //Voltage - ZERO G bias (3.3/2) / Sensitivity
+
+    //Calculate Angle + map range
+    AccAngleX = Rad2Deg* atan2(-Y,-Z) - 90; //arctan = O/A, radian to degree
+
+    if (AccAngleX<-180)  AccAngleX=AccAngleX+360; //Maintain -180 to 180
+
+    //Complementary Filter
+    Roll = (0.98)*(Roll + Xrate*t.read()) + (0.02)*(AccAngleX);
+    t.reset();
+}
+
+
+float IMU::getRoll(void) {
+
+    return Roll;
+}
+
+float IMU::getGyrox(void) {
+
+    return GyroAngleX;
+}
+
+float IMU::getAccelx(void) {
+
+    return AccAngleX;
+}
\ No newline at end of file