グルグル回る軸の角度変動の平均値を出す方法#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ならメモリー確保成功していると判断で
きます。
| 固定リンク
| コメント (0)
| トラックバック (0)
最近のコメント