Index: trunk/Mars/mcore/DrsCalib.h
===================================================================
--- trunk/Mars/mcore/DrsCalib.h	(revision 14095)
+++ trunk/Mars/mcore/DrsCalib.h	(revision 14101)
@@ -208,4 +208,91 @@
             vec[i] = double(v)*scalegain/div;
         }
+    }
+
+    static double FindStep(const size_t ch0, const float *vec, int16_t roi, const int16_t pos, const uint16_t *map)
+    {
+        if (pos<=1 || pos>=roi)
+            return 0;
+
+        double step = 0; // before
+
+        // Exclude TM channel
+        for (int p=0; p<8; p++)
+        {
+            const size_t hw = ch0+p;
+            const size_t sw = map[hw]*roi + pos;
+
+            step += vec[sw]-vec[sw-1];
+        }
+
+        return step/8;
+    }
+
+    static void SubtractStep(const size_t ch0, const double avg, float *vec, int16_t roi, uint32_t dist, const uint16_t *map)
+    {
+        const int begin  = avg>0 ? dist :    0;
+        const int end    = avg>0 ?  roi : dist;
+
+        const double sub = fabs(avg);
+
+        for (int p=0; p<9; p++)
+        {
+            for (int j=begin; j<end; j++)
+            {
+                const size_t hw = ch0+p;
+                const size_t sw = map[hw]*roi + j;
+
+                vec[sw] -= sub;
+            }
+        }
+    }
+
+    struct Step
+    {
+        Step() : avg(0), rms(0), pos(0), cnt(0) { }
+        double   avg;
+        double   rms;
+        double   pos;
+        uint16_t cnt;
+    };
+
+    static Step CorrectStep(float *vec, uint16_t nch, uint16_t roi,
+                            const int16_t *prev, const int16_t *start,
+                            const int16_t offset, const uint16_t *map)
+    {
+        Step rc;
+
+        for (size_t ch=0; ch<nch; ch += 9)
+        {
+            if (prev[ch]<0 || start[ch]<0)
+                continue;
+
+            const int16_t dist = (prev[ch]-start[ch]+1024+offset)%1024;
+            const double  step = FindStep(ch, vec, roi, dist, map);
+            if (step==0)
+                continue;
+
+            rc.pos += dist;
+            rc.avg += step;
+            rc.rms += step*step;
+            rc.cnt++;
+        }
+
+        if (rc.cnt==0 || rc.avg==0)
+            return Step();
+
+        rc.pos /= rc.cnt;
+        rc.avg /= rc.cnt;
+        rc.rms /= rc.cnt;
+
+        rc.rms = sqrt(rc.rms-rc.avg*rc.avg);
+
+        for (size_t ch=0; ch<nch; ch += 9)
+        {
+            const int16_t dist = (prev[ch]-start[ch]+1024+offset)%1024;
+            SubtractStep(ch, rc.avg, vec, roi, dist, map);
+        }
+
+        return rc;
     }