Changeset 198 in flair-src

Timestamp:

Sep 14, 2017, 3:52:29 PM (7 years ago)

Author:

Sanahuja Guillaume

Message:

modifs imu

Location:

trunk/lib

Files:

• FlairFilter/src/AhrsComplementaryFilter.cpp (modified) (3 diffs)
• FlairFilter/src/AhrsComplementaryFilter.h (modified) (2 diffs)
• FlairSensorActuator/src/Imu.cpp (modified) (5 diffs)
• FlairSensorActuator/src/Imu.h (modified) (1 diff)
• FlairSensorActuator/src/SimuImu.cpp (modified) (2 diffs)

trunk/lib/FlairFilter/src/AhrsComplementaryFilter.cpp

@@ -20 +20 @@
 #include <Imu.h>
 #include <DoubleSpinBox.h>
+#include <Vector3DSpinBox.h>
 #include <GridLayout.h>
 #include <ImuData.h>
@@ -36 +37 @@
 AhrsComplementaryFilter::AhrsComplementaryFilter(const Imu* parent,string name): isInit(false), Ahrs(parent,name) {
 
-    QHat.q0=1;
-    QHat.q1=0;
-    QHat.q2=0;
-    QHat.q3=0;
-    BHat.x=0;
-    BHat.y=0;
-    BHat.z=0;
+  QHat.q0=1;
+  QHat.q1=0;
+  QHat.q2=0;
+  QHat.q3=0;
+  BHat.x=0;
+  BHat.y=0;
+  BHat.z=0;
 
-    ka[0]=new DoubleSpinBox(parent->GetLayout()->NewRow(),"ka[0]:",0.,10,0.1,2,0.5);
-    ka[1]=new DoubleSpinBox(parent->GetLayout()->LastRowLastCol(),"ka[1]:",0.,10,0.1,2,0.5);
-    ka[2]=new DoubleSpinBox(parent->GetLayout()->LastRowLastCol(),"ka[2]:",0.,10.,0.1,2,0.);
+  ka[0]=new DoubleSpinBox(parent->GetLayout()->NewRow(),"ka[0]:",0.,10,0.1,2,0.5);
+  ka[1]=new DoubleSpinBox(parent->GetLayout()->LastRowLastCol(),"ka[1]:",0.,10,0.1,2,0.5);
+  ka[2]=new DoubleSpinBox(parent->GetLayout()->LastRowLastCol(),"ka[2]:",0.,10.,0.1,2,0.);
 
-    kb[0]=new DoubleSpinBox(parent->GetLayout()->NewRow(),"kb[0]:",0.,10,0.1,2,0.01);
-    kb[1]=new DoubleSpinBox(parent->GetLayout()->LastRowLastCol(),"kb[1]:",0.,10,0.1,2,0.01);
-    kb[2]=new DoubleSpinBox(parent->GetLayout()->LastRowLastCol(),"kb[2]:",0.,10,0.1,2,0.01);
-    
-    km[0]=new DoubleSpinBox(parent->GetLayout()->NewRow(),"km[0]:",0.,10,0.1,2,0.01);
-    km[1]=new DoubleSpinBox(parent->GetLayout()->LastRowLastCol(),"km[1]:",0.,10,0.1,2,0.01);
-    km[2]=new DoubleSpinBox(parent->GetLayout()->LastRowLastCol(),"km[2]:",0.,10,0.1,2,0.01);
+  kb[0]=new DoubleSpinBox(parent->GetLayout()->NewRow(),"kb[0]:",0.,10,0.1,2,0.01);
+  kb[1]=new DoubleSpinBox(parent->GetLayout()->LastRowLastCol(),"kb[1]:",0.,10,0.1,2,0.01);
+  kb[2]=new DoubleSpinBox(parent->GetLayout()->LastRowLastCol(),"kb[2]:",0.,10,0.1,2,0.01);
+
+  //put km to [0,0,0] to disable magnetometers
+  km[0]=new DoubleSpinBox(parent->GetLayout()->NewRow(),"km[0]:",0.,50,0.1,2,0.01);
+  km[1]=new DoubleSpinBox(parent->GetLayout()->LastRowLastCol(),"km[1]:",0.,50,0.1,2,0.01);
+  km[2]=new DoubleSpinBox(parent->GetLayout()->LastRowLastCol(),"km[2]:",0.,50,0.1,2,0.01);
   
+  magRef=new Vector3DSpinBox(parent->GetLayout()->NewRow(),"ref magnetic field (uT)",
+                                 0, 100, 1,
+                                 3,Vector3Df(20.5324,.2367,43.6682)); //Compiegne, France
   SetIsReady(true);
 }
@@ -82 +87 @@
     //float kb[3]={0.01,0.01,0.01};
     //float k_m[3]= {0,0,0};
-magRef.x=20.8296;
-magRef.y=.2095;
-magRef.z=43.3333;
+    magRef=this->magRef->Value();
 
     if(isInit==true) {

trunk/lib/FlairFilter/src/AhrsComplementaryFilter.h

@@ -24 +24 @@
     namespace gui {
         class DoubleSpinBox;
+        class Vector3DSpinBox;
     }
 }
@@ -70 +71 @@
             gui::DoubleSpinBox *kb[3];
             gui::DoubleSpinBox *km[3];
+            gui::Vector3DSpinBox *magRef;
     };
 } // end namespace filter

trunk/lib/FlairSensorActuator/src/Imu.cpp

@@ -33 +33 @@
 namespace sensor {
 
-Imu::Imu(string name) : IODevice(getFrameworkManager(), name) {
+Imu::Imu(string name,bool needRotation) : IODevice(getFrameworkManager(), name) {
   imuData = new ImuData(this);
 
@@ -41 +41 @@
   sensorTab = new Tab(tab, "Reglages");
   setupGroupbox = new GroupBox(sensorTab->NewRow(), name);
-  rotation = new OneAxisRotation(sensorTab->NewRow(), "post rotation",OneAxisRotation::PostRotation);
+  if(needRotation) {
+    rotation = new OneAxisRotation(sensorTab->NewRow(), "post rotation",OneAxisRotation::PostRotation);
+  } else {
+    rotation=NULL;
+  }
   AddDataToLog(imuData);
 }
@@ -66 +70 @@
 
 OneAxisRotation *Imu::GetOneAxisRotation(void) const {
+  if (rotation == NULL) {
+    Err("not applicable\n");
+  }
   return rotation;
 }
@@ -71 +78 @@
 void Imu::ApplyRotation(Vector3Df& vector) {
   if (rotation == NULL) {
-    Err("not applicable for simulation part.\n");
+    Err("not applicable\n");
     return;
   }
@@ -79 +86 @@
 void Imu::ApplyRotation(Quaternion& quaternion) {
   if (rotation == NULL) {
-    Err("not applicable for simulation part.\n");
+    Err("not applicable\n");
     return;
   }

trunk/lib/FlairSensorActuator/src/Imu.h

@@ -53 +53 @@
   *
   * \param name name
+  * \param needRotation true will enable post rotation in GCS. Post rotation must be applied manually in reimplemented code
   */
-  Imu(std::string name);
+  Imu(std::string name,bool needRotation=true);
 
   /*!

trunk/lib/FlairSensorActuator/src/SimuImu.cpp

@@ -36 +36 @@
 SimuImu::SimuImu(string name, uint32_t modelId,uint32_t deviceId,
                  uint8_t priority)
-    : Imu(name), Thread(getFrameworkManager(), name, priority) {
+    : Imu(name,false), Thread(getFrameworkManager(), name, priority) {
   dataRate =
       new SpinBox(GetGroupBox()->NewRow(), "data rate", " Hz", 1, 500, 1, 200);
@@ -117 +117 @@
     Vector3Df rawMag(state.mx, state.my, state.mz);
     Vector3Df rawGyr(state.wx, state.wy, state.wz);
+    //we do not need rotation in simulation
+    /*
     ApplyRotation(angRate);
     ApplyRotation(quaternion);
     ApplyRotation(rawAcc);
     ApplyRotation(rawMag);
-    ApplyRotation(rawGyr);
+    ApplyRotation(rawGyr);*/
     ahrsData->SetQuaternionAndAngularRates(quaternion,angRate);
     imuData->SetRawAccMagAndGyr(rawAcc,rawMag,rawGyr);