Changeset 555 in Sophya for trunk/Poubelle/archTOI.old/gyrocalibrator.cc

Timestamp:

Nov 9, 1999, 3:04:05 PM (26 years ago)

Author:

ansari

Message:

portage cxx en cours

File:

• trunk/Poubelle/archTOI.old/gyrocalibrator.cc (modified) (2 diffs)

trunk/Poubelle/archTOI.old/gyrocalibrator.cc

@@ -4 +4 @@
 // assumption : same calibration for all gyros...
 
+// Warning, current implementation can only output ONE calibration.
+// workaround : clone the object for different options.
+
 
 #include "gyrocalibrator.h"
+#include "archexc.h"
+#include "archparam.h"
 
 #define gyroCal    "gyroCal"
+#define gyroOffset "gyroOffset"
 #define gyroSpeed  "gyroSpeed"
 
 GyroCalibrator::GyroCalibrator() {
-  possibleTOIs.insert(TOI(gyroCal,      TOI::unspec, "linearCal", "deg/s/V"));  
+  possibleTOIs.insert(TOI(gyroCal,      TOI::all,    "linearCal", "deg/s/V"));  
+  possibleTOIs.insert(TOI(gyroOffset,   TOI::all,    "linearCal", "deg/s/V"));  
   possibleTOIs.insert(TOI(gyroSpeed,    TOI::all   , "linearCal", "deg/s"));  
   
-  //needAfter = 3600;  // about one turn...
+  needAfter = 3600;  // about one turn...
   startSample = -1;
+  lastRotSpeed = -1;
   lastFence1 = lastFence2 = -1;
+  for (int i=0; i<3; i++) {
+    lastCalib[i] = -99999;
+    lastOffset[i] = 0;
+  }
+  gyroProducer = NULL;
 }
 
@@ -35 +48 @@
 }
 
-void GyroCalibrator::dataFeed(TOIProducer* source, TOI const& toi, long sampleNum, double value) {
-  if (toi.name != "gyroV" || toi.index != 2) return;
+void GyroCalibrator::dataFeed(TOIProducer* , TOI const& toi, long sampleNum, double value) {
+ // if (toi.name != "gyroV" || toi.index != 2) return;
+  if (toi.name != "gyroV" ) return;
   
-  if (gyro2.empty()) {
+  if (gyro[2].empty()) {
     startSample = sampleNum;
   }
   
-  gyro2.push_back(value);
+  gyro[toi.index].push_back(value);
+}
+
+void GyroCalibrator::recomputeCalib() {
+  // Integral of gyro signal between the two fences.
+  
+  // can we do it ?
+  for (int i=0; i<3; i++) {
+    lastCalib[i] = -99999;
+  }
+  if (startSample > lastFence1) return;
+  if ((long)gyro[2].size()+startSample < lastFence2) return;
+        
+  double sum[3];
+  for (int i=0; i<3; i++) {
+    sum[i] = 0;
+  }
+  vector<double>::iterator i0     = gyro[0].begin() + (lastFence1-startSample);
+  vector<double>::iterator i1     = gyro[1].begin() + (lastFence1-startSample);
+  vector<double>::iterator i2     = gyro[2].begin() + (lastFence1-startSample);
+  vector<double>::iterator stop2  = gyro[2].begin() + (lastFence2-startSample);
+  for (; i2 != stop2; i0++,i1++,i2++) {
+    sum[0] += *i0;
+    sum[1] += *i1;
+    sum[2] += *i2;
+  }
+    
+  // average of signal
+  sum[0] /= (lastFence2-lastFence1);
+  sum[1] /= (lastFence2-lastFence1);
+  sum[2] /= (lastFence2-lastFence1);
+  
+  lastOffset[0] = sum[0];
+  lastOffset[1] = sum[1];
+  lastOffset[2] = (lastOffset[0] + lastOffset[1])/2.;  // We don't have a better estimate...
+  
+  lastCalib[2]  = lastRotSpeed / (sum[2] - lastOffset[2]);
+  lastCalib[0] = lastCalib[1] = lastCalib[2];          // We don't have a better estimate...
+}
+
+bool GyroCalibrator::fetchFences(long sampleNum) {
+  map<TOI, TOIProducer*> & m = (*(neededTOIs.begin())).second;
+  
+  long oldf2 = lastFence2;
+  for (map<TOI, TOIProducer*>::iterator i = m.begin(); i != m.end(); i++) {
+    TOI const& inToi = (*i).first;
+    TOIProducer* prod =  (*i).second;
+    if (inToi.name == "rotSpeed")        lastRotSpeed = prod->getValue(sampleNum, inToi);
+    if (inToi.name == "rotSpeedSample1") lastFence1   = prod->getValue(sampleNum, inToi);
+    if (inToi.name == "rotSpeedSample2") lastFence2   = prod->getValue(sampleNum, inToi);
+    if (inToi.name == "gyroV")           gyroProducer = prod;
+  }
+  
+  return (oldf2 != lastFence2);
 }
 
 
+bool GyroCalibrator::canGetValue(long sampleNum, TOI const& ) {
+  // We can get value if sampleNum is between the two fences, or
+  // if a new fence later than sampleNum is now available.
+  // In any case, we must have gyro data up to the highest fence.
+  
+  if (startSample > lastFence1) return false; // will never work...
+  if (sampleNum >= lastFence1 && sampleNum < lastFence2 && (gyro[2].size()+startSample >= lastFence2)) return true;
+  if ((long)gyro[2].size()+startSample < lastFence2) return false; // We have to wait for more gyro data
+  if (fetchFences(sampleNum)) {
+    recomputeCalib(); // do it now to keep a consistent state
+  }
+  if (sampleNum >= lastFence1 && sampleNum < lastFence2) {
+    return true;
+  } else {
+    return false;
+  }  
+}
 
+bool GyroCalibrator::canGetValueLater(long sampleNum, TOI const& ) {
+  if (sampleNum >= lastFence1 && sampleNum < lastFence2 && ((long)gyro[2].size()+startSample >= lastFence2)) return false;
+      // because can get now
+  if (sampleNum >= lastFence2) { // check if new fence...
+    if (fetchFences(sampleNum)) {
+      recomputeCalib(); // do it now to keep a consistent state
+    }
+  }
+  if (lastFence1<0 || lastFence2<0) return true;
+  if (sampleNum > lastFence2) return true;
+  return (startSample <= lastFence1 && (long)gyro[2].size()+startSample < lastFence2 && sampleNum > lastFence1);
+}
+
+double GyroCalibrator::getValue(long sampleNum, TOI const& toi) {
+  if (startSample > lastFence1) return -1; // will never work...
+  if ((long)gyro[2].size()+startSample < lastFence2) return -1; // We have to wait for more gyro data
+  if (!(sampleNum >= lastFence1 && sampleNum < lastFence2)) {
+    if (fetchFences(sampleNum)) {
+      if ((long)gyro[2].size()+startSample < lastFence2) return -1; // We have to wait for more gyro data
+      recomputeCalib(); // do it now to keep a consistent state
+    }
+  }
+  
+  if (!(sampleNum >= lastFence1 && sampleNum < lastFence2)) 
+    return -1;
+    
+  if (lastCalib[0] < 0) recomputeCalib();
+    
+  if (toi.name == gyroCal)    return lastCalib[toi.index];
+  if (toi.name == gyroOffset) return lastOffset[toi.index];
+  if (toi.name == gyroSpeed) {
+    TOI toi2 = TOI("gyroV", toi.index); // $CHECK$ maybe should get from reqtoi ?
+    double gv = gyroProducer->getValue(sampleNum, toi2);
+    return (gv-lastOffset[toi.index])*lastCalib[toi.index];
+  }
+  throw ArchExc("Cannot produce "+toi.fullName());
+}
+
+
+void GyroCalibrator::propagateLowBound(TOI const&, long /*sampleNum*/) {
+   if (startSample < lastFence1) {
+     if (gyro[0].size() > lastFence1 - startSample) {
+       for (int i=0; i<3; i++) {
+         vector<double>::iterator x = gyro[i].begin() + lastFence1 - startSample;
+         gyro[i].erase(gyro[i].begin(), x);
+       }
+       startSample = lastFence1;
+     }
+   }
+}
+