File content as of revision 24:c5945aaae777:

#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;
}

//=========================================================
//パラメータゲッター
//=========================================================
float HbAttitude::getPp(){return p;}
float HbAttitude::getP() {return kp;}
float HbAttitude::getI() {return ki;}
float HbAttitude::getD() {return kd;}

//=========================================================
//パラメータセッター
//=========================================================
void HbAttitude::setPp(float iPp){p = iPp;}
void HbAttitude::setP(float iP)  {kp= iP;}
void HbAttitude::setI(float iI)  {ki= iI;}
void HbAttitude::setD(float iD)  {kd= iD;}
void HbAttitude::setIMax(float val){limitH = val;}
void HbAttitude::setIMin(float val){limitL = val;}

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