File content as of revision 4:ad48484f3a9e:

/*
kinematics library
by clemo


go to header file for instructions


*/


#include "kinematics.h"
#include <math.h>
#include "mbed.h"


//////////////////
//Kinetics Class//
//////////////////
Kinematics::Kinematics(int dataRate_){
    
    this->dataRate = dataRate_;
    this->inv_dataRate = 1/dataRate_;
    timestamp = 0;
    
    
    
    acc.reset();
    lacc.reset();
    vel.reset();
    pos.reset();
}

double Kinematics::uptadeDistTraveled(){
    //distTraveled += vel.getabs();
    return distTraveled;
}

void Kinematics::transver_vector(){
    
    ///*** Rotation Matrix approach
    ori.makeRM();
    double ix,iy,iz;
    ix = acc.x;
    iy = acc.y;
    iz = acc.z;
    lacc.x = ix * ori.rm[0] + iy * ori.rm[1] + iz * ori.rm[2];
    lacc.y = ix * ori.rm[3] + iy * ori.rm[4] + iz * ori.rm[5];
    lacc.z = ix * ori.rm[6] + iy * ori.rm[7] + iz * ori.rm[8];
    //*/
    
    /*// vector approach
    double tx,ty,tz;
    double qw,qx,qy,qz;
    qw = ori.qw;
    qx = ori.qx;
    qy = ori.qy;
    qz = ori.qz;
    double ix,iy,iz;
    ix = acc.x;
    iy = acc.y;
    iz = acc.z;   
    
    tx = qy*iz - qz*iy + qw*ix;
    ty = qz*ix - qx*iz + qw*iy;
    tz = qx*iy - qy*ix + qw*iz;
    
    //printf("%f\t%f\t%f\t\n",qw,qx,qy);
    
    lacc.x = ix + (qy+qy)*tz - (qz+qz)*ty;
    lacc.y = iy + (qz+qz)*tx - (qx+qx)*tz;
    lacc.z = iz + (qx+qx)*ty - (qy+qy)*tx;
    */
}


int Kinematics::updateTime(){
    timestamp++;
    return timestamp;
}

void Kinematics::updateAll(double ax, double ay, double az){ //mainly used for testing
    updateTime();
    lacc.set(ax,ay,az);
    updateVel();
    updatePos();
    
}

void Kinematics::updateAll(Accel lacc_){
    updateTime();
    lacc = lacc_;
    updateVel();
    updatePos();
    
}

void Kinematics::updateVel(){
    vel.x += (lacc.x/dataRate);
    vel.y += (lacc.y/dataRate);
    vel.z += (lacc.z/dataRate);
}

void Kinematics::updatePos(){
    pos.x += vel.x/dataRate;
    pos.y += vel.y/dataRate;
    pos.z += vel.z/dataRate;  
}

void Kinematics::resetAll(){
    acc.reset();
    lacc.reset();
    vel.reset();
    pos.reset();    
    timestamp = 0;
}

int Kinematics::getTime(){
    return timestamp;
}

double Kinematics::getTimeSec(){
    return timestamp/dataRate;
}

//////////////////////
//Acceleration class//
//////////////////////

Accel::Accel(){
    reset();
}

double Accel::getabs(){
    return sqrt(x*x+z*z+y*y);
    
}

void Accel::reset(){
    set(0.0, 0.0, 0.0);
}
    
void Accel::set(double ax, double ay, double az){
    x = ax; y = ay; z = az;  
}

//////////////////
//Velocity class//
//////////////////

Velocity::Velocity(){
    reset();
}

double Velocity::getabs(){
    return sqrt(x*x+z*z+y*y);
}

void Velocity::reset(){
    set(0.0, 0.0, 0.0);
}
    
void Velocity::set(double vx, double vy, double vz){
    x = vx; y = vy; z = vz;  
}

//////////////////
//Position class//
//////////////////
Position::Position(){
    reset();
}

double Position::getabs(){
    return sqrt(x*x+z*z+y*y);
}

double Position::getDistTo(double px, double py, double pz){
    double dx = px-x;
    double dy = py-y;
    double dz = pz-z;
    return sqrt(dx*dx+dy*dy+dz*dz);
}

void Position::reset(){
    set(0.0, 0.0, 0.0);
}
    
void Position::set(double px, double py, double pz){
    x = px; y = py; z = pz;  
}

/////////////////////
//Orientation class//
/////////////////////

Orientation::Orientation(){
    resetQuat();
    resetEuler();
}

void Orientation::setQuat(double qw_, double qx_, double qy_, double qz_){
    qw = qw_;
    qx = qx_;
    qy = qy_;
    qz = qz_;
}   
 
void Orientation::resetQuat(){
    setQuat(0,0,0,0);
}

void Orientation::setEuler(double r_, double p_, double y_){
    r = r_;
    p = p_;
    y = y_;
}

void Orientation::resetEuler(){
    setEuler(0,0,0);
}

void Orientation::setGRV(double grvX_, double grvY_, double grvZ_){
    grvX = grvX_;
    grvY = grvY_;
    grvZ = grvZ_;
}

void Orientation::resetGRV(){
    setGRV(0,0,0);
}

void Orientation::makeRM(){
    /*****************
    rotationmatrix structure:
     /rm[0] rm[1] rm[2]\
    ( rm[3] rm[4] rm[5] )
     \rm[9] rm[7] rm[8]/
     
    ******************/
    double a=qw,b=qx,c=qy,d=qz;
    rm[0]=a*a+b*b-c*c-d*d;
    rm[1]=2*b*c-2*a*d;
    rm[2]=2*b*d+2*a*c;
    rm[3]=2*b*c+2*a*d;
    rm[4]=a*a-b*b+c*c-d*d;
    rm[5]=2*c*d-2*a*b;
    rm[6]=2*b*d-2*a*c;
    rm[7]=2*c*d+2*a*b;
    rm[8]=a*a-b*b-c*c+d*d;
}


/////////////////
//RawData class//
/////////////////

RawData::RawData(){
    ax = 0;
    ay = 0;
    az = 0;
    resetMag();
}
void RawData::setMag(double mx_, double my_, double mz_){
    mx = mx_;
    my = my_;
    mz = mz_;
}

void RawData::resetMag(){
    setMag(0,0,0);
}

void RawData::setA(double ax_, double ay_, double az_){
    ax = ax_;
    ay = ay_;
    az = az_;
}

void RawData::resetA(){
    setA(0,0,0);
}

void RawData::setG(double gx_, double gy_, double gz_){
    gx = gx_;
    gy = gy_;
    gz = gz_;
}

void RawData::resetG(){
    setG(0,0,0);
}