GmMfamFilter.hpp

#ifndef GM_MFAM_FILTER_HPP_
#define GM_MFAM_FILTER_HPP_
 
#ifndef GEO_MFAM_FULL_FILTER
#error GmMfamFilter.hpp should only be included if the feature is enabled.
#endif
 
#include <stdint.h>
#include <geotypes.hpp>
 
#ifndef GMMFAMDATA_HPP_
#include <GmMfamData.hpp>
#endif
 
 
namespace Geometrics {
 
class MfamLokiInterpolator;
 
#if GEO_MFAM_FULL_FILTER == GEO_TRUE
 
 
   /******************************************************************************
    *  Filter names.  There are as many as 12 filters active at one point, for the
    * mag data and for the aux fields.  They're all active at the same point, all
    * in sync, all emit results at the same time, etc., etc., so they can all
    * share some common structure and data.  Each of those filters must have its
    * own accumulators, of course. These filter names are indexes into various
    * structures.
   ***************************************************************************** */
    #define MFAM_MAG1_FILTER        0
    #define MFAM_MAG2_FILTER        1
    #define MFAM_COUNT_STAGE_1_FILTERS (MFAM_MAG2_FILTER + 1)
    #define MFAM_GYRO_X_FILTER      2
    #define MFAM_GYRO_Y_FILTER      3
    #define MFAM_GYRO_Z_FILTER      4
    #define MFAM_COMPASS_X_FILTER   5
    #define MFAM_COMPASS_Y_FILTER   6
    #define MFAM_COMPASS_Z_FILTER   7
    #define MFAM_ACCEL_X_FILTER     8
    #define MFAM_ACCEL_Y_FILTER     9
    #define MFAM_ACCEL_Z_FILTER     10
    #define MFAM_TEMP_FILTER        11
    #define MFAM_COUNT_STAGE_2_FILTERS  (MFAM_TEMP_FILTER + 1)
 
 
   //#define MFAM_FILTER_1000_TO_100_HALF_LENGTH 25
   /******************************************************************************
    * GmFilterDef describes the filter structure, per individual filter.  It is
    * not the description of the group of filters.
   *******************************************************************************/
   struct GmFilterDef {
       public:
       uint32_t mStartHz,   // e.g. 1000
                mStageStartHz,  // e.g. 1000 or 100
                mStageTargetHz,     // e.g. 100
                mTargetHz, // After all stages.
                mStage, // 1, 2
                mCountHalfTaps, // e.g. 249
             //   mSizeTaps,
                fidStepSize;  // e.g. 4 or 8, for single or double,
       const double  *mHalfTaps;
   };
 
   /*******************************************************************
    * GmMfamFullFilter is the group of filters - mag values, aux
    * values, GPS and other values, etc.  It includes accumulators for
    * each of those values, all of which operate in lock step with
    * each other.  All of the individual filters in a GmMfamFullFilter
    * share the same GmFilterDef.
   *********************************************************************/
   class GmMfamFullFilter {
   public:
      GmMfamFullFilter          ();
      virtual ~GmMfamFullFilter ();
      void   Initialize         ();
      bool   ClearFilters       ();
      bool   StartFilter        ();
      bool   IsAvailable        (uint32_t          startRate,
                                 uint32_t          targetRate);
 
      /**************************************************************
       * Choose the filter def from among the available filters,
       * and do some setup calculations.
      ***************************************************************/
      bool   SelectFilter    (uint32_t          startRate,
                              uint32_t          targetRate,
                              uint32_t          stage,
                              bool              onlyMagValues);
      void   SetNextStage    (GmMfamFullFilter *nextStage);
 
      /**************************************************************
       * Add values to all of the filters in this group (newValues is
       * a structure with many values).  Return values that verify
       * that the filter worked properly, and also specifying whether
       * this action resulted in filter values being emitted.
      **************************************************************/
      int    AddValues       (int               fid,
                              uint64_t          tezzeretCounter,
                              IndexedMfamFilteredPacket *newValues);
      static GmFilterDef      sFilterDefs[];
 
      static int FilterMfamData (IndexedMfamSpiPacketWithHeader      *pimsphData,
                                 IndexedMfamFilteredPacketWithHeader &newPacket,
                                 GmMfamFullFilter                    *mfamStageOneFilter,
                                 GmMfamFullFilter                    *mfamStageTwoFilter,
                                 MfamLokiInterpolator                *lokiInterpolator,
                                 ti_sysbios_knl_Semaphore_Handle      decimationSemaphore,
                                 bool                                &recordEmitted);
 
     // static GmFilterDef      *sCurrentDef;
 
   private:
 
      double      *MakeAccumulators (int count);
      bool         MakeCenterRecords ();
      bool         MakeValueIndexes  ();
      void         CleanAccumulators (int accumulatorIndex);
 
 
      int               mCountAccumulatorsPerFilter,
                        mStep,                           // How far apart are accumulators?
                        mLastValueIndex,                 // how many taps, minus one.
                        mModulusOfFirstValue,            // How do we know what is first value?
                        mCenterCounterOffset,
                        mFirstFilter;                    // Which accumulator is most full
      bool              //mStarted,
                        mOnlyMagValues;
      double           *mAccumulators;
      GmMfamFullFilter *mNextStage;
   public:
      GmFilterDef      *mDef;
      uint64_t         *mCenterRecordCounters;
      int              *mValueIndexes,
                        mLastCompleteAccumulator;        // Which just emitted a value?
      IndexedMfamFilteredPacket *mCenterRecords;
 
 
   };
 
   class MfamLokiInterpolator
   {
   public:
       IndexedMfamFilteredPacket mStashedRecord,
                                 mFilterRecords[10];
       void Initialize          ();
       void AddRecord           (IndexedMfamFilteredPacket *newRecord, int index);
       void InterpolateAccelValues (int index);
       void InterpolateGyroValues  (int index);
       void InterpolateCompassValues (int index);
       void InterpolateAuxValues(int index);
 
 
 
   };
#endif
 
   #define RAW_FREQUENCY_DATA_BUFFER_COUNT (200)
 
   #if GEO_MFAM_FREQUENCY_MEASURE == GEO_TRUE
   class GmMfamFrequencyDetector {
   public:
      GmMfamFrequencyDetector (int sampleRate, int samplesMeasured, double frequencyToMeasure);
      void Initialize ();
      int SetDownsampleRate (int downsampleRate);
      int AddValue          (double newValue);
      double CalculateFrequencyResponse ();
      void   ResetCache ();
 
   private:
      int mDownsampleRate;
      int mSampleRate;
      int mSamplesMeasured;
      int mBinNumber;
      double mFrequencyToMeasure;
      double mRealWeight, mImaginaryWeight;
   };
   #endif
 
}
#endif