Diff: kinematics.cpp

Revision:

3:7e7159e2f589

Parent:

1:79adfaec299a

Child:

4:ad48484f3a9e

--- a/kinematics.cpp	Fri Mar 20 20:17:13 2020 +0000
+++ b/kinematics.cpp	Mon Mar 30 09:22:22 2020 +0000
@@ -41,20 +41,30 @@
 
 void Kinematics::updateAll(double ax, double ay, double az){
     updateTime();
-    
-    // do each integral seperatly in the future
+    accel.set(ax,ay,az);
+    updateVel();
+    updatePos();
     
-    accel.x = ax;
-    accel.y = ay;
-    accel.z = az;
+}
+
+void Kinematics::updateAll(LinAccel accel_){
+    updateTime();
+    accel = accel_;
+    updateVel();
+    updatePos();
     
+}
+
+void Kinematics::updateVel(){
     vel.x += (accel.x/dataRate);
     vel.y += (accel.y/dataRate);
     vel.z += (accel.z/dataRate);
-    
+}
+
+void Kinematics::updatePos(){
     pos.x += vel.x/dataRate;
     pos.y += vel.y/dataRate;
-    pos.z += vel.z/dataRate;
+    pos.z += vel.z/dataRate;  
 }
 
 void Kinematics::resetAll(){
@@ -139,12 +149,82 @@
     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);
+}
+/////////////////
+//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);
+}
+