« 2月の撃墜数 | トップページ | お雛さま・・・また来年 »

2017年2月26日 (日)

グルグル回る軸の角度変動の平均値を出す方法#4

回転角度の平均値計算
風向の平均計算

グルグル回る軸の角度変動の平均値を出したい
グルグル回る軸の角度変動の平均値を出す方法
グルグル回る軸の角度変動の平均値を出す方法#2
グルグル回る軸の角度変動の平均値を出す方法#3 

これのまとめです。

ひとつのルーチンで、いくつもデータを扱えるようにしてみました。
これまでのは、平均処理したい角度がいくつもある場合、
それぞれの値に対して関数を作らなくちゃなりませんでした。

これも面倒なんで、移動平均処理するそれぞれのデータを
置いておく場所を「構造体」で記述し、一つの関数でこれを
処理できるようにしました。
構造体はそれぞれの角度ごとに必要ですが、それを処理
する関数は一つだけ。
入力データ値と構造体名、それに平均する回数を指定して
関数を呼び出します。

こんな具合・・・・

//    BYTEとWORD
typedef unsigned char    BYTE;
typedef unsigned short   WORD;
#define PAI   3.141592654  // πの値

/***** 四捨五入    *****/
//   正負で±0.5して整数に変換
short rounds(float d)
{
  if(d >= 0){
    return (short)(d + 0.5);
  }else{
    return (short)(d - 0.5);
  }
}

/***** 360度1回転規制   *****/
//   0~359度を得る
WORD lim359(short d)
{
  if(d < 0){       // マイナスかもしれない
    d = -d;       // 正の数に
    d = d % 360;    // 0~359に
    d = 360 - d;    // 360から減算
  }
  d = d % 360;      // 0~359に
  return d;
}

/***** 角度スムージング処理  *****/
// 計算のための構造体
typedef struct{
  short  *bfx;      // Xスムージングバッファ
  short  *bfy;      // Y
  long  adx;      // X合計加算値
  long  ady;      // Y
  short  p;       // データポインタ
  short  f1;       // 初回チェックフラグ
}SMZ_359;

/***** 角度スムージング処理  *****/
// 浮動小数点計算での誤差を累積ささないため
// X,Yを10000倍したwordデータで計算する
// sin,cos ±1.0が最大なので2バイトで足りる
// deg : 0~359 入力角度
// smz : スムージングバッファ 構造体
// size: スムージング処理加算回数=バッファの大きさ 
WORD smz359c(short deg, SMZ_359 *smz, const WORD size)
{
short *px, *py;
short i, x, y;
float d, e;
  PH3_H;         // (!!!)PH3 17pin H/L
  d = (PAI / 180.0) * (float)(lim359(deg));  // 0~2πに
  x = rounds(cos(d) * 10000.0);    // X,Yの値
  y = rounds(sin(d) * 10000.0);    // 10000倍して整数で
  if(smz->f1 == 0){          // 初めて
    smz->bfx = malloc(sizeof(short) * size); // バッファ確保
    smz->bfy = malloc(sizeof(short) * size);
    if((smz->bfx == NULL) ||         // メモリ確保失敗なら
      (smz->bfy == NULL))   return deg; // 元数値を持ってリターン
    smz->adx = (long)x * size;   // X,Y合計加算値
    smz->ady = (long)y * size;
    px = smz->bfx;         // バッファの先頭
    py = smz->bfy;
    for(i = 0; i < size; i++){   // バッファを初期値で埋める
      *px++ = x;
      *py++ = y;
    }
    smz->p = 0;          // データポインタ先頭に
    smz->f1 = 1;          // 初回処理終了
  }
  else{                // 2回目以降
    px = smz->bfx + smz->p;
    py = smz->bfy + smz->p;
    smz->adx = smz->adx - *px + x; // 古い値を引いて
    smz->ady = smz->ady - *py + y; // 新値を加算
    *px = x;            // 新値をセット
    *py = y;
    smz->p++;            // ポインタを後ろに
    if(smz->p >= size) smz->p = 0; // 最後まで来たらゼロに
  }
  e = 10000.0 * (float)size;     // スケーリング
  d = (180.0 / PAI) *         // 角度に
     atan2((float)smz->ady / e,  // X,Y平均値
        (float)smz->adx / e);
  PH3_L;               // (!!!)PH3 17pin H/L
  return lim359(rounds(d));      // 0~359
}

// ★使用例
BYTE f_tm10ms;    // A/D値が10mSごとに確定 (0/1)
WORD ad_deg;     // A/D値から算出した角度データ (0~359度)
SMZ_359 smz_deg;   // 角度データ平均用配列:構造体
    :
    :
// 1秒ごとに平均角度を出力
  if(f_tm10ms){  // 10mSごとにA/D値が確定
    printf("%3d deg\n",           // 3桁で出力
        smz359c(ad_deg, &smz_deg, 100)); // スムージング処理
    f_10ms = 0;
    :
  }

※全角スペースを使ってます。

移動平均データを置いておく配列は「malloc」でメモリーを確保。
メモリー不足だった場合は平均処理は行わず、元データを持って
リターンするようにしています。

メモリー不足のエラーは出していません。
smz.bfx、smz.bfyがNULLじゃないことを、この関数実行後に
別場所で調べればと。
あるいは、smz.f1が1ならメモリー確保成功していると判断で
きます。

|

« 2月の撃墜数 | トップページ | お雛さま・・・また来年 »

電子回路工作」カテゴリの記事

コメント

コメントを書く



(ウェブ上には掲載しません)




トラックバック


この記事へのトラックバック一覧です: グルグル回る軸の角度変動の平均値を出す方法#4:

« 2月の撃墜数 | トップページ | お雛さま・・・また来年 »