import code from Fabrice's repository

diff --git a/IMU_Razor9DOF.py b/IMU_Razor9DOF.py
new file mode 100644 (file)
index 0000000..91351ae
--- /dev/null
+++ b/IMU_Razor9DOF.py
@@ -0,0 +1,146 @@
+# Test for Razor 9DOF IMU\r
+# Jose Julio @2009\r
+# This script needs VPhyton, pyserial and pywin modules\r
+\r
+# First Install Python 2.6.4\r
+# Install pywin from http://sourceforge.net/projects/pywin32/\r
+# Install pyserial from http://sourceforge.net/projects/pyserial/files/\r
+# Install Vphyton from http://vpython.org/contents/download_windows.html\r
+\r
+from visual import *\r
+import serial\r
+import string\r
+import math\r
+import sys\r
+\r
+from time import time\r
+\r
+#ev = scene.waitfor('click keydown')\r
+\r
+grad2rad = 3.141592/180.0\r
+\r
+# Check your COM port and baud rate\r
+ser = serial.Serial(port='/dev/ttyUSB0',baudrate=57600, timeout=1)\r
+\r
+# Main scene\r
+scene=display(title="9DOF Razor IMU test")\r
+scene.range=(1.2,1.2,1.2)\r
+#scene.forward = (0,-1,-0.25)\r
+scene.forward = (1,0,-0.25)\r
+scene.up=(0,0,1)\r
+\r
+# Second scene (Roll, Pitch, Yaw)\r
+scene2 = display(title='9DOF Razor IMU test',x=0, y=0, width=500, height=200,center=(0,0,0), background=(0,0,0))\r
+scene2.range=(1,1,1)\r
+scene.width=500\r
+scene.y=200\r
+\r
+scene2.select()\r
+#Roll, Pitch, Yaw\r
+cil_roll = cylinder(pos=(-0.4,0,0),axis=(0.2,0,0),radius=0.01,color=color.red)\r
+cil_roll2 = cylinder(pos=(-0.4,0,0),axis=(-0.2,0,0),radius=0.01,color=color.red)\r
+cil_pitch = cylinder(pos=(0.1,0,0),axis=(0.2,0,0),radius=0.01,color=color.green)\r
+cil_pitch2 = cylinder(pos=(0.1,0,0),axis=(-0.2,0,0),radius=0.01,color=color.green)\r
+#cil_course = cylinder(pos=(0.6,0,0),axis=(0.2,0,0),radius=0.01,color=color.blue)\r
+#cil_course2 = cylinder(pos=(0.6,0,0),axis=(-0.2,0,0),radius=0.01,color=color.blue)\r
+arrow_course = arrow(pos=(0.6,0,0),color=color.cyan,axis=(-0.2,0,0), shaftwidth=0.02, fixedwidth=1)\r
+\r
+#Roll,Pitch,Yaw labels\r
+label(pos=(-0.4,0.3,0),text="Roll",box=0,opacity=0)\r
+label(pos=(0.1,0.3,0),text="Pitch",box=0,opacity=0)\r
+label(pos=(0.55,0.3,0),text="Yaw",box=0,opacity=0)\r
+label(pos=(0.6,0.22,0),text="N",box=0,opacity=0,color=color.yellow)\r
+label(pos=(0.6,-0.22,0),text="S",box=0,opacity=0,color=color.yellow)\r
+label(pos=(0.38,0,0),text="W",box=0,opacity=0,color=color.yellow)\r
+label(pos=(0.82,0,0),text="E",box=0,opacity=0,color=color.yellow)\r
+label(pos=(0.75,0.15,0),height=7,text="NE",box=0,color=color.yellow)\r
+label(pos=(0.45,0.15,0),height=7,text="NW",box=0,color=color.yellow)\r
+label(pos=(0.75,-0.15,0),height=7,text="SE",box=0,color=color.yellow)\r
+label(pos=(0.45,-0.15,0),height=7,text="SW",box=0,color=color.yellow)\r
+\r
+L1 = label(pos=(-0.4,0.22,0),text="-",box=0,opacity=0)\r
+L2 = label(pos=(0.1,0.22,0),text="-",box=0,opacity=0)\r
+L3 = label(pos=(0.7,0.3,0),text="-",box=0,opacity=0)\r
+\r
+# Main scene objects\r
+scene.select()\r
+# Reference axis (x,y,z)\r
+arrow(color=color.green,axis=(1,0,0), shaftwidth=0.02, fixedwidth=1)\r
+arrow(color=color.green,axis=(0,-1,0), shaftwidth=0.02 , fixedwidth=1)\r
+arrow(color=color.green,axis=(0,0,-1), shaftwidth=0.02, fixedwidth=1)\r
+# labels\r
+label(pos=(0,0,0.8),text="9DOF Razor IMU test",box=0,opacity=0)\r
+label(pos=(1,0,0),text="X",box=0,opacity=0)\r
+label(pos=(0,-1,0),text="Y",box=0,opacity=0)\r
+label(pos=(0,0,-1),text="Z",box=0,opacity=0)\r
+# IMU object\r
+platform = box(length=1, height=0.05, width=1, color=color.red)\r
+p_line = box(length=1,height=0.08,width=0.1,color=color.yellow)\r
+plat_arrow = arrow(color=color.green,axis=(1,0,0), shaftwidth=0.06, fixedwidth=1)\r
+\r
+\r
+f = open("Serial"+str(time())+".txt", 'w')\r
+\r
+roll=0\r
+pitch=0\r
+yaw=0\r
+cpt = 0\r
+t_start = time()\r
+while 1:\r
+    line = ser.readline()\r
+    line = line.replace("!ANG:","")   # Delete "!ANG:"\r
+    #print line\r
+    f.write(line)                     # Write to the output log file\r
+    words = string.split(line,"\t")    # Fields split\r
+    #print words\r
+    print words\r
+    if len(words) != 3:\r
+        continue\r
+\r
+    """\r
+    a = float(words[0])#*grad2rad\r
+    b = float(words[1])#*grad2rad\r
+    c = float(words[2])#*grad2rad\r
+    d = float(words[3])#*grad2rad\r
+\r
+    platform.axis = (1, 1, 1)\r
+    #print a, b, c, d\r
+    platform.rotate(angle=a, axis = (b, c, d))\r
+    continue\r
+    """\r
+    try:\r
+        roll = float(words[0])#*grad2rad\r
+        pitch = float(words[1])#*grad2rad\r
+        yaw = float(words[2])#*grad2rad\r
+    except:\r
+        print "Invalid line"\r
+        continue\r
+\r
+    cpt += 1\r
+    if cpt > 10:\r
+        t = time() - t_start\r
+        t_start = time()\r
+        print "XXX", cpt/t\r
+        cpt = 0\r
+\r
+    axis=(cos(pitch)*cos(yaw),-cos(pitch)*sin(yaw),sin(pitch))\r
+    up=(sin(roll)*sin(yaw)+cos(roll)*sin(pitch)*cos(yaw),sin(roll)*cos(yaw)-cos(roll)*sin(pitch)*sin(yaw),-cos(roll)*cos(pitch))\r
+    platform.axis=axis\r
+    platform.up=up\r
+    platform.length=1.0\r
+    platform.width=0.65\r
+    plat_arrow.axis=axis\r
+    plat_arrow.up=up\r
+    plat_arrow.length=0.8\r
+    p_line.axis=axis\r
+    p_line.up=up\r
+    cil_roll.axis=(0.2*cos(roll),0.2*sin(roll),0)\r
+    cil_roll2.axis=(-0.2*cos(roll),-0.2*sin(roll),0)\r
+    cil_pitch.axis=(0.2*cos(pitch),0.2*sin(pitch),0)\r
+    cil_pitch2.axis=(-0.2*cos(pitch),-0.2*sin(pitch),0)\r
+    arrow_course.axis=(0.2*sin(yaw),0.2*cos(yaw),0)\r
+    L1.text = str(float(words[0]))\r
+    L2.text = str(float(words[1]))\r
+    L3.text = str(float(words[2]))\r
+ser.close()\r
+f.close()\r

diff --git a/MadgwickAHRS.c b/MadgwickAHRS.c
new file mode 100644 (file)
index 0000000..fc547d2
--- /dev/null
+++ b/MadgwickAHRS.c
@@ -0,0 +1,441 @@
+//=====================================================================================================\r
+// MadgwickAHRS.c\r
+//=====================================================================================================\r
+//\r
+// Implementation of Madgwick's IMU and AHRS algorithms.\r
+// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms\r
+//\r
+// Date                        Author          Notes\r
+// 29/09/2011  SOH Madgwick    Initial release\r
+// 02/10/2011  SOH Madgwick    Optimised for reduced CPU load\r
+// 19/02/2012  SOH Madgwick    Magnetometer measurement is normalised\r
+//\r
+//=====================================================================================================\r
+\r
+//---------------------------------------------------------------------------------------------------\r
+// Header files\r
+\r
+#include "MadgwickAHRS.h"\r
+#include <math.h>\r
+\r
+#include <f32.h>\r
+\r
+\r
+//---------------------------------------------------------------------------------------------------\r
+// Definitions\r
+\r
+//#define sampleFreq   512.0f          // sample frequency in Hz\r
+//#define sampleFreq   46.0f           // sample frequency in Hz\r
+#define sampleFreq     85.0f           // sample frequency in Hz\r
+#define betaDef                0.1f            // 2 * proportional gain\r
+\r
+//---------------------------------------------------------------------------------------------------\r
+// Variable definitions\r
+\r
+volatile float beta = betaDef;                                                         // 2 * proportional gain (Kp)\r
+volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;     // quaternion of sensor frame relative to auxiliary frame\r
+\r
+\r
+//---------------------------------------------------------------------------------------------------\r
+// Function declarations\r
+\r
+float invSqrt(float x);\r
+\r
+//====================================================================================================\r
+// Functions\r
+\r
+//---------------------------------------------------------------------------------------------------\r
+// AHRS algorithm update\r
+\r
+void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {\r
+       float recipNorm;\r
+       float s0, s1, s2, s3;\r
+       float qDot1, qDot2, qDot3, qDot4;\r
+       float hx, hy;\r
+       float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;\r
+\r
+       // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)\r
+       if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {\r
+               MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);\r
+               return;\r
+       }\r
+\r
+       // Rate of change of quaternion from gyroscope\r
+       qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);\r
+       qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);\r
+       qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);\r
+       qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);\r
+\r
+       // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)\r
+       if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {\r
+\r
+               // Normalise accelerometer measurement\r
+               recipNorm = invSqrt(ax * ax + ay * ay + az * az);\r
+               ax *= recipNorm;\r
+               ay *= recipNorm;\r
+               az *= recipNorm;\r
+\r
+               // Normalise magnetometer measurement\r
+               recipNorm = invSqrt(mx * mx + my * my + mz * mz);\r
+               mx *= recipNorm;\r
+               my *= recipNorm;\r
+               mz *= recipNorm;\r
+\r
+               // Auxiliary variables to avoid repeated arithmetic\r
+               _2q0mx = 2.0f * q0 * mx;\r
+               _2q0my = 2.0f * q0 * my;\r
+               _2q0mz = 2.0f * q0 * mz;\r
+               _2q1mx = 2.0f * q1 * mx;\r
+               _2q0 = 2.0f * q0;\r
+               _2q1 = 2.0f * q1;\r
+               _2q2 = 2.0f * q2;\r
+               _2q3 = 2.0f * q3;\r
+               _2q0q2 = 2.0f * q0 * q2;\r
+               _2q2q3 = 2.0f * q2 * q3;\r
+               q0q0 = q0 * q0;\r
+               q0q1 = q0 * q1;\r
+               q0q2 = q0 * q2;\r
+               q0q3 = q0 * q3;\r
+               q1q1 = q1 * q1;\r
+               q1q2 = q1 * q2;\r
+               q1q3 = q1 * q3;\r
+               q2q2 = q2 * q2;\r
+               q2q3 = q2 * q3;\r
+               q3q3 = q3 * q3;\r
+\r
+               // Reference direction of Earth's magnetic field\r
+               hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;\r
+               hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;\r
+               _2bx = sqrt(hx * hx + hy * hy);\r
+               _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;\r
+               _4bx = 2.0f * _2bx;\r
+               _4bz = 2.0f * _2bz;\r
+\r
+               // Gradient decent algorithm corrective step\r
+               s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);\r
+               s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);\r
+               s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);\r
+               s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);\r
+               recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude\r
+               s0 *= recipNorm;\r
+               s1 *= recipNorm;\r
+               s2 *= recipNorm;\r
+               s3 *= recipNorm;\r
+\r
+               // Apply feedback step\r
+               qDot1 -= beta * s0;\r
+               qDot2 -= beta * s1;\r
+               qDot3 -= beta * s2;\r
+               qDot4 -= beta * s3;\r
+       }\r
+\r
+       // Integrate rate of change of quaternion to yield quaternion\r
+       q0 += qDot1 * (1.0f / sampleFreq);\r
+       q1 += qDot2 * (1.0f / sampleFreq);\r
+       q2 += qDot3 * (1.0f / sampleFreq);\r
+       q3 += qDot4 * (1.0f / sampleFreq);\r
+\r
+       // Normalise quaternion\r
+       recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);\r
+       q0 *= recipNorm;\r
+       q1 *= recipNorm;\r
+       q2 *= recipNorm;\r
+       q3 *= recipNorm;\r
+}\r
+\r
+//---------------------------------------------------------------------------------------------------\r
+// IMU algorithm update\r
+\r
+void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {\r
+       float recipNorm;\r
+       float s0, s1, s2, s3;\r
+       float qDot1, qDot2, qDot3, qDot4;\r
+       float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;\r
+\r
+       // Rate of change of quaternion from gyroscope\r
+       qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);\r
+       qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);\r
+       qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);\r
+       qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);\r
+\r
+\r
+       // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)\r
+       if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {\r
+\r
+               // Normalise accelerometer measurement\r
+               recipNorm = invSqrt(ax * ax + ay * ay + az * az);\r
+               ax *= recipNorm;\r
+               ay *= recipNorm;\r
+               az *= recipNorm;\r
+\r
+               // Auxiliary variables to avoid repeated arithmetic\r
+               _2q0 = 2.0f * q0;\r
+               _2q1 = 2.0f * q1;\r
+               _2q2 = 2.0f * q2;\r
+               _2q3 = 2.0f * q3;\r
+               _4q0 = 4.0f * q0;\r
+               _4q1 = 4.0f * q1;\r
+               _4q2 = 4.0f * q2;\r
+               _8q1 = 8.0f * q1;\r
+               _8q2 = 8.0f * q2;\r
+               q0q0 = q0 * q0;\r
+               q1q1 = q1 * q1;\r
+               q2q2 = q2 * q2;\r
+               q3q3 = q3 * q3;\r
+\r
+\r
+               // Gradient decent algorithm corrective step\r
+               s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;\r
+               s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;\r
+               s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;\r
+               s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;\r
+               recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude\r
+\r
+\r
+\r
+               s0 *= recipNorm;\r
+               s1 *= recipNorm;\r
+               s2 *= recipNorm;\r
+               s3 *= recipNorm;\r
+\r
+\r
+               // Apply feedback step\r
+               qDot1 -= beta * s0;\r
+               qDot2 -= beta * s1;\r
+               qDot3 -= beta * s2;\r
+               qDot4 -= beta * s3;\r
+       }\r
+\r
+       // Integrate rate of change of quaternion to yield quaternion\r
+       q0 += qDot1 * (1.0f / sampleFreq);\r
+       q1 += qDot2 * (1.0f / sampleFreq);\r
+       q2 += qDot3 * (1.0f / sampleFreq);\r
+       q3 += qDot4 * (1.0f / sampleFreq);\r
+\r
+\r
+       // Normalise quaternion\r
+       recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);\r
+       q0 *= recipNorm;\r
+       q1 *= recipNorm;\r
+       q2 *= recipNorm;\r
+       q3 *= recipNorm;\r
+\r
+       //printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", q0, q1, q2);\r
+\r
+}\r
+\r
+\r
+//---------------------------------------------------------------------------------------------------\r
+// IMU algorithm update\r
+\r
+f32 f_q0;\r
+f32 f_q1;\r
+f32 f_q2;\r
+f32 f_q3;\r
+\r
+\r
+void Mad_f32_init()\r
+{\r
+       f_q0 = f32_from_double((double)1.0);\r
+       f_q1 = f32_from_double((double)0.0);\r
+       f_q2 = f32_from_double((double)0.0);\r
+       f_q3 = f32_from_double((double)0.0);\r
+\r
+}\r
+void MadgwickAHRSupdateIMU_f32(float gx, float gy, float gz, float ax, float ay, float az) {\r
+       f32 f_recipNorm;\r
+       f32 f_s0, f_s1, f_s2, f_s3;\r
+       f32 f_qDot1, f_qDot2, f_qDot3, f_qDot4;\r
+       f32 f__2q0, f__2q1, f__2q2, f__2q3, f__4q0, f__4q1, f__4q2 ,f__8q1, f__8q2, f_q0q0, f_q1q1, f_q2q2, f_q3q3;\r
+\r
+       f32 f_gx, f_gy, f_gz;\r
+       f32 f_ax, f_ay, f_az;\r
+\r
+       f32 f_beta;\r
+       f32 f_sampleFreq;\r
+\r
+\r
+       f_gx = f32_from_double((double)gx);\r
+       f_gy = f32_from_double((double)gy);\r
+       f_gz = f32_from_double((double)gz);\r
+\r
+       f_ax = f32_from_double((double)ax);\r
+       f_ay = f32_from_double((double)ay);\r
+       f_az = f32_from_double((double)az);\r
+\r
+       f_beta = f32_from_double((double)beta);\r
+       f_sampleFreq = f32_from_double((double)sampleFreq);\r
+\r
+\r
+       // Rate of change of quaternion from gyroscope\r
+       /*\r
+       qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);\r
+       qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);\r
+       qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);\r
+       qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);\r
+       */\r
+\r
+       f_qDot1 = f32_mul(f32_sub(f32_sub(f32_neg(f32_mul(f_gx, f_q1)), f32_mul(f_gy, f_q2)), f32_mul(f_gz, f_q3)), f32_from_double(0.5));\r
+       f_qDot2 = f32_mul(f32_sub(f32_add(f32_mul(f_gx, f_q0), f32_mul(f_gz, f_q2)), f32_mul(f_gy, f_q3)), f32_from_double(0.5));\r
+       f_qDot3 = f32_mul(f32_add(f32_mul(f_gx, f_q3), f32_sub(f32_mul(f_gy, f_q0), f32_mul(f_gz, f_q1))), f32_from_double(0.5));\r
+       f_qDot4 = f32_mul(f32_sub(f32_add(f32_mul(f_gy, f_q1), f32_mul(f_gz, f_q0)), f32_mul(f_gx, f_q2)), f32_from_double(0.5));\r
+\r
+\r
+\r
+\r
+       // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)\r
+       if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {\r
+\r
+               // Normalise accelerometer measurement\r
+               //recipNorm = invSqrt(ax * ax + ay * ay + az * az);\r
+               f_recipNorm = f32_inv(f32_sqrt((f32_add(f32_mul(f_ax, f_ax), f32_add(f32_mul(f_ay, f_ay), f32_mul(f_az, f_az))))));\r
+               /*\r
+               ax *= recipNorm;\r
+               ay *= recipNorm;\r
+               az *= recipNorm;\r
+               */\r
+\r
+               f_ax = f32_mul(f_ax, f_recipNorm);\r
+               f_ay = f32_mul(f_ay, f_recipNorm);\r
+               f_az = f32_mul(f_az, f_recipNorm);\r
+\r
+               // Auxiliary variables to avoid repeated arithmetic\r
+\r
+               /*\r
+               _2q0 = 2.0f * q0;\r
+               _2q1 = 2.0f * q1;\r
+               _2q2 = 2.0f * q2;\r
+               _2q3 = 2.0f * q3;\r
+               _4q0 = 4.0f * q0;\r
+               _4q1 = 4.0f * q1;\r
+               _4q2 = 4.0f * q2;\r
+               _8q1 = 8.0f * q1;\r
+               _8q2 = 8.0f * q2;\r
+               q0q0 = q0 * q0;\r
+               q1q1 = q1 * q1;\r
+               q2q2 = q2 * q2;\r
+               q3q3 = q3 * q3;\r
+               */\r
+\r
+               f__2q0 = f32_mul(f32_from_double(2.0f), f_q0);\r
+               f__2q1 = f32_mul(f32_from_double(2.0f), f_q1);\r
+               f__2q2 = f32_mul(f32_from_double(2.0f), f_q2);\r
+               f__2q3 = f32_mul(f32_from_double(2.0f), f_q3);\r
+               f__4q0 = f32_mul(f32_from_double(4.0f), f_q0);\r
+               f__4q1 = f32_mul(f32_from_double(4.0f), f_q1);\r
+               f__4q2 = f32_mul(f32_from_double(4.0f), f_q2);\r
+               f__8q1 = f32_mul(f32_from_double(8.0f), f_q1);\r
+               f__8q2 = f32_mul(f32_from_double(8.0f), f_q2);\r
+               f_q0q0 = f32_mul(f_q0, f_q0);\r
+               f_q1q1 = f32_mul(f_q1, f_q1);\r
+               f_q2q2 = f32_mul(f_q2, f_q2);\r
+               f_q3q3 = f32_mul(f_q3, f_q3);\r
+\r
+\r
+               // Gradient decent algorithm corrective step\r
+\r
+               /*\r
+               s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;\r
+               s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;\r
+               s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;\r
+               s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;\r
+               recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude\r
+               */\r
+\r
+               f_s0 = f32_sub(f32_add(f32_mul(f__2q2, f_ax), f32_add(f32_mul(f__4q0, f_q1q1), f32_mul(f__4q0, f_q2q2))), f32_mul(f__2q1, f_ay));\r
+               f_s1 = f32_add(f32_mul(f__4q1, f_az), f32_add(f32_mul(f__8q1, f_q1q1), f32_add(f32_mul(f__8q1, f_q2q2), f32_sub(f32_sub(f32_add(f32_mul(f_q0q0, f32_mul(f_q1, f32_from_double(4.0f))), f32_sub(f32_mul(f__4q1, f_q3q3), f32_mul(f__2q3, f_ax))), f32_mul(f__2q0, f_ay)), f__4q1))));\r
+               f_s2 = f32_add(f32_mul(f__4q2, f_az), f32_add(f32_mul(f__8q2, f_q1q1), f32_add(f32_mul(f__8q2, f_q2q2), f32_sub(f32_sub(f32_add(f32_mul(f__2q0, f_ax), f32_add(f32_mul(f__4q2, f_q3q3), f32_mul(f_q0q0, f32_mul(f_q2, f32_from_double(4.0))))), f32_mul(f__2q3, f_ay)), f__4q2))));\r
+               f_s3 = f32_sub(f32_add(f32_mul(f_q2q2, f32_mul(f_q3, f32_from_double(4.0))), f32_sub(f32_mul(f_q1q1, f32_mul(f_q3, f32_from_double(4.0))), f32_mul(f__2q1, f_ax))), f32_mul(f__2q2, f_ay));\r
+               f_recipNorm = f32_inv(f32_sqrt(f32_add(f32_mul(f_s0, f_s0), f32_add(f32_mul(f_s1, f_s1), f32_add(f32_mul(f_s2, f_s2), f32_mul(f_s3, f_s3))))));\r
+\r
+               /*\r
+               s0 *= recipNorm;\r
+               s1 *= recipNorm;\r
+               s2 *= recipNorm;\r
+               s3 *= recipNorm;\r
+               */\r
+\r
+               f_s0 = f32_mul(f_s0, f_recipNorm);\r
+               f_s1 = f32_mul(f_s1, f_recipNorm);\r
+               f_s2 = f32_mul(f_s2, f_recipNorm);\r
+               f_s3 = f32_mul(f_s3, f_recipNorm);\r
+\r
+\r
+               // Apply feedback step\r
+\r
+               /*\r
+               qDot1 -= beta * s0;\r
+               qDot2 -= beta * s1;\r
+               qDot3 -= beta * s2;\r
+               qDot4 -= beta * s3;\r
+               */\r
+\r
+               f_qDot1 = f32_sub(f_qDot1, f32_mul(f_beta, f_s0));\r
+               f_qDot2 = f32_sub(f_qDot2, f32_mul(f_beta, f_s1));\r
+               f_qDot3 = f32_sub(f_qDot3, f32_mul(f_beta, f_s2));\r
+               f_qDot4 = f32_sub(f_qDot4, f32_mul(f_beta, f_s3));\r
+\r
+       }\r
+\r
+       // Integrate rate of change of quaternion to yield quaternion\r
+\r
+       /*\r
+       q0 += qDot1 * (1.0f / sampleFreq);\r
+       q1 += qDot2 * (1.0f / sampleFreq);\r
+       q2 += qDot3 * (1.0f / sampleFreq);\r
+       q3 += qDot4 * (1.0f / sampleFreq);\r
+       */\r
+\r
+       f_q0 = f32_add(f_q0, f32_mul(f_qDot1, f32_inv(f_sampleFreq)));\r
+       f_q1 = f32_add(f_q1, f32_mul(f_qDot2, f32_inv(f_sampleFreq)));\r
+       f_q2 = f32_add(f_q2, f32_mul(f_qDot3, f32_inv(f_sampleFreq)));\r
+       f_q3 = f32_add(f_q3, f32_mul(f_qDot4, f32_inv(f_sampleFreq)));\r
+\r
+       // Normalise quaternion\r
+       //recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);\r
+\r
+       f_recipNorm = f32_inv(f32_sqrt(f32_add(f32_mul(f_q0, f_q0), f32_add(f32_mul(f_q1, f_q1), f32_add(f32_mul(f_q2, f_q2), f32_mul(f_q3, f_q3))))));\r
+\r
+       /*\r
+       q0 *= recipNorm;\r
+       q1 *= recipNorm;\r
+       q2 *= recipNorm;\r
+       q3 *= recipNorm;\r
+       */\r
+\r
+       f_q0 = f32_mul(f_q0, f_recipNorm);\r
+       f_q1 = f32_mul(f_q1, f_recipNorm);\r
+       f_q2 = f32_mul(f_q2, f_recipNorm);\r
+       f_q3 = f32_mul(f_q3, f_recipNorm);\r
+\r
+\r
+       q0 = f32_to_double(f_q0);\r
+       q1 = f32_to_double(f_q1);\r
+       q2 = f32_to_double(f_q2);\r
+       q3 = f32_to_double(f_q3);\r
+\r
+       //printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", q0, q1, q2);\r
+\r
+}\r
+\r
+\r
+//---------------------------------------------------------------------------------------------------\r
+// Fast inverse square-root\r
+// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root\r
+/*\r
+float invSqrt(float x) {\r
+       float halfx = 0.5f * x;\r
+       float y = x;\r
+       long i = *(long*)&y;\r
+       i = 0x5f3759df - (i>>1);\r
+       y = *(float*)&i;\r
+       y = y * (1.5f - (halfx * y * y));\r
+       return y;\r
+}\r
+*/\r
+float invSqrt(float x) {\r
+       return 1.0f / sqrtf(x);\r
+}\r
+//====================================================================================================\r
+// END OF CODE\r
+//====================================================================================================\r

diff --git a/MadgwickAHRS.h b/MadgwickAHRS.h
new file mode 100644 (file)
index 0000000..7e42291
--- /dev/null
+++ b/MadgwickAHRS.h
@@ -0,0 +1,33 @@
+//=====================================================================================================\r
+// MadgwickAHRS.h\r
+//=====================================================================================================\r
+//\r
+// Implementation of Madgwick's IMU and AHRS algorithms.\r
+// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms\r
+//\r
+// Date                        Author          Notes\r
+// 29/09/2011  SOH Madgwick    Initial release\r
+// 02/10/2011  SOH Madgwick    Optimised for reduced CPU load\r
+//\r
+//=====================================================================================================\r
+#ifndef MadgwickAHRS_h\r
+#define MadgwickAHRS_h\r
+\r
+//----------------------------------------------------------------------------------------------------\r
+// Variable declaration\r
+\r
+extern volatile float beta;                            // algorithm gain\r
+extern volatile float q0, q1, q2, q3;  // quaternion of sensor frame relative to auxiliary frame\r
+\r
+//---------------------------------------------------------------------------------------------------\r
+// Function declarations\r
+\r
+void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);\r
+void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az);\r
+void Mad_f32_init();\r
+void MadgwickAHRSupdateIMU_f32(float gx, float gy, float gz, float ax, float ay, float az);\r
+\r
+#endif\r
+//=====================================================================================================\r
+// End of file\r
+//=====================================================================================================\r

diff --git a/Makefile b/Makefile
new file mode 100644 (file)
index 0000000..8ba697b
--- /dev/null
+++ b/Makefile
@@ -0,0 +1,22 @@
+TARGET = imu
+
+# repertoire des modules
+AVERSIVE_DIR =../..# VALUE, absolute or relative path : example ../.. #
+
+# List C source files here. (C dependencies are automatically generated.)
+# SRC = $(TARGET).c bma150.c itg3200.c ak8500.c i2c_helper.c vector.c matrix.c MadgwickAHRS.c mpu6050.c
+SRC = $(TARGET).c  i2c_helper.c  mpu6050.c MadgwickAHRS.c i2cm_sw.c
+
+# List Assembler source files here.
+# Make them always end in a capital .S.  Files ending in a lowercase .s
+# will not be considered source files but generated files (assembler
+# output from the compiler), and will be deleted upon "make clean"!
+# Even though the DOS/Win* filesystem matches both .s and .S the same,
+# it will preserve the spelling of the filenames, and gcc itself does
+# care about how the name is spelled on its command-line.
+ASRC = 
+
+########################################
+
+-include .aversive_conf
+include $(AVERSIVE_DIR)/mk/aversive_project.mk

diff --git a/error_config.h b/error_config.h
new file mode 100644 (file)
index 0000000..95c3076
--- /dev/null
+++ b/error_config.h
@@ -0,0 +1,31 @@
+/*  
+ *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: error_config.h,v 1.3.4.2 2007-12-06 08:58:00 zer0 Exp $
+ *
+ */
+
+#ifndef _ERROR_CONFIG_
+#define _ERROR_CONFIG_
+
+/** enable the dump of the comment */
+#define ERROR_DUMP_TEXTLOG 
+
+/** enable the dump of filename and line number */
+#define ERROR_DUMP_FILE_LINE
+
+#endif

diff --git a/i2c_config.h b/i2c_config.h
new file mode 100644 (file)
index 0000000..48d84e5
--- /dev/null
+++ b/i2c_config.h
@@ -0,0 +1,30 @@
+/*  
+ *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: i2c_config.h,v 1.2 2009-03-05 23:01:32 zer0 Exp $
+ *
+ */
+
+
+#define I2C_BITRATE 1 // divider dor i2c baudrate, see TWBR in doc
+#define I2C_PRESCALER 3 // prescaler config, rate = 2^(n*2)
+
+/* Size of transmission buffer */
+#define I2C_SEND_BUFFER_SIZE 32
+
+/* Size of reception buffer */
+#define I2C_RECV_BUFFER_SIZE 32

diff --git a/i2c_helper.c b/i2c_helper.c
new file mode 100644 (file)
index 0000000..de929ae
--- /dev/null
+++ b/i2c_helper.c
@@ -0,0 +1,57 @@
+#include <stdio.h>
+#include <string.h>
+
+#include <scheduler.h>
+#include <timer.h>
+
+#include <aversive/wait.h>
+#include <uart.h>
+#include <i2c.h>
+
+
+
+
+uint8_t read_reg(uint8_t address_dev, uint8_t address_reg, uint8_t * value)
+{
+       uint8_t err = 0;
+       err = i2c_send(address_dev, &address_reg, 1, I2C_CTRL_SYNC);
+       if (err) {
+               printf("i2c error\r\n");
+               return err;
+       }
+       err = i2c_recv(address_dev, 1, I2C_CTRL_SYNC);
+       if (err) {
+               printf("i2c error\r\n");
+               return err;
+       }
+       err = i2c_get_recv_buffer(value, 1);
+       if (err != 1) {
+               printf("i2c error\r\n");
+               return 0xff;
+       }
+       return 0;
+
+}
+
+
+uint8_t read_reg_len(uint8_t address_dev, uint8_t address_reg, uint8_t * values, uint8_t len)
+{
+       uint8_t err = 0;
+       err = i2c_send(address_dev, &address_reg, 1, I2C_CTRL_SYNC);
+       if (err) {
+               printf("i2c error\r\n");
+               return err;
+       }
+       err = i2c_recv(address_dev, len, I2C_CTRL_SYNC);
+       if (err) {
+               printf("i2c error\r\n");
+               return err;
+       }
+       err = i2c_get_recv_buffer(values, len);
+       if (err != len) {
+               printf("i2c error\r\n");
+               return 0xDD;
+       }
+       return 0;
+
+}

diff --git a/i2c_helper.h b/i2c_helper.h
new file mode 100644 (file)
index 0000000..57f16df
--- /dev/null
+++ b/i2c_helper.h
@@ -0,0 +1,8 @@
+
+#ifndef _I2C_HELPER_H_
+#define _I2C_HELPER_H_
+
+uint8_t read_reg(uint8_t address_dev, uint8_t address_reg, uint8_t * value);
+uint8_t read_reg_len(uint8_t address_dev, uint8_t address_reg, uint8_t * value, uint8_t len);
+
+#endif // _I2C_HELPER_H_

diff --git a/i2cm_sw.c b/i2cm_sw.c
new file mode 100644 (file)
index 0000000..3a264d5
--- /dev/null
+++ b/i2cm_sw.c
@@ -0,0 +1,338 @@
+/*
+ *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id$
+ *
+ */
+
+
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <aversive.h>
+
+#include <aversive/wait.h>
+
+
+#include <util/delay.h>
+
+
+#include "i2cm_sw.h"
+
+
+
+
+volatile i2cm_state g_i2cm_NUM_state = NOT_INIT;
+volatile uint8_t g_i2cm_NUM_byte;
+
+void (*g_i2cm_NUM_event)(i2cm_state state);
+
+/*
+  Mini Arduino
+  I2C:
+  A4 = PC4/SDA
+  A5 = PC5/SCL
+*/
+
+#define I2CM_NUM_SCL_PORT  PORTC
+#define I2CM_NUM_SCL_BIT   5
+
+#define  I2CM_NUM_SDA_PORT PORTC
+#define  I2CM_NUM_SDA_BIT  4
+
+#define I2CM_NUM_SCL   I2CM_NUM_SCL_PORT, I2CM_NUM_SCL_BIT
+#define I2CM_NUM_SDA   I2CM_NUM_SDA_PORT, I2CM_NUM_SDA_BIT
+
+
+
+
+void i2cm_NUM_init(void)
+{
+       /*
+         - port SCL/SDA is set to 0
+         - i2c is then driven using DDR 0 (Hi-Z) or 1 (For Low)
+        */
+
+       // SCL high
+       cbi(DDR(I2CM_NUM_SCL_PORT),I2CM_NUM_SCL_BIT);
+       I2C_HIGH(I2CM_NUM_SCL_PORT, I2CM_NUM_SCL_BIT);
+
+       // SDA high
+       cbi(DDR(I2CM_NUM_SDA_PORT),I2CM_NUM_SDA_BIT);
+       I2C_HIGH(I2CM_NUM_SDA_PORT, I2CM_NUM_SDA_BIT);
+       I2CM_NUM_DELAY();
+
+       g_i2cm_NUM_state = I2CM_READY;
+}
+
+
+void i2cm_NUM_manage(void)
+{
+       if ( (g_i2cm_NUM_event  != NULL) && (g_i2cm_NUM_state  != I2CM_READY) ) {
+               g_i2cm_NUM_event( g_i2cm_NUM_state );
+               // reset notification
+               if (g_i2cm_NUM_state != I2CM_BUSY)
+                       g_i2cm_NUM_state = I2CM_READY;
+       }
+}
+
+uint8_t i2cm_NUM_get_state(void)
+{
+       return g_i2cm_NUM_state;
+}
+
+
+uint8_t i2cm_NUM_get_received_byte(void)
+{
+       g_i2cm_NUM_state = I2CM_READY;
+       return g_i2cm_NUM_byte;
+}
+
+void i2cm_NUM_register_event(void (*func)(i2cm_state state))
+{
+       uint8_t flags;
+       IRQ_LOCK(flags);
+       g_i2cm_NUM_event = func;
+       IRQ_UNLOCK(flags);
+}
+
+
+uint8_t i2cm_NUM_send_byte(uint8_t byte)
+{
+       uint8_t mask;
+       uint8_t err = 0;
+
+
+       g_i2cm_NUM_state = I2CM_BUSY;
+
+       // SCL should already be low
+       //  I2C_LOW(I2CM_NUM_SCL_PORT, I2CM_NUM_SCL_BIT);
+       //  I2CM_NUM_DELAY();
+
+       mask = 0x80;
+       while (mask) {
+
+
+               // data out
+               if (mask & byte)
+                       I2C_HIGH(I2CM_NUM_SDA_PORT, I2CM_NUM_SDA_BIT);
+               else
+                       I2C_LOW(I2CM_NUM_SDA_PORT, I2CM_NUM_SDA_BIT);
+               I2CM_NUM_DELAY();
+               mask >>=1;
+
+               // delay needed ?
+               //I2CM_NUM_DELAY();
+
+               // clock High
+               I2C_HIGH(I2CM_NUM_SCL_PORT, I2CM_NUM_SCL_BIT);
+               I2CM_NUM_DELAY();
+               while ( bit_is_clear(PIN(I2CM_NUM_SCL_PORT), I2CM_NUM_SCL_BIT) );// slave handshake
+
+               // clock low
+               I2C_LOW(I2CM_NUM_SCL_PORT, I2CM_NUM_SCL_BIT);
+
+       }
+
+       /* receive ack */
+
+       // release SDA
+       I2C_HIGH(I2CM_NUM_SDA_PORT, I2CM_NUM_SDA_BIT);
+       I2CM_NUM_BIT_DELAY();
+
+       // clock HIGH
+       I2C_HIGH(I2CM_NUM_SCL_PORT, I2CM_NUM_SCL_BIT);
+       I2CM_NUM_DELAY();
+       while ( bit_is_clear(PIN(I2CM_NUM_SCL_PORT), I2CM_NUM_SCL_BIT) );// slave handshake
+
+       // receive ACK
+       if (bit_is_set(I2CM_NUM_SDA_PORT, I2CM_NUM_SDA_BIT))
+               err = I2CM_SENT_NO_ACK;
+
+       // clock low
+       I2C_LOW(I2CM_NUM_SCL_PORT, I2CM_NUM_SCL_BIT);
+       I2CM_NUM_DELAY();
+
+       return err;
+}
+
+
+uint8_t i2cm_NUM_send_start(uint8_t sla_w)
+{
+       uint8_t err;
+       uint16_t i;
+       g_i2cm_NUM_state = I2CM_BUSY;
+
+       I2C_HIGH(I2CM_NUM_SCL_PORT, I2CM_NUM_SCL_BIT);
+       I2C_HIGH(I2CM_NUM_SDA_PORT, I2CM_NUM_SDA_BIT);
+
+       I2CM_NUM_DELAY();
+
+       /* wait for bus realese */
+       for (i=0;i<0x8000; i++){
+               if( bit_is_set(PIN(I2CM_NUM_SCL_PORT), I2CM_NUM_SCL_BIT) && \
+                   bit_is_set(PIN(I2CM_NUM_SDA_PORT), I2CM_NUM_SDA_BIT) )
+                       break;
+       }
+
+       //      while ( bit_is_clear(PIN(I2CM_NUM_SCL_PORT), I2CM_NUM_SCL_BIT) );// slave handshake
+
+       // start condition
+       I2C_LOW(I2CM_NUM_SDA_PORT, I2CM_NUM_SDA_BIT);
+       I2CM_NUM_DELAY();
+       I2C_LOW(I2CM_NUM_SCL_PORT, I2CM_NUM_SCL_BIT);
+       I2CM_NUM_DELAY();
+
+       err = i2cm_NUM_send_byte(sla_w);
+       return err;
+}
+
+
+
+uint8_t i2cm_NUM_send_stop(void)
+{
+       g_i2cm_NUM_state = I2CM_BUSY;
+
+       // data down
+       I2C_LOW(I2CM_NUM_SCL_PORT, I2CM_NUM_SCL_BIT);
+       I2C_LOW(I2CM_NUM_SDA_PORT, I2CM_NUM_SDA_BIT);
+       I2CM_NUM_DELAY();
+
+       // stop condition
+       I2C_HIGH(I2CM_NUM_SCL_PORT, I2CM_NUM_SCL_BIT);
+       I2CM_NUM_DELAY();
+       I2C_HIGH(I2CM_NUM_SDA_PORT, I2CM_NUM_SDA_BIT);
+       I2CM_NUM_DELAY();
+
+       g_i2cm_NUM_state = I2CM_SENT_STOP;
+       return 0;
+}
+
+
+uint8_t i2cm_NUM_receive_byte(uint8_t last)
+{
+       uint8_t mask,data;
+
+       g_i2cm_NUM_state = I2CM_BUSY;
+
+       I2C_HIGH(I2CM_NUM_SDA_PORT, I2CM_NUM_SDA_BIT);
+
+
+       data = 0;
+       mask = 0x80;
+       while (mask) {
+
+               // clock High
+               I2C_HIGH(I2CM_NUM_SCL_PORT, I2CM_NUM_SCL_BIT);
+               I2CM_NUM_DELAY();
+               while ( bit_is_clear(PIN(I2CM_NUM_SCL_PORT), I2CM_NUM_SCL_BIT) );// slave handshake
+
+               if (bit_is_set(PIN(I2CM_NUM_SDA_PORT),I2CM_NUM_SDA_BIT))
+                       data |= mask;
+
+               // clock low
+               I2C_LOW(I2CM_NUM_SCL_PORT, I2CM_NUM_SCL_BIT);
+               I2CM_NUM_DELAY();
+
+               mask >>=1;
+       }
+
+       if (!last){
+               /* send ack */
+               I2C_LOW(I2CM_NUM_SDA_PORT, I2CM_NUM_SDA_BIT);
+               I2CM_NUM_BIT_DELAY();
+       }
+
+       // clock HIGH
+       I2C_HIGH(I2CM_NUM_SCL_PORT, I2CM_NUM_SCL_BIT);
+       I2CM_NUM_DELAY();
+       while ( bit_is_clear(PIN(I2CM_NUM_SCL_PORT), I2CM_NUM_SCL_BIT) );// slave handshake
+
+       // clock low
+       I2C_LOW(I2CM_NUM_SCL_PORT, I2CM_NUM_SCL_BIT);
+       I2CM_NUM_DELAY();
+
+       if (!last) {
+               // release DATA
+               I2C_HIGH(I2CM_NUM_SDA_PORT, I2CM_NUM_SDA_BIT);
+               I2CM_NUM_BIT_DELAY();
+       }
+
+       g_i2cm_NUM_byte  = data;
+       g_i2cm_NUM_state = I2CM_RECEIVED_BYTE;
+
+       I2C_HIGH(I2CM_NUM_SCL_PORT, I2CM_NUM_SCL_BIT);
+
+       return 0;
+}
+
+
+
+uint8_t i2c_send(uint8_t addr, uint8_t* data, uint8_t len, uint8_t dummy)
+{
+       uint8_t i;
+       uint8_t err = 0;
+
+       err = i2cm_NUM_send_start((addr<<1) | 0);
+       if (err)
+               return err;
+
+       for (i=0; i<len; i++) {
+               err = i2cm_NUM_send_byte(data[i]);
+               if (err)
+                       break;
+       }
+
+       i2cm_NUM_send_stop();
+       return err;
+}
+
+
+uint8_t i2c_buf[0x20];
+
+uint8_t i2c_recv(uint8_t addr, uint8_t len, uint8_t dummy)
+{
+       uint8_t i;
+       uint8_t err = 0;
+
+       err = i2cm_NUM_send_start((addr<<1) | 1);
+       if (err)
+               return err;
+
+       I2CM_NUM_DELAY();
+
+       for (i=0; i<len; i++){
+               err =i2cm_NUM_receive_byte(i == len-1);
+               i2c_buf[i] = g_i2cm_NUM_byte;
+               if (err)
+                       break;
+       }
+       i2cm_NUM_send_stop();
+       return err;
+}
+
+
+uint8_t i2c_get_recv_buffer(uint8_t* buf, uint8_t len)
+{
+       uint8_t i;
+       for (i=0; i<len; i++)
+               buf[i] = i2c_buf[i];
+       return len;
+}
+
+

diff --git a/i2cm_sw.h b/i2cm_sw.h
new file mode 100644 (file)
index 0000000..48255b6
--- /dev/null
+++ b/i2cm_sw.h
@@ -0,0 +1,54 @@
+#ifndef I2CM_SW_H
+#define I2CM_SW_H
+
+/*
+#define BIT(x) (1 << (x))
+#define SETBITS(x,y) ((x) |= (y))
+#define CLEARBITS(x,y) ((x) &= (~(y)))
+#define SETBIT(x,y) SETBITS((x), (BIT((y))))
+#define CLEARBIT(x,y) CLEARBITS((x), (BIT((y))))
+#define BITSET(x,y) ((x) & (BIT(y)))
+#define BITCLEAR(x,y) !BITSET((x), (y))
+#define BITSSET(x,y) (((x) & (y)) == (y))
+#define BITSCLEAR(x,y) (((x) & (y)) == 0)
+#define BITVAL(x,y) (((x)>>(y)) & 1)
+*/
+
+#define I2C_LOW(port, bit) sbi(DDR(port),bit)
+#define I2C_HIGH(port, bit) cbi(DDR(port),bit)
+
+
+typedef uint8_t i2cm_state;
+
+#define NOT_INIT   0
+#define I2CM_READY 1
+#define I2CM_BUSY  2
+#define I2CM_SENT  3
+#define I2CM_SENT_NO_ACK 4
+#define I2CM_SENT_START  5
+#define I2CM_SENT_START_NO_ACK 6
+#define I2CM_SENT_STOP 7
+#define I2CM_RECEIVED_BYTE 8
+
+#define I2CM_NUM_DELAY()  _delay_loop_2(1)
+#define I2CM_NUM_BIT_DELAY() _delay_loop_2(1)
+
+void i2cm_NUM_init(void);
+void i2cm_NUM_manage(void);
+uint8_t i2cm_NUM_get_state(void);
+uint8_t i2cm_NUM_get_received_byte(void);
+void i2cm_NUM_register_event(void (*func)(i2cm_state state));
+uint8_t i2cm_NUM_send_byte(uint8_t byte);
+uint8_t i2cm_NUM_send_start(uint8_t sla_w);
+uint8_t i2cm_NUM_send_stop(void);
+uint8_t i2cm_NUM_receive_byte(uint8_t last);
+
+#define I2C_CTRL_SYNC 0
+
+
+#define I2C_ERR_SEND_START 1
+#define I2C_ERR_SEND_BYTE 2
+#define I2C_ERR_RECV_BYTE 3
+
+
+#endif //I2CM_SW_H

diff --git a/imu.c b/imu.c
new file mode 100644 (file)
index 0000000..8486960
--- /dev/null
+++ b/imu.c
@@ -0,0 +1,398 @@
+#include <stdio.h>
+#include <string.h>
+
+#include <scheduler.h>
+#include <timer.h>
+
+#include <aversive/wait.h>
+#include <uart.h>
+
+
+#include <i2c.h>
+//#include "i2cm_sw.h"
+
+
+#include "math.h"
+
+#include "i2c_helper.h"
+//#include "bma150.h"
+//#include "itg3200.h"
+//#include "ak8500.h"
+
+#include "mpu6050.h"
+
+#include "vector.h"
+#include "matrix.h"
+
+#include "MadgwickAHRS.h"
+
+
+#define LED_PRIO           170
+#define GYRO_PRIO           100
+
+int internal_mag_x;
+int internal_mag_y;
+int internal_mag_z;
+
+int mag_x;
+int mag_y;
+int mag_z;
+
+
+void i2c_recvevent(uint8_t * buf, int8_t size)
+{
+}
+
+void i2c_sendevent(int8_t size)
+{
+}
+
+
+static void main_timer_interrupt(void)
+{
+       static uint8_t cpt = 0;
+       cpt++;
+       sei();
+       if ((cpt & 0x3) == 0)
+               scheduler_interrupt();
+}
+
+
+#define LED1_TOGGLE()   PORTB ^= 0x20;
+
+
+uint16_t counter;
+
+void do_led_blink(void *dummy)
+{
+#if 1 /* simple blink */
+       LED1_TOGGLE();
+#endif
+       //counter ++;
+       printf("\r\n%"PRId16"\r\n", counter);
+       counter = 0;
+
+}
+
+
+
+
+/* for i2c */
+//uint8_t command_buf[I2C_SEND_BUFFER_SIZE];
+
+
+float Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector
+float Gyro_Vector[3]= {0,0,0};//Store the gyros rutn rate in a vector
+float Magnet_Vector[3]= {0,0,0}; //Store the acceleration in a vector
+
+float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
+float Omega_P[3]= {0,0,0};//Omega Proportional correction
+float Omega_I[3]= {0,0,0};//Omega Integrator
+float Omega[3]= {0,0,0};
+
+float Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
+
+float DCM_Matrix[3][3]= {
+  {
+    1,0,0  }
+  ,{
+    0,1,0  }
+  ,{
+    0,0,1  }
+};
+
+float Temporary_Matrix[3][3]={
+  {
+    0,0,0  }
+  ,{
+    0,0,0  }
+  ,{
+    0,0,0  }
+};
+
+float errorRollPitch[3]= {0,0,0};
+float errorYaw[3]= {0,0,0};
+float errorCourse=180;
+float COGX=0; //Course overground X axis
+float COGY=1; //Course overground Y axis
+
+
+
+#define OUTPUTMODE 2
+
+#define ToRad(x) (x*0.01745329252)  // *pi/180
+#define ToDeg(x) (x*57.2957795131)  // *180/pi
+
+/*
+#define Gyro_Gain_X 0.92 //X axis Gyro gain
+#define Gyro_Gain_Y 0.92 //Y axis Gyro gain
+#define Gyro_Gain_Z 0.94 //Z axis Gyro gain
+*/
+#define Gyro_Gain_X (1.) //X axis Gyro gain
+#define Gyro_Gain_Y (1.) //Y axis Gyro gain
+#define Gyro_Gain_Z (1.) //Z axis Gyro gain
+
+#define Gyro_Scaled_X(x) ((x)*ToRad(Gyro_Gain_X)) //Return the scaled ADC raw data of the gyro in radians for second
+#define Gyro_Scaled_Y(x) ((x)*ToRad(Gyro_Gain_Y)) //Return the scaled ADC raw data of the gyro in radians for second
+#define Gyro_Scaled_Z(x) ((x)*ToRad(Gyro_Gain_Z)) //Return the scaled ADC raw data of the gyro in radians for second
+
+float G_Dt=0.02;    // Integration time (DCM algorithm)
+
+#define GRAVITY 1.01 //this equivalent to 1G in the raw data coming from the accelerometer
+#define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square
+
+
+#define Kp_ROLLPITCH (1.515/GRAVITY)
+#define Ki_ROLLPITCH (0.00101/GRAVITY)
+
+#define Kp_YAW 1.2
+//#define Kp_YAW 2.5      //High yaw drift correction gain - use with caution!
+#define Ki_YAW 0.00005
+
+#define MAGNETIC_DECLINATION 4.0
+
+#define constrain(v, a, b) (((v)<(a))?(a):((v)>(b)?(b):(v)))
+
+
+int mag_offset[3];
+float Heading;
+float Heading_X;
+float Heading_Y;
+
+
+// Euler angles
+float roll;
+float pitch;
+float yaw;
+
+int SENSOR_SIGN[]= {
+       1,1,1,   // GYROX, GYROY, GYROZ,
+       1,1,1,   // ACCELX, ACCELY, ACCELZ,
+       1,1,1    // MAGX, MAGY, MAGZ,
+};
+
+#if 0
+
+float read_adc(uint8_t index)
+{
+       int16_t value;
+
+       switch(index) {
+               case 0:
+                       itg3200_read_axe(0, &value);
+                       return (float) (SENSOR_SIGN[index] * value);
+               case 1:
+                       itg3200_read_axe(1, &value);
+                       return (float) (SENSOR_SIGN[index] * value);
+               case 2:
+                       itg3200_read_axe(2, &value);
+                       return (float) (SENSOR_SIGN[index] * value);
+               case 3:
+                       bma150_read_axe(0, &value);
+                       return (float) (SENSOR_SIGN[index] * bma15_axe2g(value));
+               case 4:
+                       bma150_read_axe(1, &value);
+                       return (float) (SENSOR_SIGN[index] * bma15_axe2g(value));
+               case 5:
+                       bma150_read_axe(2, &value);
+                       return (float) (SENSOR_SIGN[index] * bma15_axe2g(value));
+       }
+       return 0.0;
+}
+
+
+uint8_t measure_time = 0;
+void read_sensors(void)
+{
+       uint8_t flags;
+       uint8_t err;
+       uint16_t axes[3];
+
+       /* read mag */
+       measure_time ++;//= (measure_time +1)%3;
+       if (measure_time%2 == 0) {
+               err = ak8500_start_measure();
+               if (err) {
+                       printf("mag start err %X\r\n", err);
+                       measure_time = 0;
+               }
+       }
+       else if (measure_time%2 == 1) {
+               err = ak8500_read_all_axes(&axes);
+               if (err == 0) {
+                       /*
+                       Magnet_Vector[0] = (float)SENSOR_SIGN[6] * (float)axes[0];
+                       Magnet_Vector[1] = (float)SENSOR_SIGN[7] * (float)axes[1];
+                       Magnet_Vector[2] = (float)SENSOR_SIGN[8] * (float)axes[2];
+                       */
+                       /*
+                       */
+                       mag_x = SENSOR_SIGN[6] * axes[0];
+                       mag_y = SENSOR_SIGN[7] * axes[1];
+                       mag_z = SENSOR_SIGN[8] * axes[2];
+                       Magnet_Vector[0] = mag_x;
+                       Magnet_Vector[1] = mag_y;
+                       Magnet_Vector[2] = mag_z;
+
+               }
+               else {
+                       printf("mag read err %X\r\n", err);
+               }
+       }
+       /*
+       printf("%d %d %d\r\n",
+              mag_x,
+              mag_y,
+              mag_z
+              );
+       */
+       /*
+       printf("%f %f %f\r\n",
+              Magnet_Vector[0],
+              Magnet_Vector[1],
+              Magnet_Vector[2]);
+       */
+       /*
+       Gyro_Vector[0]=Gyro_Scaled_X(read_adc(0)); //gyro x roll
+       Gyro_Vector[1]=Gyro_Scaled_Y(read_adc(1)); //gyro y pitch
+       Gyro_Vector[2]=Gyro_Scaled_Z(read_adc(2)); //gyro Z yaw
+       */
+       Gyro_Vector[0]=ToRad(read_adc(0)); //gyro x roll
+       Gyro_Vector[1]=ToRad(read_adc(1)); //gyro y pitch
+       Gyro_Vector[2]=ToRad(read_adc(2)); //gyro Z yaw
+
+       Accel_Vector[0]=9.81 * read_adc(3); // acc x
+       Accel_Vector[1]=9.81 * read_adc(4); // acc y
+       Accel_Vector[2]=9.81 * read_adc(5); // acc z
+
+}
+
+#endif
+
+
+void quaternion2euler(void)
+{
+       /*
+       roll = atan2f(2. * (q0*q1 + q2*q3), 1. - 2. * (q1*q1 + q2*q2));
+       pitch = asinf(2 * (q0*q2 - q3*q1));
+       yaw =  atan2f(2. * (q0*q3 + q1*q2), 1. - 2. * (q2*q2 + q3*q3));
+       */
+       roll = atan2f(2.0f * (q0 * q1 + q2 * q3), q0*q0 - q1*q1 - q2*q2 + q3*q3);
+       pitch = -asinf(2.0f * (q1 * q3 - q0 * q2));
+       yaw = atan2f(2.0f * (q1 * q2 + q0 * q3), q0*q0 + q1*q1 - q2*q2 - q3*q3);
+}
+
+#define swap_u16(a) (((a>>8)&0xff) | (((a&0xFF)<<8)))
+int main(void)
+{
+       int16_t temp;
+       //uint8_t err;
+       uint16_t * ptr;
+       uint8_t a;
+       int i;
+
+
+       int16_t mpu6050_axes[10];
+
+       /* UART */
+       uart_init();
+
+       fdevopen(uart0_dev_send, uart0_dev_recv);
+
+#if 0
+       i2c_init(I2C_MODE_MASTER,  1/* I2C_MAIN_ADDR */);
+       i2c_register_recv_event(i2c_recvevent);
+       i2c_register_send_event(i2c_sendevent);
+#else
+
+       i2cm_NUM_init();
+#endif
+
+
+
+       // LED output
+       DDRB |= 0x20;
+
+
+       /* TIMER */
+       timer_init();
+       timer0_register_OV_intr(main_timer_interrupt);
+
+
+       /* SCHEDULER */
+       scheduler_init();
+
+
+       sei();
+
+
+       /*
+       bma150_init();
+       itg3200_init();
+       ak8975_read_sensitivity();
+       */
+       scheduler_add_periodical_event_priority(do_led_blink, NULL,
+                                               1000000L / SCHEDULER_UNIT,
+                                               LED_PRIO);
+       /*
+       scheduler_add_periodical_event_priority(update_gyro, NULL,
+                                               1000000L / SCHEDULER_UNIT,
+                                               GYRO_PRIO);
+       */
+       mpu6050_init();
+
+       Mad_f32_init();
+
+       while (1) {
+               counter ++;
+               mpu6050_read_all_axes(mpu6050_axes);
+               /*
+               for (i=0;i<7; i++){
+                       printf("%"PRId16" ", mpu6050_axes[i]);
+               }
+               printf("\r\n");
+               */
+               /*
+               printf("%3.3f %3.3f %3.3f\r\n",
+                      mpu6050_gx,
+                      mpu6050_gy,
+                      mpu6050_gz);
+               continue;
+               */
+
+               /*
+               MadgwickAHRSupdateIMU(mpu6050_gx,
+                                     mpu6050_gy,
+                                     mpu6050_gz,
+                                     mpu6050_ax,
+                                     mpu6050_ay,
+                                     mpu6050_az);
+               */
+
+               MadgwickAHRSupdate(mpu6050_gx,
+                                  mpu6050_gy,
+                                  mpu6050_gz,
+                                  mpu6050_ax,
+                                  mpu6050_ay,
+                                  mpu6050_az,
+                                  mpu6050_mx,
+                                  mpu6050_my,
+                                  mpu6050_mz
+                                  );
+
+               quaternion2euler();
+
+               /*
+               mpu6050_axes[7] = swap_u16(mpu6050_axes[7]);
+               mpu6050_axes[8] = swap_u16(mpu6050_axes[8]);
+               mpu6050_axes[9] = swap_u16(mpu6050_axes[9]);
+               */
+               printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", roll, pitch, yaw);
+               //printf("%+.4d %+.4d %+.4d\r\n", mpu6050_axes[7], mpu6050_axes[8], mpu6050_axes[9]);
+               //printf("%+3.3f\r\n", mpu6050_temp);//, mpu6050_axes[9]);
+               //printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", mpu6050_mx, mpu6050_my, mpu6050_mz );
+
+       }
+
+       return 0;
+}

diff --git a/matrix.c b/matrix.c
new file mode 100644 (file)
index 0000000..898aa82
--- /dev/null
+++ b/matrix.c
@@ -0,0 +1,16 @@
+//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!).
+void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
+{
+       float op[3];
+       for(int x=0; x<3; x++) {
+               for(int y=0; y<3; y++) {
+                       for(int w=0; w<3; w++) {
+                               op[w]=a[x][w]*b[w][y];
+                       }
+                       mat[x][y]=0;
+                       mat[x][y]=op[0]+op[1]+op[2];
+
+                       float test=mat[x][y];
+               }
+       }
+}

diff --git a/matrix.h b/matrix.h
new file mode 100644 (file)
index 0000000..6258f1c
--- /dev/null
+++ b/matrix.h
@@ -0,0 +1,6 @@
+#ifndef _MATRIX_H_
+#define _MATRIX_H_
+
+// Multiply two 3x3 matrixs.
+void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]);
+#endif// _MATRIX_H_

diff --git a/mpu6050.c b/mpu6050.c
new file mode 100644 (file)
index 0000000..4f1ab81
--- /dev/null
+++ b/mpu6050.c
@@ -0,0 +1,337 @@
+#include <stdio.h>
+#include <string.h>
+
+#include <scheduler.h>
+#include <timer.h>
+
+#include <aversive/wait.h>
+#include <uart.h>
+#include <i2c.h>
+
+#include "i2c_helper.h"
+
+#include "mpu6050.h"
+
+#define ToRad(x) ((x)*0.01745329252)  // *pi/180
+
+// fs_sel = 1
+// 500°/s
+#define gyro_x_resolution 66.5
+#define gyro_y_resolution 66.5
+#define gyro_z_resolution 66.5
+
+// fs_sel = 3
+// 2000°/s
+#define gyro_x_resolution 16.4
+#define gyro_y_resolution 16.4
+#define gyro_z_resolution 16.4
+
+#define accel_x_resolution 8192.0
+#define accel_y_resolution 8192.0
+#define accel_z_resolution 8192.0
+
+
+
+
+#define MPU6050_OFF_MSB 1
+#define MPU6050_OFF_LSB 0
+
+
+#define MPU6050_ADDRESS 0x68
+#define MPU6050_MAGNETO_ADDRESS 0x0C
+
+#define SWAP_16(a) ((( (a) & 0xff)<<8) | (( (a) >> 8) & 0xff))
+
+float mpu6050_gx;
+float mpu6050_gy;
+float mpu6050_gz;
+
+float mpu6050_ax;
+float mpu6050_ay;
+float mpu6050_az;
+
+float mpu6050_mx;
+float mpu6050_my;
+float mpu6050_mz;
+
+float mpu6050_temp;
+
+int16_t drift_g[3] = {0, 0, 0};
+
+
+uint8_t mpu6050_read_gyro_raw(int16_t *values)
+{
+       uint8_t err;
+       uint8_t i;
+       err = read_reg_len(MPU6050_ADDRESS, 0x43, (uint8_t*)values, sizeof(int16_t)*3);
+       for (i=0;i<3; i++) {
+               values[i] = SWAP_16(values[i]);
+       }
+
+       return err;
+}
+
+uint8_t mpu6050_read_all_axes(int16_t *values)
+{
+       uint8_t err;
+       uint8_t i;
+       err = read_reg_len(MPU6050_ADDRESS, 0x3b, (uint8_t*)values, sizeof(int16_t)*10);
+
+       for (i=0;i<7; i++) {
+               values[i] = SWAP_16(values[i]);
+       }
+
+       mpu6050_ax = 9.81 * (float)values[0] / accel_x_resolution ;
+       mpu6050_ay = 9.81 * (float)values[1] / accel_y_resolution ;
+       mpu6050_az = 9.81 * (float)values[2] / accel_z_resolution ;
+
+       mpu6050_temp = (float)values[3]/340. + 36.5;
+
+       mpu6050_gx = ToRad((float)(values[4] - drift_g[0]) / gyro_x_resolution );
+       mpu6050_gy = ToRad((float)(values[5] - drift_g[1]) / gyro_y_resolution );
+       mpu6050_gz = ToRad((float)(values[6] - drift_g[2]) / gyro_z_resolution );
+
+       mpu6050_mx = (float) values[7] * 0.3;
+       mpu6050_my = (float) values[8] * 0.3;
+       mpu6050_mz = (float) values[9] * 0.3;
+
+
+       return err;
+}
+
+
+
+
+
+uint8_t send_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val)
+{
+       uint8_t err;
+       uint8_t buffer[2];
+
+       buffer[0] = reg;
+       buffer[1] = val;
+
+       err = i2c_send(address, (unsigned char*)buffer, 2, I2C_CTRL_SYNC);
+       if (err)
+               printf("error %.2X\r\n", err);
+       return err;
+}
+
+
+void mpu6050_compute_drift(void)
+{
+       uint16_t i;
+       int32_t s_gx, s_gy, s_gz;
+       int16_t g_values[3];
+
+       drift_g[0] = 0;
+       drift_g[1] = 0;
+       drift_g[2] = 0;
+
+       s_gx = s_gy = s_gz = 0;
+
+       for (i = 0; i < 0x100; i ++) {
+               mpu6050_read_gyro_raw(&g_values);
+               s_gx += g_values[0];
+               s_gy += g_values[1];
+               s_gz += g_values[2];
+       }
+       printf("%"PRId32" %"PRId32" %"PRId32" (%"PRId32") \r\n", s_gx, s_gy, s_gz, i);
+       s_gx /= i;
+       s_gy /= i;
+       s_gz /= i;
+
+       drift_g[0] = s_gx;
+       drift_g[1] = s_gy;
+       drift_g[2] = s_gz;
+       printf("gyro drift:\r\n");
+       printf("%d %d %d\r\n",
+              drift_g[0],
+              drift_g[1],
+              drift_g[2]);
+}
+
+
+uint8_t setup_mpu9150_magneto()
+{
+       //MPU9150_I2C_ADDRESS = 0x0C;      //change Adress to Compass
+
+       send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode
+       send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x0F); //SelfTest
+       send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode
+
+       //MPU9150_I2C_ADDRESS = 0x69;      //change Adress to MPU
+
+       send_mpu6050_cmd(MPU6050_ADDRESS, 0x24, 0x40); //Wait for Data at Slave0
+       send_mpu6050_cmd(MPU6050_ADDRESS, 0x25, 0x8C); //Set i2c address at slave0 at 0x0C
+       //send_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x02); //Set where reading at slave 0 starts
+       send_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x03); //Set where reading at slave 0 starts
+       //send_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x88); //set offset at start reading and enable
+       send_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x86); //set offset at start reading and enable
+       send_mpu6050_cmd(MPU6050_ADDRESS, 0x28, 0x0C); //set i2c address at slv1 at 0x0C
+       send_mpu6050_cmd(MPU6050_ADDRESS, 0x29, 0x0A); //Set where reading at slave 1 starts
+       send_mpu6050_cmd(MPU6050_ADDRESS, 0x2A, 0x81); //Enable at set length to 1
+       send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //overvride register
+       send_mpu6050_cmd(MPU6050_ADDRESS, 0x67, 0x03); //set delay rate
+       send_mpu6050_cmd(MPU6050_ADDRESS, 0x01, 0x80);
+
+       send_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x04); //set i2c slv4 delay
+       send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x00); //override register
+       send_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x00); //clear usr setting
+       send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //override register
+       send_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x20); //enable master i2c mode
+       send_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x13); //disable slv4
+
+}
+
+uint8_t mpu6050_init(void)
+{
+       uint8_t err;
+       uint8_t id = 0;
+       uint8_t config = 0;
+
+
+       while(1) {
+               err = read_reg(MPU6050_ADDRESS, 0x75, &id);
+               if (err)
+                       continue;
+               if (id == 0x68)
+                       break;
+       }
+
+
+
+       //Sets sample rate to 1000/1+1 = 500Hz
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x01);
+       //Disable FSync, 48Hz DLPF
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_CONFIG, 0x03);
+       //Disable gyro self tests, scale of 500 degrees/s
+       //send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00001000);
+
+       //Disable gyro self tests, scale of 2000 degrees/s
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00011000);
+
+       //Disable accel self tests, scale of +-4g, no DHPF
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0b00001000);
+       //Freefall threshold of <|0mg|
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00);
+       //Freefall duration limit of 0
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00);
+       //Motion threshold of >0mg
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00);
+       //Motion duration of >0s
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00);
+       //Zero motion threshold
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00);
+       //Zero motion duration threshold
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00);
+       //Disable sensor output to FIFO buffer
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_EN, 0x00);
+
+       //AUX I2C setup
+       //Sets AUX I2C to single master control, plus other config
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_CTRL, 0x00);
+       //Setup AUX I2C slaves
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_ADDR, 0x00);
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_REG, 0x00);
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_CTRL, 0x00);
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_ADDR, 0x00);
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_REG, 0x00);
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_CTRL, 0x00);
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_ADDR, 0x00);
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_REG, 0x00);
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_CTRL, 0x00);
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_ADDR, 0x00);
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_REG, 0x00);
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_CTRL, 0x00);
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_ADDR, 0x00);
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_REG, 0x00);
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DO, 0x00);
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_CTRL, 0x00);
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DI, 0x00);
+
+
+       //MPU6050_RA_I2C_MST_STATUS //Read-only
+       //Setup INT pin and AUX I2C pass through
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00);
+       //Enable data ready interrupt
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00);
+
+       //MPU6050_RA_DMP_INT_STATUS                //Read-only
+       //MPU6050_RA_INT_STATUS 3A                //Read-only
+       //MPU6050_RA_ACCEL_XOUT_H                 //Read-only
+       //MPU6050_RA_ACCEL_XOUT_L                 //Read-only
+       //MPU6050_RA_ACCEL_YOUT_H                 //Read-only
+       //MPU6050_RA_ACCEL_YOUT_L                 //Read-only
+       //MPU6050_RA_ACCEL_ZOUT_H                 //Read-only
+       //MPU6050_RA_ACCEL_ZOUT_L                 //Read-only
+       //MPU6050_RA_TEMP_OUT_H                 //Read-only
+       //MPU6050_RA_TEMP_OUT_L                 //Read-only
+       //MPU6050_RA_GYRO_XOUT_H                 //Read-only
+       //MPU6050_RA_GYRO_XOUT_L                 //Read-only
+       //MPU6050_RA_GYRO_YOUT_H                 //Read-only
+       //MPU6050_RA_GYRO_YOUT_L                 //Read-only
+       //MPU6050_RA_GYRO_ZOUT_H                 //Read-only
+       //MPU6050_RA_GYRO_ZOUT_L                 //Read-only
+       //MPU6050_RA_EXT_SENS_DATA_00         //Read-only
+       //MPU6050_RA_EXT_SENS_DATA_01         //Read-only
+       //MPU6050_RA_EXT_SENS_DATA_02         //Read-only
+       //MPU6050_RA_EXT_SENS_DATA_03         //Read-only
+       //MPU6050_RA_EXT_SENS_DATA_04         //Read-only
+       //MPU6050_RA_EXT_SENS_DATA_05         //Read-only
+       //MPU6050_RA_EXT_SENS_DATA_06         //Read-only
+       //MPU6050_RA_EXT_SENS_DATA_07         //Read-only
+       //MPU6050_RA_EXT_SENS_DATA_08         //Read-only
+       //MPU6050_RA_EXT_SENS_DATA_09         //Read-only
+       //MPU6050_RA_EXT_SENS_DATA_10         //Read-only
+       //MPU6050_RA_EXT_SENS_DATA_11         //Read-only
+       //MPU6050_RA_EXT_SENS_DATA_12         //Read-only
+       //MPU6050_RA_EXT_SENS_DATA_13         //Read-only
+       //MPU6050_RA_EXT_SENS_DATA_14         //Read-only
+       //MPU6050_RA_EXT_SENS_DATA_15         //Read-only
+       //MPU6050_RA_EXT_SENS_DATA_16         //Read-only
+       //MPU6050_RA_EXT_SENS_DATA_17         //Read-only
+       //MPU6050_RA_EXT_SENS_DATA_18         //Read-only
+       //MPU6050_RA_EXT_SENS_DATA_19         //Read-only
+       //MPU6050_RA_EXT_SENS_DATA_20         //Read-only
+       //MPU6050_RA_EXT_SENS_DATA_21         //Read-only
+       //MPU6050_RA_EXT_SENS_DATA_22         //Read-only
+       //MPU6050_RA_EXT_SENS_DATA_23         //Read-only
+       //MPU6050_RA_MOT_DETECT_STATUS         //Read-only
+
+       //Slave out, dont care
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_DO, 0x00);
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_DO, 0x00);
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_DO, 0x00);
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_DO, 0x00);
+       //More slave config
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00);
+       //Reset sensor signal paths
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x00);
+       //Motion detection control
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DETECT_CTRL, 0x00);
+       //Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0x00);
+       //Sets clock source to gyro reference w/ PLL
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010);
+       //Controls frequency of wakeups in accel low power mode plus the sensor standby modes
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_2, 0x00);
+       //MPU6050_RA_BANK_SEL                        //Not in datasheet
+       //MPU6050_RA_MEM_START_ADDR                //Not in datasheet
+       //MPU6050_RA_MEM_R_W                        //Not in datasheet
+       //MPU6050_RA_DMP_CFG_1                        //Not in datasheet
+       //MPU6050_RA_DMP_CFG_2                        //Not in datasheet
+       //MPU6050_RA_FIFO_COUNTH                //Read-only
+       //MPU6050_RA_FIFO_COUNTL                //Read-only
+       //Data transfer to and from the FIFO buffer
+       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00);
+       //MPU6050_RA_WHO_AM_I                         //Read-only, I2C address
+
+       setup_mpu9150_magneto();
+
+
+       printf("MPU6050 Setup Complete\r\n");
+       mpu6050_compute_drift();
+       printf("MPU6050 drift computed\r\n");
+
+}

diff --git a/mpu6050.h b/mpu6050.h
new file mode 100644 (file)
index 0000000..5113b0d
--- /dev/null
+++ b/mpu6050.h
@@ -0,0 +1,137 @@
+
+#ifndef _MPU6050_H_
+#define _MPU6050_H_
+
+
+#define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
+#define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
+#define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
+#define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN
+#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN
+#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN
+#define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS
+#define MPU6050_RA_XA_OFFS_L_TC 0x07
+#define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS
+#define MPU6050_RA_YA_OFFS_L_TC 0x09
+#define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS
+#define MPU6050_RA_ZA_OFFS_L_TC 0x0B
+#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR
+#define MPU6050_RA_XG_OFFS_USRL 0x14
+#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR
+#define MPU6050_RA_YG_OFFS_USRL 0x16
+#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR
+#define MPU6050_RA_ZG_OFFS_USRL 0x18
+#define MPU6050_RA_SMPLRT_DIV 0x19
+#define MPU6050_RA_CONFIG 0x1A
+#define MPU6050_RA_GYRO_CONFIG 0x1B
+#define MPU6050_RA_ACCEL_CONFIG 0x1C
+#define MPU6050_RA_FF_THR 0x1D
+#define MPU6050_RA_FF_DUR 0x1E
+#define MPU6050_RA_MOT_THR 0x1F
+#define MPU6050_RA_MOT_DUR 0x20
+#define MPU6050_RA_ZRMOT_THR 0x21
+#define MPU6050_RA_ZRMOT_DUR 0x22
+#define MPU6050_RA_FIFO_EN 0x23
+#define MPU6050_RA_I2C_MST_CTRL 0x24
+#define MPU6050_RA_I2C_SLV0_ADDR 0x25
+#define MPU6050_RA_I2C_SLV0_REG 0x26
+#define MPU6050_RA_I2C_SLV0_CTRL 0x27
+#define MPU6050_RA_I2C_SLV1_ADDR 0x28
+#define MPU6050_RA_I2C_SLV1_REG 0x29
+#define MPU6050_RA_I2C_SLV1_CTRL 0x2A
+#define MPU6050_RA_I2C_SLV2_ADDR 0x2B
+#define MPU6050_RA_I2C_SLV2_REG 0x2C
+#define MPU6050_RA_I2C_SLV2_CTRL 0x2D
+#define MPU6050_RA_I2C_SLV3_ADDR 0x2E
+#define MPU6050_RA_I2C_SLV3_REG 0x2F
+#define MPU6050_RA_I2C_SLV3_CTRL 0x30
+#define MPU6050_RA_I2C_SLV4_ADDR 0x31
+#define MPU6050_RA_I2C_SLV4_REG 0x32
+#define MPU6050_RA_I2C_SLV4_DO 0x33
+#define MPU6050_RA_I2C_SLV4_CTRL 0x34
+#define MPU6050_RA_I2C_SLV4_DI 0x35
+#define MPU6050_RA_I2C_MST_STATUS 0x36
+#define MPU6050_RA_INT_PIN_CFG 0x37
+#define MPU6050_RA_INT_ENABLE 0x38
+#define MPU6050_RA_DMP_INT_STATUS 0x39
+#define MPU6050_RA_INT_STATUS 0x3A
+#define MPU6050_RA_ACCEL_XOUT_H 0x3B
+#define MPU6050_RA_ACCEL_XOUT_L 0x3C
+#define MPU6050_RA_ACCEL_YOUT_H 0x3D
+#define MPU6050_RA_ACCEL_YOUT_L 0x3E
+#define MPU6050_RA_ACCEL_ZOUT_H 0x3F
+#define MPU6050_RA_ACCEL_ZOUT_L 0x40
+#define MPU6050_RA_TEMP_OUT_H 0x41
+#define MPU6050_RA_TEMP_OUT_L 0x42
+#define MPU6050_RA_GYRO_XOUT_H 0x43
+#define MPU6050_RA_GYRO_XOUT_L 0x44
+#define MPU6050_RA_GYRO_YOUT_H 0x45
+#define MPU6050_RA_GYRO_YOUT_L 0x46
+#define MPU6050_RA_GYRO_ZOUT_H 0x47
+#define MPU6050_RA_GYRO_ZOUT_L 0x48
+#define MPU6050_RA_EXT_SENS_DATA_00 0x49
+#define MPU6050_RA_EXT_SENS_DATA_01 0x4A
+#define MPU6050_RA_EXT_SENS_DATA_02 0x4B
+#define MPU6050_RA_EXT_SENS_DATA_03 0x4C
+#define MPU6050_RA_EXT_SENS_DATA_04 0x4D
+#define MPU6050_RA_EXT_SENS_DATA_05 0x4E
+#define MPU6050_RA_EXT_SENS_DATA_06 0x4F
+#define MPU6050_RA_EXT_SENS_DATA_07 0x50
+#define MPU6050_RA_EXT_SENS_DATA_08 0x51
+#define MPU6050_RA_EXT_SENS_DATA_09 0x52
+#define MPU6050_RA_EXT_SENS_DATA_10 0x53
+#define MPU6050_RA_EXT_SENS_DATA_11 0x54
+#define MPU6050_RA_EXT_SENS_DATA_12 0x55
+#define MPU6050_RA_EXT_SENS_DATA_13 0x56
+#define MPU6050_RA_EXT_SENS_DATA_14 0x57
+#define MPU6050_RA_EXT_SENS_DATA_15 0x58
+#define MPU6050_RA_EXT_SENS_DATA_16 0x59
+#define MPU6050_RA_EXT_SENS_DATA_17 0x5A
+#define MPU6050_RA_EXT_SENS_DATA_18 0x5B
+#define MPU6050_RA_EXT_SENS_DATA_19 0x5C
+#define MPU6050_RA_EXT_SENS_DATA_20 0x5D
+#define MPU6050_RA_EXT_SENS_DATA_21 0x5E
+#define MPU6050_RA_EXT_SENS_DATA_22 0x5F
+#define MPU6050_RA_EXT_SENS_DATA_23 0x60
+#define MPU6050_RA_MOT_DETECT_STATUS 0x61
+#define MPU6050_RA_I2C_SLV0_DO 0x63
+#define MPU6050_RA_I2C_SLV1_DO 0x64
+#define MPU6050_RA_I2C_SLV2_DO 0x65
+#define MPU6050_RA_I2C_SLV3_DO 0x66
+#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67
+#define MPU6050_RA_SIGNAL_PATH_RESET 0x68
+#define MPU6050_RA_MOT_DETECT_CTRL 0x69
+#define MPU6050_RA_USER_CTRL 0x6A
+#define MPU6050_RA_PWR_MGMT_1 0x6B
+#define MPU6050_RA_PWR_MGMT_2 0x6C
+#define MPU6050_RA_BANK_SEL 0x6D
+#define MPU6050_RA_MEM_START_ADDR 0x6E
+#define MPU6050_RA_MEM_R_W 0x6F
+#define MPU6050_RA_DMP_CFG_1 0x70
+#define MPU6050_RA_DMP_CFG_2 0x71
+#define MPU6050_RA_FIFO_COUNTH 0x72
+#define MPU6050_RA_FIFO_COUNTL 0x73
+#define MPU6050_RA_FIFO_R_W 0x74
+#define MPU6050_RA_WHO_AM_I 0x75
+
+
+uint8_t mpu6050_init(void);
+uint8_t mpu6050_read_all_axes(int16_t *values);
+
+
+extern float mpu6050_gx;
+extern float mpu6050_gy;
+extern float mpu6050_gz;
+
+extern float mpu6050_ax;
+extern float mpu6050_ay;
+extern float mpu6050_az;
+
+
+extern float mpu6050_mx;
+extern float mpu6050_my;
+extern float mpu6050_mz;
+
+extern float mpu6050_temp;
+
+#endif // _MPU6050_H_

diff --git a/scheduler_config.h b/scheduler_config.h
new file mode 100644 (file)
index 0000000..708bfd6
--- /dev/null
+++ b/scheduler_config.h
@@ -0,0 +1,77 @@
+/*
+ *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: scheduler_config.h,v 1.1.2.1 2007-12-06 08:58:00 zer0 Exp $
+ *
+ */
+
+#ifndef _SCHEDULER_CONFIG_H_
+#define _SCHEDULER_CONFIG_H_
+
+#define _SCHEDULER_CONFIG_VERSION_ 4
+
+/** maximum number of allocated events */
+#define SCHEDULER_NB_MAX_EVENT 5
+
+
+/* define it only if CONFIG_MODULE_SCHEDULER_USE_TIMERS is enabled. In
+   this case, precaler is defined in timers_config.h in your project
+   directory. */
+#ifdef CONFIG_MODULE_SCHEDULER_USE_TIMERS
+/** the num of the timer to use for the scheduler */
+#define SCHEDULER_TIMER_NUM 0
+
+/* or set the prescaler manually (in this case, you use must TIMER0,
+   and the prescaler must be a correct value regarding the AVR device
+   you are using (look in include/aversive/parts.h). Obviously, the
+   values of SCHEDULER_CK and SCHEDULER_CLOCK_PRESCALER must also be
+   coherent (TIMER0_PRESCALER_DIV_VALUE and VALUE) */
+#endif /* CONFIG_MODULE_SCHEDULER_USE_TIMERS */
+
+
+#ifdef CONFIG_MODULE_SCHEDULER_TIMER0
+/* The 2 values below MUST be coherent:
+ * if  SCHEDULER_CK = TIMER0_PRESCALER_DIV_x, then
+ * you must have SCHEDULER_CLOCK_PRESCALER = x too !!! */
+#define SCHEDULER_CK TIMER0_PRESCALER_DIV_8
+#define SCHEDULER_CLOCK_PRESCALER 8
+
+#endif /* CONFIG_MODULE_SCHEDULER_TIMER0 */
+
+/* last case, the scheduler is called manually. The user has to
+   define the period here */
+#ifdef CONFIG_MODULE_SCHEDULER_MANUAL
+
+#define SCHEDULER_UNIT_FLOAT 1024.0
+#define SCHEDULER_UNIT 1024UL
+
+#endif /* CONFIG_MODULE_SCHEDULER_MANUAL */
+
+/** number of allowed imbricated scheduler interrupts. The maximum
+ * should be SCHEDULER_NB_MAX_EVENT since we never need to imbricate
+ * more than once per event. If it is less, it can avoid to browse the
+ * event table, events are delayed (we loose precision) but it takes
+ * less CPU */
+#define SCHEDULER_NB_STACKING_MAX SCHEDULER_NB_MAX_EVENT
+
+/** define it for debug infos (not recommended, because very slow on
+ *  an AVR, it uses printf in an interrupt). It can be useful if
+ *  prescaler is very high, making the timer interrupt period very
+ *  long in comparison to printf() */
+/* #define SCHEDULER_DEBUG */
+
+#endif // _SCHEDULER_CONFIG_H_

diff --git a/time_config.h b/time_config.h
new file mode 100644 (file)
index 0000000..94b541d
--- /dev/null
+++ b/time_config.h
@@ -0,0 +1,23 @@
+/*  
+ *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: time_config.h,v 1.1.2.1 2007-12-06 08:58:00 zer0 Exp $
+ *
+ */
+
+/** precision of the time processor, in us */
+#define TIME_PRECISION 10000l

diff --git a/timer_config.h b/timer_config.h
new file mode 100644 (file)
index 0000000..986a7f5
--- /dev/null
+++ b/timer_config.h
@@ -0,0 +1,25 @@
+/*  
+ *  Copyright Droids Corporation, Microb Technology, Eirbot (2006)
+ * 
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: timer_config.h,v 1.1.2.1 2007-12-06 08:58:00 zer0 Exp $
+ *
+ */
+
+#define TIMER0_ENABLED
+
+
+#define TIMER0_PRESCALER_DIV 8

diff --git a/translate_code.py b/translate_code.py
new file mode 100644 (file)
index 0000000..715a290
--- /dev/null
+++ b/translate_code.py
@@ -0,0 +1,90 @@
+#! /usr/bin/env python
+import os
+
+
+
+filename = os.environ.get('PYTHONSTARTUP')
+if filename and os.path.isfile(filename):
+    execfile(filename)
+
+from miasm2.core.cpu  import parse_ast
+from miasm2.arch.x86_arch  import mn_x86, base_expr, variable
+from miasm2.core.bin_stream import bin_stream
+from miasm2.core import parse_asm
+from elfesteem import *
+from pdb import pm
+from miasm2.core import asmbloc
+import struct
+from miasm2.expression.expression import *
+
+
+def my__sub__(self, a):
+    return ExprOp('-', self, a)
+Expr.__sub__ = my__sub__
+
+
+def my_parse_op(t):
+    print "OP", t
+    v = t[0]
+    return (ExprOp, v)
+
+
+
+reg_and_id = {}
+
+def my_ast_int2expr(a):
+    return ExprInt32(a)
+def my_ast_id2expr(t):
+    #print "XXX", t
+    t = 'f_'+t
+    if not t in reg_and_id:
+        i = ExprId(t)
+        reg_and_id[t] = i
+    return reg_and_id.get(t, ExprId(t, size=32))
+
+my_var_parser = parse_ast(my_ast_id2expr, my_ast_int2expr)
+base_expr.setParseAction(my_var_parser)
+
+all_bloc, symbol_pool = parse_asm.parse_txt(mn_x86, 32, '''
+main:
+    PUSH q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3
+''')
+
+
+
+a = all_bloc[0][0].lines[0].args[0]
+
+
+
+
+def ExprOp_to_Math(self):
+    print self.op, self.args
+    ops2 = {'+':'f32_add',
+           '*':'f32_mul',
+            '-':'f32_sub',
+           }
+    out = [x.Expr_to_Math() for x in self.args]
+    if len(self.args) == 1:
+        return "f32_neg(%s)"%(out[0])
+    elif self.op in ops2:
+        last = out.pop()
+        print out
+        while out:
+            o = out.pop()
+            last = '%s(%s, %s)'%(ops2[self.op],
+                                 o, last)
+        return last
+    fds
+def ExprId_to_Math(self):
+    return self.name
+
+
+def ExprInt_to_Math(self):
+    return "f32_from_double(%s)"%int(self.arg)
+ExprOp.Expr_to_Math = ExprOp_to_Math
+ExprId.Expr_to_Math = ExprId_to_Math
+ExprInt.Expr_to_Math = ExprInt_to_Math
+
+xx = a.Expr_to_Math()
+print xx
+

diff --git a/uart_config.h b/uart_config.h
new file mode 100644 (file)
index 0000000..6a9ae2c
--- /dev/null
+++ b/uart_config.h
@@ -0,0 +1,73 @@
+/*  \r
+ *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)\r
+ * \r
+ *  This program is free software; you can redistribute it and/or modify\r
+ *  it under the terms of the GNU General Public License as published by\r
+ *  the Free Software Foundation; either version 2 of the License, or\r
+ *  (at your option) any later version.\r
+ *\r
+ *  This program is distributed in the hope that it will be useful,\r
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
+ *  GNU General Public License for more details.\r
+ *\r
+ *  You should have received a copy of the GNU General Public License\r
+ *  along with this program; if not, write to the Free Software\r
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA\r
+ *\r
+ *  Revision : $Id: uart_config.h,v 1.2.8.1 2006-11-26 21:06:06 zer0 Exp $\r
+ *\r
+ */\r
+\r
+/* Droids-corp 2004 - Zer0\r
+ * config for uart module\r
+ */\r
+\r
+#ifndef UART_CONFIG_H\r
+#define UART_CONFIG_H\r
+\r
+/*\r
+ * UART0 definitions \r
+ */\r
+\r
+/* compile uart0 fonctions, undefine it to pass compilation */\r
+#define UART0_COMPILE  \r
+\r
+/* enable uart0 if == 1, disable if == 0 */\r
+#define UART0_ENABLED  1\r
+\r
+/* enable uart0 interrupts if == 1, disable if == 0 */\r
+#define UART0_INTERRUPT_ENABLED  1\r
+\r
+#define UART0_BAUDRATE 57600\r
+//#define UART0_BAUDRATE 115200\r
+\r
+/* \r
+ * if you enable this, the maximum baudrate you can reach is \r
+ * higher, but the precision is lower. \r
+ */\r
+//#define UART0_USE_DOUBLE_SPEED 0\r
+#define UART0_USE_DOUBLE_SPEED 1\r
+\r
+#define UART0_RX_FIFO_SIZE 4\r
+#define UART0_TX_FIFO_SIZE 4\r
+//#define UART0_NBITS 5\r
+//#define UART0_NBITS 6\r
+//#define UART0_NBITS 7\r
+#define UART0_NBITS 8\r
+//#define UART0_NBITS 9\r
+\r
+#define UART0_PARITY UART_PARTITY_NONE\r
+//#define UART0_PARITY UART_PARTITY_ODD\r
+//#define UART0_PARITY UART_PARTITY_EVEN\r
+\r
+#define UART0_STOP_BIT UART_STOP_BITS_1\r
+//#define UART0_STOP_BIT UART_STOP_BITS_2\r
+\r
+\r
+\r
+\r
+/* .... same for uart 1, 2, 3 ... */\r
+\r
+#endif\r
+\r

diff --git a/vector.c b/vector.c
new file mode 100644 (file)
index 0000000..3938be2
--- /dev/null
+++ b/vector.c
@@ -0,0 +1,35 @@
+//Computes the dot product of two vectors
+float Vector_Dot_Product(float vector1[3],float vector2[3])
+{
+       float op=0;
+
+       for(int c=0; c<3; c++) {
+               op+=vector1[c]*vector2[c];
+       }
+
+       return op;
+}
+
+//Computes the cross product of two vectors
+void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3])
+{
+       vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
+       vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
+       vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
+}
+
+//Multiply the vector by a scalar.
+void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2)
+{
+       for(int c=0; c<3; c++) {
+               vectorOut[c]=vectorIn[c]*scale2;
+       }
+}
+
+void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3])
+{
+       for(int c=0; c<3; c++) {
+               vectorOut[c]=vectorIn1[c]+vectorIn2[c];
+       }
+}
+

diff --git a/vector.h b/vector.h
new file mode 100644 (file)
index 0000000..588def3
--- /dev/null
+++ b/vector.h
@@ -0,0 +1,9 @@
+#ifndef _VECTOR_H_
+#define _VECTOR_H_
+
+float Vector_Dot_Product(float vector1[3],float vector2[3]);
+void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3]);
+void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2);
+void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3]);;
+
+#endif// _VECTOR_H_