From: Olivier Matz Date: Thu, 26 Jun 2014 18:09:09 +0000 (+0200) Subject: import code from Fabrice's repository X-Git-Url: http://git.droids-corp.org/?a=commitdiff_plain;h=96d834bdfb8df4e3369ca1b3c7bc7bc8534fda31;p=protos%2Fimu.git import code from Fabrice's repository --- 96d834bdfb8df4e3369ca1b3c7bc7bc8534fda31 diff --git a/IMU_Razor9DOF.py b/IMU_Razor9DOF.py new file mode 100644 index 0000000..91351ae --- /dev/null +++ b/IMU_Razor9DOF.py @@ -0,0 +1,146 @@ +# Test for Razor 9DOF IMU +# Jose Julio @2009 +# This script needs VPhyton, pyserial and pywin modules + +# First Install Python 2.6.4 +# Install pywin from http://sourceforge.net/projects/pywin32/ +# Install pyserial from http://sourceforge.net/projects/pyserial/files/ +# Install Vphyton from http://vpython.org/contents/download_windows.html + +from visual import * +import serial +import string +import math +import sys + +from time import time + +#ev = scene.waitfor('click keydown') + +grad2rad = 3.141592/180.0 + +# Check your COM port and baud rate +ser = serial.Serial(port='/dev/ttyUSB0',baudrate=57600, timeout=1) + +# Main scene +scene=display(title="9DOF Razor IMU test") +scene.range=(1.2,1.2,1.2) +#scene.forward = (0,-1,-0.25) +scene.forward = (1,0,-0.25) +scene.up=(0,0,1) + +# Second scene (Roll, Pitch, Yaw) +scene2 = display(title='9DOF Razor IMU test',x=0, y=0, width=500, height=200,center=(0,0,0), background=(0,0,0)) +scene2.range=(1,1,1) +scene.width=500 +scene.y=200 + +scene2.select() +#Roll, Pitch, Yaw +cil_roll = cylinder(pos=(-0.4,0,0),axis=(0.2,0,0),radius=0.01,color=color.red) +cil_roll2 = cylinder(pos=(-0.4,0,0),axis=(-0.2,0,0),radius=0.01,color=color.red) +cil_pitch = cylinder(pos=(0.1,0,0),axis=(0.2,0,0),radius=0.01,color=color.green) +cil_pitch2 = cylinder(pos=(0.1,0,0),axis=(-0.2,0,0),radius=0.01,color=color.green) +#cil_course = cylinder(pos=(0.6,0,0),axis=(0.2,0,0),radius=0.01,color=color.blue) +#cil_course2 = cylinder(pos=(0.6,0,0),axis=(-0.2,0,0),radius=0.01,color=color.blue) +arrow_course = arrow(pos=(0.6,0,0),color=color.cyan,axis=(-0.2,0,0), shaftwidth=0.02, fixedwidth=1) + +#Roll,Pitch,Yaw labels +label(pos=(-0.4,0.3,0),text="Roll",box=0,opacity=0) +label(pos=(0.1,0.3,0),text="Pitch",box=0,opacity=0) +label(pos=(0.55,0.3,0),text="Yaw",box=0,opacity=0) +label(pos=(0.6,0.22,0),text="N",box=0,opacity=0,color=color.yellow) +label(pos=(0.6,-0.22,0),text="S",box=0,opacity=0,color=color.yellow) +label(pos=(0.38,0,0),text="W",box=0,opacity=0,color=color.yellow) +label(pos=(0.82,0,0),text="E",box=0,opacity=0,color=color.yellow) +label(pos=(0.75,0.15,0),height=7,text="NE",box=0,color=color.yellow) +label(pos=(0.45,0.15,0),height=7,text="NW",box=0,color=color.yellow) +label(pos=(0.75,-0.15,0),height=7,text="SE",box=0,color=color.yellow) +label(pos=(0.45,-0.15,0),height=7,text="SW",box=0,color=color.yellow) + +L1 = label(pos=(-0.4,0.22,0),text="-",box=0,opacity=0) +L2 = label(pos=(0.1,0.22,0),text="-",box=0,opacity=0) +L3 = label(pos=(0.7,0.3,0),text="-",box=0,opacity=0) + +# Main scene objects +scene.select() +# Reference axis (x,y,z) +arrow(color=color.green,axis=(1,0,0), shaftwidth=0.02, fixedwidth=1) +arrow(color=color.green,axis=(0,-1,0), shaftwidth=0.02 , fixedwidth=1) +arrow(color=color.green,axis=(0,0,-1), shaftwidth=0.02, fixedwidth=1) +# labels +label(pos=(0,0,0.8),text="9DOF Razor IMU test",box=0,opacity=0) +label(pos=(1,0,0),text="X",box=0,opacity=0) +label(pos=(0,-1,0),text="Y",box=0,opacity=0) +label(pos=(0,0,-1),text="Z",box=0,opacity=0) +# IMU object +platform = box(length=1, height=0.05, width=1, color=color.red) +p_line = box(length=1,height=0.08,width=0.1,color=color.yellow) +plat_arrow = arrow(color=color.green,axis=(1,0,0), shaftwidth=0.06, fixedwidth=1) + + +f = open("Serial"+str(time())+".txt", 'w') + +roll=0 +pitch=0 +yaw=0 +cpt = 0 +t_start = time() +while 1: + line = ser.readline() + line = line.replace("!ANG:","") # Delete "!ANG:" + #print line + f.write(line) # Write to the output log file + words = string.split(line,"\t") # Fields split + #print words + print words + if len(words) != 3: + continue + + """ + a = float(words[0])#*grad2rad + b = float(words[1])#*grad2rad + c = float(words[2])#*grad2rad + d = float(words[3])#*grad2rad + + platform.axis = (1, 1, 1) + #print a, b, c, d + platform.rotate(angle=a, axis = (b, c, d)) + continue + """ + try: + roll = float(words[0])#*grad2rad + pitch = float(words[1])#*grad2rad + yaw = float(words[2])#*grad2rad + except: + print "Invalid line" + continue + + cpt += 1 + if cpt > 10: + t = time() - t_start + t_start = time() + print "XXX", cpt/t + cpt = 0 + + axis=(cos(pitch)*cos(yaw),-cos(pitch)*sin(yaw),sin(pitch)) + 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)) + platform.axis=axis + platform.up=up + platform.length=1.0 + platform.width=0.65 + plat_arrow.axis=axis + plat_arrow.up=up + plat_arrow.length=0.8 + p_line.axis=axis + p_line.up=up + cil_roll.axis=(0.2*cos(roll),0.2*sin(roll),0) + cil_roll2.axis=(-0.2*cos(roll),-0.2*sin(roll),0) + cil_pitch.axis=(0.2*cos(pitch),0.2*sin(pitch),0) + cil_pitch2.axis=(-0.2*cos(pitch),-0.2*sin(pitch),0) + arrow_course.axis=(0.2*sin(yaw),0.2*cos(yaw),0) + L1.text = str(float(words[0])) + L2.text = str(float(words[1])) + L3.text = str(float(words[2])) +ser.close() +f.close() diff --git a/MadgwickAHRS.c b/MadgwickAHRS.c new file mode 100644 index 0000000..fc547d2 --- /dev/null +++ b/MadgwickAHRS.c @@ -0,0 +1,441 @@ +//===================================================================================================== +// MadgwickAHRS.c +//===================================================================================================== +// +// Implementation of Madgwick's IMU and AHRS algorithms. +// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms +// +// Date Author Notes +// 29/09/2011 SOH Madgwick Initial release +// 02/10/2011 SOH Madgwick Optimised for reduced CPU load +// 19/02/2012 SOH Madgwick Magnetometer measurement is normalised +// +//===================================================================================================== + +//--------------------------------------------------------------------------------------------------- +// Header files + +#include "MadgwickAHRS.h" +#include + +#include + + +//--------------------------------------------------------------------------------------------------- +// Definitions + +//#define sampleFreq 512.0f // sample frequency in Hz +//#define sampleFreq 46.0f // sample frequency in Hz +#define sampleFreq 85.0f // sample frequency in Hz +#define betaDef 0.1f // 2 * proportional gain + +//--------------------------------------------------------------------------------------------------- +// Variable definitions + +volatile float beta = betaDef; // 2 * proportional gain (Kp) +volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame + + +//--------------------------------------------------------------------------------------------------- +// Function declarations + +float invSqrt(float x); + +//==================================================================================================== +// Functions + +//--------------------------------------------------------------------------------------------------- +// AHRS algorithm update + +void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { + float recipNorm; + float s0, s1, s2, s3; + float qDot1, qDot2, qDot3, qDot4; + float hx, hy; + 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; + + // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) + if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { + MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az); + return; + } + + // Rate of change of quaternion from gyroscope + qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); + qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); + qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); + qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + + // Normalise accelerometer measurement + recipNorm = invSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Normalise magnetometer measurement + recipNorm = invSqrt(mx * mx + my * my + mz * mz); + mx *= recipNorm; + my *= recipNorm; + mz *= recipNorm; + + // Auxiliary variables to avoid repeated arithmetic + _2q0mx = 2.0f * q0 * mx; + _2q0my = 2.0f * q0 * my; + _2q0mz = 2.0f * q0 * mz; + _2q1mx = 2.0f * q1 * mx; + _2q0 = 2.0f * q0; + _2q1 = 2.0f * q1; + _2q2 = 2.0f * q2; + _2q3 = 2.0f * q3; + _2q0q2 = 2.0f * q0 * q2; + _2q2q3 = 2.0f * q2 * q3; + q0q0 = q0 * q0; + q0q1 = q0 * q1; + q0q2 = q0 * q2; + q0q3 = q0 * q3; + q1q1 = q1 * q1; + q1q2 = q1 * q2; + q1q3 = q1 * q3; + q2q2 = q2 * q2; + q2q3 = q2 * q3; + q3q3 = q3 * q3; + + // Reference direction of Earth's magnetic field + hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3; + hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3; + _2bx = sqrt(hx * hx + hy * hy); + _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3; + _4bx = 2.0f * _2bx; + _4bz = 2.0f * _2bz; + + // Gradient decent algorithm corrective step + 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); + 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); + 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); + 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); + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude + s0 *= recipNorm; + s1 *= recipNorm; + s2 *= recipNorm; + s3 *= recipNorm; + + // Apply feedback step + qDot1 -= beta * s0; + qDot2 -= beta * s1; + qDot3 -= beta * s2; + qDot4 -= beta * s3; + } + + // Integrate rate of change of quaternion to yield quaternion + q0 += qDot1 * (1.0f / sampleFreq); + q1 += qDot2 * (1.0f / sampleFreq); + q2 += qDot3 * (1.0f / sampleFreq); + q3 += qDot4 * (1.0f / sampleFreq); + + // Normalise quaternion + recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; +} + +//--------------------------------------------------------------------------------------------------- +// IMU algorithm update + +void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) { + float recipNorm; + float s0, s1, s2, s3; + float qDot1, qDot2, qDot3, qDot4; + float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; + + // Rate of change of quaternion from gyroscope + qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); + qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); + qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); + qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); + + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + + // Normalise accelerometer measurement + recipNorm = invSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Auxiliary variables to avoid repeated arithmetic + _2q0 = 2.0f * q0; + _2q1 = 2.0f * q1; + _2q2 = 2.0f * q2; + _2q3 = 2.0f * q3; + _4q0 = 4.0f * q0; + _4q1 = 4.0f * q1; + _4q2 = 4.0f * q2; + _8q1 = 8.0f * q1; + _8q2 = 8.0f * q2; + q0q0 = q0 * q0; + q1q1 = q1 * q1; + q2q2 = q2 * q2; + q3q3 = q3 * q3; + + + // Gradient decent algorithm corrective step + s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; + s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; + s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; + s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude + + + + s0 *= recipNorm; + s1 *= recipNorm; + s2 *= recipNorm; + s3 *= recipNorm; + + + // Apply feedback step + qDot1 -= beta * s0; + qDot2 -= beta * s1; + qDot3 -= beta * s2; + qDot4 -= beta * s3; + } + + // Integrate rate of change of quaternion to yield quaternion + q0 += qDot1 * (1.0f / sampleFreq); + q1 += qDot2 * (1.0f / sampleFreq); + q2 += qDot3 * (1.0f / sampleFreq); + q3 += qDot4 * (1.0f / sampleFreq); + + + // Normalise quaternion + recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + + //printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", q0, q1, q2); + +} + + +//--------------------------------------------------------------------------------------------------- +// IMU algorithm update + +f32 f_q0; +f32 f_q1; +f32 f_q2; +f32 f_q3; + + +void Mad_f32_init() +{ + f_q0 = f32_from_double((double)1.0); + f_q1 = f32_from_double((double)0.0); + f_q2 = f32_from_double((double)0.0); + f_q3 = f32_from_double((double)0.0); + +} +void MadgwickAHRSupdateIMU_f32(float gx, float gy, float gz, float ax, float ay, float az) { + f32 f_recipNorm; + f32 f_s0, f_s1, f_s2, f_s3; + f32 f_qDot1, f_qDot2, f_qDot3, f_qDot4; + 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; + + f32 f_gx, f_gy, f_gz; + f32 f_ax, f_ay, f_az; + + f32 f_beta; + f32 f_sampleFreq; + + + f_gx = f32_from_double((double)gx); + f_gy = f32_from_double((double)gy); + f_gz = f32_from_double((double)gz); + + f_ax = f32_from_double((double)ax); + f_ay = f32_from_double((double)ay); + f_az = f32_from_double((double)az); + + f_beta = f32_from_double((double)beta); + f_sampleFreq = f32_from_double((double)sampleFreq); + + + // Rate of change of quaternion from gyroscope + /* + qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); + qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); + qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); + qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); + */ + + 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)); + 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)); + 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)); + 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)); + + + + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + + // Normalise accelerometer measurement + //recipNorm = invSqrt(ax * ax + ay * ay + az * az); + 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)))))); + /* + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + */ + + f_ax = f32_mul(f_ax, f_recipNorm); + f_ay = f32_mul(f_ay, f_recipNorm); + f_az = f32_mul(f_az, f_recipNorm); + + // Auxiliary variables to avoid repeated arithmetic + + /* + _2q0 = 2.0f * q0; + _2q1 = 2.0f * q1; + _2q2 = 2.0f * q2; + _2q3 = 2.0f * q3; + _4q0 = 4.0f * q0; + _4q1 = 4.0f * q1; + _4q2 = 4.0f * q2; + _8q1 = 8.0f * q1; + _8q2 = 8.0f * q2; + q0q0 = q0 * q0; + q1q1 = q1 * q1; + q2q2 = q2 * q2; + q3q3 = q3 * q3; + */ + + f__2q0 = f32_mul(f32_from_double(2.0f), f_q0); + f__2q1 = f32_mul(f32_from_double(2.0f), f_q1); + f__2q2 = f32_mul(f32_from_double(2.0f), f_q2); + f__2q3 = f32_mul(f32_from_double(2.0f), f_q3); + f__4q0 = f32_mul(f32_from_double(4.0f), f_q0); + f__4q1 = f32_mul(f32_from_double(4.0f), f_q1); + f__4q2 = f32_mul(f32_from_double(4.0f), f_q2); + f__8q1 = f32_mul(f32_from_double(8.0f), f_q1); + f__8q2 = f32_mul(f32_from_double(8.0f), f_q2); + f_q0q0 = f32_mul(f_q0, f_q0); + f_q1q1 = f32_mul(f_q1, f_q1); + f_q2q2 = f32_mul(f_q2, f_q2); + f_q3q3 = f32_mul(f_q3, f_q3); + + + // Gradient decent algorithm corrective step + + /* + s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; + s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; + s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; + s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude + */ + + 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)); + 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)))); + 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)))); + 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)); + 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)))))); + + /* + s0 *= recipNorm; + s1 *= recipNorm; + s2 *= recipNorm; + s3 *= recipNorm; + */ + + f_s0 = f32_mul(f_s0, f_recipNorm); + f_s1 = f32_mul(f_s1, f_recipNorm); + f_s2 = f32_mul(f_s2, f_recipNorm); + f_s3 = f32_mul(f_s3, f_recipNorm); + + + // Apply feedback step + + /* + qDot1 -= beta * s0; + qDot2 -= beta * s1; + qDot3 -= beta * s2; + qDot4 -= beta * s3; + */ + + f_qDot1 = f32_sub(f_qDot1, f32_mul(f_beta, f_s0)); + f_qDot2 = f32_sub(f_qDot2, f32_mul(f_beta, f_s1)); + f_qDot3 = f32_sub(f_qDot3, f32_mul(f_beta, f_s2)); + f_qDot4 = f32_sub(f_qDot4, f32_mul(f_beta, f_s3)); + + } + + // Integrate rate of change of quaternion to yield quaternion + + /* + q0 += qDot1 * (1.0f / sampleFreq); + q1 += qDot2 * (1.0f / sampleFreq); + q2 += qDot3 * (1.0f / sampleFreq); + q3 += qDot4 * (1.0f / sampleFreq); + */ + + f_q0 = f32_add(f_q0, f32_mul(f_qDot1, f32_inv(f_sampleFreq))); + f_q1 = f32_add(f_q1, f32_mul(f_qDot2, f32_inv(f_sampleFreq))); + f_q2 = f32_add(f_q2, f32_mul(f_qDot3, f32_inv(f_sampleFreq))); + f_q3 = f32_add(f_q3, f32_mul(f_qDot4, f32_inv(f_sampleFreq))); + + // Normalise quaternion + //recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + + 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)))))); + + /* + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + */ + + f_q0 = f32_mul(f_q0, f_recipNorm); + f_q1 = f32_mul(f_q1, f_recipNorm); + f_q2 = f32_mul(f_q2, f_recipNorm); + f_q3 = f32_mul(f_q3, f_recipNorm); + + + q0 = f32_to_double(f_q0); + q1 = f32_to_double(f_q1); + q2 = f32_to_double(f_q2); + q3 = f32_to_double(f_q3); + + //printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", q0, q1, q2); + +} + + +//--------------------------------------------------------------------------------------------------- +// Fast inverse square-root +// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root +/* +float invSqrt(float x) { + float halfx = 0.5f * x; + float y = x; + long i = *(long*)&y; + i = 0x5f3759df - (i>>1); + y = *(float*)&i; + y = y * (1.5f - (halfx * y * y)); + return y; +} +*/ +float invSqrt(float x) { + return 1.0f / sqrtf(x); +} +//==================================================================================================== +// END OF CODE +//==================================================================================================== diff --git a/MadgwickAHRS.h b/MadgwickAHRS.h new file mode 100644 index 0000000..7e42291 --- /dev/null +++ b/MadgwickAHRS.h @@ -0,0 +1,33 @@ +//===================================================================================================== +// MadgwickAHRS.h +//===================================================================================================== +// +// Implementation of Madgwick's IMU and AHRS algorithms. +// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms +// +// Date Author Notes +// 29/09/2011 SOH Madgwick Initial release +// 02/10/2011 SOH Madgwick Optimised for reduced CPU load +// +//===================================================================================================== +#ifndef MadgwickAHRS_h +#define MadgwickAHRS_h + +//---------------------------------------------------------------------------------------------------- +// Variable declaration + +extern volatile float beta; // algorithm gain +extern volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame + +//--------------------------------------------------------------------------------------------------- +// Function declarations + +void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); +void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az); +void Mad_f32_init(); +void MadgwickAHRSupdateIMU_f32(float gx, float gy, float gz, float ax, float ay, float az); + +#endif +//===================================================================================================== +// End of file +//===================================================================================================== diff --git a/Makefile b/Makefile new file mode 100644 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 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 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 index 0000000..de929ae --- /dev/null +++ b/i2c_helper.c @@ -0,0 +1,57 @@ +#include +#include + +#include +#include + +#include +#include +#include + + + + +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 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 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 +#include +#include +#include + +#include + + +#include + + +#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>(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 index 0000000..8486960 --- /dev/null +++ b/imu.c @@ -0,0 +1,398 @@ +#include +#include + +#include +#include + +#include +#include + + +#include +//#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 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 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 index 0000000..4f1ab81 --- /dev/null +++ b/mpu6050.c @@ -0,0 +1,337 @@ +#include +#include + +#include +#include + +#include +#include +#include + +#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 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 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 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 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 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 index 0000000..6a9ae2c --- /dev/null +++ b/uart_config.h @@ -0,0 +1,73 @@ +/* + * 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: uart_config.h,v 1.2.8.1 2006-11-26 21:06:06 zer0 Exp $ + * + */ + +/* Droids-corp 2004 - Zer0 + * config for uart module + */ + +#ifndef UART_CONFIG_H +#define UART_CONFIG_H + +/* + * UART0 definitions + */ + +/* compile uart0 fonctions, undefine it to pass compilation */ +#define UART0_COMPILE + +/* enable uart0 if == 1, disable if == 0 */ +#define UART0_ENABLED 1 + +/* enable uart0 interrupts if == 1, disable if == 0 */ +#define UART0_INTERRUPT_ENABLED 1 + +#define UART0_BAUDRATE 57600 +//#define UART0_BAUDRATE 115200 + +/* + * if you enable this, the maximum baudrate you can reach is + * higher, but the precision is lower. + */ +//#define UART0_USE_DOUBLE_SPEED 0 +#define UART0_USE_DOUBLE_SPEED 1 + +#define UART0_RX_FIFO_SIZE 4 +#define UART0_TX_FIFO_SIZE 4 +//#define UART0_NBITS 5 +//#define UART0_NBITS 6 +//#define UART0_NBITS 7 +#define UART0_NBITS 8 +//#define UART0_NBITS 9 + +#define UART0_PARITY UART_PARTITY_NONE +//#define UART0_PARITY UART_PARTITY_ODD +//#define UART0_PARITY UART_PARTITY_EVEN + +#define UART0_STOP_BIT UART_STOP_BITS_1 +//#define UART0_STOP_BIT UART_STOP_BITS_2 + + + + +/* .... same for uart 1, 2, 3 ... */ + +#endif + diff --git a/vector.c b/vector.c new file mode 100644 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 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_