MultirateLiPh.cpp

00001 //-----------------------------------------------------------
00002 //  出力を 4 倍にアップサンプリングするクラス
00003 //  補間処理で使うフィルタ：直線位相 FIR フィルタ
00004 //  
00005 //  2021/12/22, Copyright (c) 2021 MIKAMI, Naoki
00006 //-----------------------------------------------------------
00007 
00008 #include "MultirateLiPh.hpp"
00009 #include "MultirateLiPhCoefs.hpp"   // デフォルトの補間用フィルタの係数
00010 
00011 namespace Mikami
00012 {
00013     // コンストラクタ（デフォルトの補間フィルタの係数を使う場合）
00014     MultirateLiPh::MultirateLiPh(float fSampling,
00015                                  PinName pin, ADC_TypeDef* const adc)
00016         : indexW_(0), FIR_LOOP_((ORDER_+2)/UR_), CENTER_((ORDER_+2)/(UR_*2)),
00017           vn_((ORDER_+2)/UR_, 0.0f), h1_((ORDER_+2)/UR_, HK1_),
00018           h2_((ORDER_+2)/UR_, HK2_), h3_((ORDER_+2)/UR_, HK3_)
00019     {   Init(fSampling, pin, adc); }
00020 
00021     // コンストラクタ（デフォルト以外の補間フィルタの係数を使う場合）
00022     MultirateLiPh::MultirateLiPh(float fSampling, int order,
00023                                  const float hk1[], const float hk2[],
00024                                  const float hk3[],
00025                                  PinName pin, ADC_TypeDef* const adc)
00026         : indexW_(0), FIR_LOOP_((order+2)/UR_), CENTER_((order+2)/(UR_*2)),
00027           vn_((order+2)/UR_, 0.0f), h1_((order+2)/UR_, hk1),
00028           h2_((order+2)/UR_, hk2), h3_((order+2)/UR_, hk3)
00029     {   Init(fSampling, pin, adc); }
00030 
00031     // 標本化の実行開始
00032     void MultirateLiPh::Start(void (*Func)())
00033     {
00034         // CAN2_TX によるソフトウェア割り込みに対応する設定
00035         NVIC_SetVector(CAN2_TX_IRQn, (uint32_t)Func);
00036         NVIC_EnableIRQ(CAN2_TX_IRQn);       
00037 
00038         // AD 変換器を使うための準備
00039         wait_us(1000);    // ある程度の待ち時間が必要
00040         adc_->SetIntrVec(&MultirateLiPh::AdcIsr);  // AD 変換終了に対応する ISR の設定
00041     }
00042 
00043     // 補間用フィルタを実行し，処理結果を出力用バッファへ書き込む
00044     void MultirateLiPh::Output(float yn)
00045     {
00046         vn_[0] = yn;    // 補間フィルタ用バッファの先頭に書き込む
00047 
00048         buf_[ModIndex(indexW_)] = vn_[CENTER_];
00049         buf_[ModIndex(indexW_)] = Interpolate(h1_);
00050         buf_[ModIndex(indexW_)] = Interpolate(h2_);
00051         buf_[ModIndex(indexW_)] = Interpolate(h3_);
00052 
00053         for (int k=FIR_LOOP_-1; k>0; k--) vn_[k] = vn_[k-1];
00054     }
00055 
00056     // ADC 変換終了割り込みに対する割り込みサービス･ルーチン
00057     void MultirateLiPh::AdcIsr()
00058     {
00059         static int count = 0;
00060 
00061         xn_ = adc_->Read();     // AD変換器の値を読み込む
00062         dac_.Write(buf_[ModIndex(indexR_)]);  // 出力バッファの内容を DAC へ書き込む
00063 
00064         if (count == 0)                 // AD変換器からの入力信号は４回に１回使う
00065             NVIC->STIR = CAN2_TX_IRQn;  // ソフトウェア割込み発生，信号処理を起動
00066         count = ++count & MASK_UR_; // 入力を４回に１回行うための処理
00067     }
00068 
00069     // 補間用 FIR フィルタ
00070     float MultirateLiPh::Interpolate(const float hk[]) const
00071     {
00072         float y = 0;
00073         for (int n=0; n<FIR_LOOP_; n++) y += vn_[n]*hk[n];
00074         return y;
00075     }
00076 
00077     // ADC の初期化と割込み優先順位の設定
00078     void MultirateLiPh::Init(float fSampling, PinName pin, ADC_TypeDef* const adc)
00079     {
00080         NVIC_SetPriority(ADC_IRQn, 0);      // ADC 終了割り込み：最優先
00081         NVIC_SetPriority(CAN2_TX_IRQn, 1);  // ソフトウェア割り込みで使用：２番目に優先
00082 
00083         adc_ = new DspAdcIntr(fSampling*UR_, pin, adc);
00084     }
00085 
00086     // static メンバの実体の宣言/初期化
00087     DspAdcIntr *MultirateLiPh::adc_;     // AD変換器のオブジェクトのポインタ
00088     DspDac MultirateLiPh::dac_;          // DA変換器のオブジェクト
00089     Array<float> MultirateLiPh::buf_(2*UR_, 0.0f);
00090     int MultirateLiPh::indexR_ = UR_;
00091     float MultirateLiPh::xn_;
00092 }