--- /dev/null
+# 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
--- /dev/null
+//=====================================================================================================\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
--- /dev/null
+//=====================================================================================================\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
--- /dev/null
+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
--- /dev/null
+/*
+ * 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
--- /dev/null
+/*
+ * 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
--- /dev/null
+#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;
+
+}
--- /dev/null
+
+#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_
--- /dev/null
+/*
+ * 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;
+}
+
+
--- /dev/null
+#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
--- /dev/null
+#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;
+}
--- /dev/null
+//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];
+ }
+ }
+}
--- /dev/null
+#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_
--- /dev/null
+#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");
+
+}
--- /dev/null
+
+#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_
--- /dev/null
+/*
+ * 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_
--- /dev/null
+/*
+ * 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
--- /dev/null
+/*
+ * 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
--- /dev/null
+#! /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
+
--- /dev/null
+/* \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
--- /dev/null
+//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];
+ }
+}
+
--- /dev/null
+#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_
git.droids-corp.org / protos / imu.git / commitdiff