orientation.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, CoroWare
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Stanford U. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "orientation.h"
00031 #include <stdio.h>
00032 
00033 Orientation::Orientation()
00034 {
00035         gravityEpsilon = 0.07;
00036         validGravityCounter = 0;
00037         validAccelerationVectorsNecessaryToDetectGravity = 10;
00038         lastGravityVectorDetected[0] = 0;
00039         lastGravityVectorDetected[1] = 0;
00040         lastGravityVectorDetected[2] = 0;
00041     roll = 0;
00042     pitch = 0;
00043     yaw = 0;
00044         timestampPreviousCall = 0;
00045         pitchOffset = 0;
00046         rollOffset = 0;
00047 }
00048 
00049 double Orientation::ToRad(double value)
00050 //  degree to radian conversion
00051 {
00052     // *pi/180
00053     return value*0.01745329252;  
00054 }
00055 
00056 bool Orientation::isEqual(double vector1[3], double vector2[3])
00057 {
00058     if(abs(vector1[0] - (vector2[0]/GRAVITY) + vector1[1] - (vector2[1]/GRAVITY) + vector1[2] - (vector2[2]/GRAVITY))>0.0001)
00059         return false;
00060     else
00061         return true;
00062 }
00063 
00064 void Orientation::updateAngles(float gyroscopeVector[3], float AccelerationVector[3], double gyroscopeTimestamp)
00065 {
00066   if (timestampPreviousCall != 0)
00067   {
00068     roll = gyroscopeVector[0] * (gyroscopeTimestamp - timestampPreviousCall);
00069     pitch = gyroscopeVector[1] * (gyroscopeTimestamp - timestampPreviousCall);
00070     yaw = gyroscopeVector[2] * (gyroscopeTimestamp - timestampPreviousCall);
00071   } 
00072         calculate_new_gravity(AccelerationVector);
00073         timestampPreviousCall = gyroscopeTimestamp;
00074 }
00075 
00076 // detect the acceleration measure is the gravity, and if yes save it
00077 void Orientation::detectGravity(double gravity[3], float acceleration[3])
00078 {
00079     double norm = sqrt(acceleration[0] * acceleration[0] + acceleration[1] * acceleration[1] + acceleration[2] * acceleration[2]);
00080     
00081     /***** If the norm is near 1, then the vector is valid******/
00082     if(norm >= (GRAVITY*(1-gravityEpsilon)) && norm<= (GRAVITY*(1+gravityEpsilon)))
00083     {
00084         validGravityCounter++;
00085         
00086         /******After validAccelerationVectorsNecessaryToDetectGravity vectors, we say that the last valid vector is really the gravity and not the gravity + a certain acceleration*********/
00087         if(validGravityCounter == validAccelerationVectorsNecessaryToDetectGravity)
00088         {
00089             lastGravityVectorDetected[0] = acceleration[0];
00090             lastGravityVectorDetected[1] = acceleration[1];
00091             lastGravityVectorDetected[2] = acceleration[2];
00092             
00093             validGravityCounter = 0;
00094         }
00095     }
00096     else
00097         validGravityCounter =0;
00098     
00099     gravity[0] = lastGravityVectorDetected[0];
00100     gravity[1] = lastGravityVectorDetected[1];
00101     gravity[2] = lastGravityVectorDetected[2];
00102     
00103 }
00104 
00105 void Orientation::calculate_new_gravity(float acceleration[3])
00106 {
00107         double gravity_[3];
00108         // Check if the acceleration vector is the gravity
00109         detectGravity(gravity_, acceleration); 
00110 
00111 // If it is, we update out gravity vector and calcule the new angle offset due to gravity. Indeed, the gravity measure give us an error on our orientation if not taken into account.
00112         if(!isEqual(gravity, gravity_)) 
00113         {
00114 
00115                 /*****We save the new gravity******/
00116 
00117                 gravity[0] = gravity_[0]/GRAVITY;
00118                 gravity[1] = gravity_[1]/GRAVITY;
00119                 gravity[2] = gravity_[2]/GRAVITY;
00120 
00121                 /****** This will give us the pitch and the yaw relative to the User axis and not the Device axis at t0
00122                  Calculations are made using the formulation of the rotation matrix*******/
00123                 pitchOffset = gravity[1];
00124                 if(pitchOffset>1.0f)
00125                     pitchOffset = 1.0f;
00126                 if(pitchOffset<-1.0f)
00127                     pitchOffset = -1.0f;
00128 
00129                 pitchOffset = asin(-pitchOffset);
00130 
00131                 rollOffset = gravity[2]/cos(pitchOffset);
00132                 if(rollOffset>1.0f)
00133                     rollOffset = 1.0f;
00134                 if(rollOffset<-1.0f)
00135                     rollOffset = -1.0f;
00136 
00137                 //We substract the actual pitch value from the algorithm
00138                 pitchOffset -= (pitch);
00139                 //We substract the actual roll value from the algorithm
00140                 rollOffset = acos(rollOffset) - (roll);
00141         }
00142 }
00143 
00144 double Orientation::get_roll()
00145 {
00146         return ToRad(roll + rollOffset);
00147 }
00148 
00149 double Orientation::get_pitch()
00150 {
00151         return ToRad(pitch + pitchOffset);
00152 }
00153 
00154 double Orientation::get_yaw()
00155 {
00156         return ToRad(yaw);
00157 }


corobot_phidget_ik
Author(s):
autogenerated on Sun Oct 5 2014 23:18:06