File content as of revision 17:f9610f3cfa1b:

#include "HbAttitude.h"

//=========================================================
//PID制御
//=========================================================
float HbAttitude::pid(float iCmdAng, float iCurAng , float iRate)
{
    //エラー量：指令値との差を求める
    float errAng = iCmdAng - iCurAng;
    //アウターループのPゲインを掛けて目標角速度とする
    float cmdRate= errAng * p;
    
    //▼角速度偏差（指令値と現在値との差）
    float devRate = cmdRate - iRate ;

    //▼比例項
    float clcP = devRate * kp; 
    
    //▼積分項
    float xi = ki * devRate;  //係数をかける
    float tmpInteg = sum + xi;//積分して

    //リミット掛ける
    if(tmpInteg > limitH){tmpInteg = limitH;}
    if(tmpInteg < limitL){tmpInteg = limitL;}

    //積分値を次回計算用に保存
    sum = tmpInteg;
    
    //▼微分項
    float clcD = kd * (devRate - old);
    //過去データ書き換え
    old = devRate;

    return  clcP + tmpInteg + clcD;
}

//=========================================================
//コンストラクタ
//=========================================================
HbAttitude::HbAttitude(float iPo , float iP , float iI , float iD){
    //パラメータ初期化
    p     =iPo ;//アウターループP制御系数
    kp    =iP  ;//インナーループP制御系数
    ki    =iI  ;//インナーループI制御系数
    kd    =iD  ;//インナーループD制御系数
    limitH=255 ;//積分上限
    limitL=0   ;//
    sum   =0   ;//積分値
    old   =0   ;//1サンプル前のデータ(微分用)
}