グルグル回る軸の角度変動の平均値を出す方法#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ならメモリー確保成功していると判断で
きます。