YawControl.cpp

Go to the documentation of this file.

/*
 * YawControl.cpp
 *
 *  Created on: Oct 27, 2011
 *      Author: mriedel
 */

#include <tk_ctrlalgo/YawControl.hpp>

#include <telekyb_base/Spaces/Angle.hpp>


namespace TELEKYB_NAMESPACE {


// Options
YawControlOptions::YawControlOptions()
        : OptionContainer("YawControl")
{
        tPropGain = addOption<double>( "tPropGain",
                        "Yaw Proportional gain for control with onboard yaw rate measure", 5.0, false, true);
        tIntGain = addOption<double>( "tIntGain",
                        "Yaw integrational gain for control with onboard absolute yaw measure", 0.5, false, true);
        tIntGainInfluenceSaturation = addOption<double>( "tIntGainInfluenceSaturation",
                        "Yaw integrational components saturation for control with onboard absolute yaw measure", 2.0, false, true);
        tPropGainExt = addOption<double>( "tPropGainExt",
                        "Yaw Proportional gain for control with external yaw rate measurement", 5.0, false, true);
        tDerivGainExt = addOption<double>( "tDerivGainExt",
                        "Yaw Derivative gain for control with external yaw rate measurement", 15.0, false, true);
        tAbsolutAngleControl = addOption<bool>( "tAbsolutAngleControl",
                        "Send a direct angle instead of a yaw rate command", false, false, true);
}

YawControl::YawControl()
{
        lastTime = ros::Time::now();
        integralYawError = 0;
}

YawControl::~YawControl()
{

}

void YawControl::run(const TKTrajectory& input, const TKState& currentState, YawCtrlOutput& output)
{
        currentTime = ros::Time::now();
        double currentYawRate = currentState.angVelocity(2);
        double currentYawAngle = currentState.getEulerRPY()(2);
        output.comYaw = 0.0;

        double yawAngleErr = Angle::normPi(input.yawAngle - currentYawAngle); // desYawAngle - currentYawAngle
        double yawRateErr = input.yawRate - currentYawRate; // desYawRate - currentYawRate

//      Decimal kp_vel=getYawVelPropGain();;
        if (input.yawCtrl == YawControlType::AngleOnBoard || input.yawCtrl == YawControlType::RateOnBoard) {
                if(options.tAbsolutAngleControl->getValue()){
                        //compute integrated error
                        integralYawError += (currentTime-lastTime).toSec()*yawAngleErr;
                        if(integralYawError > options.tIntGainInfluenceSaturation->getValue()){
                                integralYawError = options.tIntGainInfluenceSaturation->getValue();
                        }
                        else if (integralYawError < -options.tIntGainInfluenceSaturation->getValue()){
                                integralYawError = -options.tIntGainInfluenceSaturation->getValue();
                        }

                        double ki = options.tIntGain->getValue();
                        output.comYaw = input.yawAngle + ki*integralYawError; //send desired yaw
                }
                else{
                        double kp = options.tPropGain->getValue();
                        output.comYaw = input.yawRate  + kp*yawAngleErr; // desired Yaw
                }
        } else if (input.yawCtrl == YawControlType::AngleOffBoard) {
                double kp = options.tPropGainExt->getValue();
                double kd = options.tDerivGainExt->getValue();
                output.comYaw = input.yawAcceleration + kd*yawRateErr + kp*yawAngleErr; // desired Yaw
        } else if (input.yawCtrl == YawControlType::RateOffBoard) {
                double kd = options.tDerivGainExt->getValue();
                output.comYaw = input.yawAcceleration + kd*yawRateErr; // desired Yaw
        } else if (input.yawCtrl == YawControlType::AccelerationOnBoard) {
                ROS_ERROR("YawControlType::AccelerationOnBoard not implemented!");
        } else if (input.yawCtrl == YawControlType::AccelerationOffBoard) {
                ROS_ERROR("YawControlType::AccelerationOffBoard not implemented!");
        }
//      output.comYaw = ka*input.yawAcceleration + kd*yawRateErr + kp*yawAngleErr + ki*integralYawError; // desired Yaw
//      std::cout << "kp: " << kp << "\t, kd: " << kd << "\t, ka: " << ka << "\t, comYaw: " << output.comYaw << "\t, intError: " << integralYawError << "\t, inAng: " << (double)input.yawAngle << "\t, stateAng: " << (double)currentState.getEulerRPY()(2) << "\t, AngErr: " << yawAngleErr << std::endl;
        lastTime = currentTime;
}

}