Revision 4:ad48484f3a9e, committed 2020-04-07

Comitter:

xenjamo

Date:

Tue Apr 07 11:05:00 2020 +0000

Parent:

3:7e7159e2f589

Commit message:

now added struct for data saving; use makeRM to create a Rotation matrix

Changed in this revision

--- a/kinematics.cpp	Mon Mar 30 09:22:22 2020 +0000
+++ b/kinematics.cpp	Tue Apr 07 11:05:00 2020 +0000
@@ -11,6 +11,7 @@
 
 #include "kinematics.h"
 #include <math.h>
+#include "mbed.h"
 
 
 //////////////////
@@ -24,7 +25,8 @@
     
     
     
-    accel.reset();
+    acc.reset();
+    lacc.reset();
     vel.reset();
     pos.reset();
 }
@@ -34,31 +36,69 @@
     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){
+void Kinematics::updateAll(double ax, double ay, double az){ //mainly used for testing
     updateTime();
-    accel.set(ax,ay,az);
+    lacc.set(ax,ay,az);
     updateVel();
     updatePos();
     
 }
 
-void Kinematics::updateAll(LinAccel accel_){
+void Kinematics::updateAll(Accel lacc_){
     updateTime();
-    accel = accel_;
+    lacc = lacc_;
     updateVel();
     updatePos();
     
 }
 
 void Kinematics::updateVel(){
-    vel.x += (accel.x/dataRate);
-    vel.y += (accel.y/dataRate);
-    vel.z += (accel.z/dataRate);
+    vel.x += (lacc.x/dataRate);
+    vel.y += (lacc.y/dataRate);
+    vel.z += (lacc.z/dataRate);
 }
 
 void Kinematics::updatePos(){
@@ -68,7 +108,8 @@
 }
 
 void Kinematics::resetAll(){
-    accel.reset();
+    acc.reset();
+    lacc.reset();
     vel.reset();
     pos.reset();    
     timestamp = 0;
@@ -86,20 +127,20 @@
 //Acceleration class//
 //////////////////////
 
-LinAccel::LinAccel(){
+Accel::Accel(){
     reset();
 }
 
-double LinAccel::getabs(){
+double Accel::getabs(){
     return sqrt(x*x+z*z+y*y);
     
 }
 
-void LinAccel::reset(){
+void Accel::reset(){
     set(0.0, 0.0, 0.0);
 }
     
-void LinAccel::set(double ax, double ay, double az){
+void Accel::set(double ax, double ay, double az){
     x = ax; y = ay; z = az;  
 }
 
@@ -188,6 +229,28 @@
 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//
 /////////////////

--- a/kinematics.h	Mon Mar 30 09:22:22 2020 +0000
+++ b/kinematics.h	Tue Apr 07 11:05:00 2020 +0000
@@ -31,16 +31,24 @@
 
 #include <math.h>
 
-#ifndef Kin_H          // no idea what this does
-#define Kin_H          // seems important
+#ifndef Kin_H
+#define Kin_H
+//
+struct kindata{
+    uint32_t time;
+    float pos[3];
+    float vel[3];
+    float quat[4];
+};
+
 
 
 //some classes
 //these classes are used by the kinetics class
 
-class LinAccel{
+class Accel{
 public:
-    LinAccel();
+    Accel();
     double x,y,z;
     double getabs();
     void reset();
@@ -73,12 +81,14 @@
     double r, p, y;
     double qw, qx, qy, qz;
     double grvX, grvY, grvZ;
+    double rm[9];
     void setQuat(double qw_, double qx_, double qy_, double qz_);
     void resetQuat();
     void setEuler(double r_, double p_, double y_);
     void resetEuler();
     void setGRV(double grvX_, double grvY_, double grvZ_);
     void resetGRV();
+    void makeRM();
 };
     
 class RawData{
@@ -102,18 +112,20 @@
 class Kinematics
 {
 public:
-    LinAccel accel;
+    Accel acc;
+    Accel lacc;
     Velocity vel;
     Position pos;
-    Orientation orient;
+    Orientation ori;
     RawData data;
     
     Kinematics(int dataRate);
     double distTraveled;
     int updateTime();
     double uptadeDistTraveled();
+    void transver_vector();
     void updateAll(double ax, double ay, double az);
-    void updateAll(LinAccel accel_);
+    void updateAll(Accel accel_);
     void updateVel();
     void updatePos();
     void resetAll();