StabController.cpp

00001 #include "StabController.h"
00002 
00003 StabController::StabController(Sensors& sensors, NavigationController& navigationController, ConfigFileWrapper& configFileWrapper) : _sensors(sensors), _navigationController(navigationController), _configFileWrapper(configFileWrapper)
00004 {
00005     float updateTime = 1.0 / (float)FLIGHT_CONTROLLER_FREQUENCY;
00006     _yawRatePidController.initialise(_configFileWrapper.getYawRateParameters(), IMU_YAW_RATE_MIN, IMU_YAW_RATE_MAX, RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX, updateTime);
00007     _pitchRatePidController.initialise(_configFileWrapper.getPitchRateParameters(), IMU_PITCH_RATE_MIN, IMU_PITCH_RATE_MAX, RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX, updateTime);
00008     _rollRatePidController.initialise(_configFileWrapper.getRollRateParameters(), IMU_ROLL_RATE_MIN, IMU_ROLL_RATE_MAX, RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX, updateTime);
00009     _yawStabPidController.initialise(_configFileWrapper.getYawStabParameters(), IMU_YAW_ANGLE_MIN, IMU_YAW_ANGLE_MAX, IMU_YAW_RATE_MIN, IMU_YAW_RATE_MAX, updateTime);
00010     _pitchStabPidController.initialise(_configFileWrapper.getPitchStabParameters(), IMU_PITCH_ANGLE_MIN, IMU_PITCH_ANGLE_MAX, IMU_PITCH_RATE_MIN, IMU_PITCH_RATE_MAX, updateTime);
00011     _rollStabPidController.initialise(_configFileWrapper.getRollStabParameters(), IMU_ROLL_ANGLE_MIN, IMU_ROLL_ANGLE_MAX, IMU_ROLL_RATE_MIN, IMU_ROLL_RATE_MAX, updateTime);
00012     
00013     DEBUG("Stab controller initialised\r\n");
00014 }
00015 
00016 StabController::~StabController(){}
00017 
00018 PidWrapper::PidOutput StabController::compute()
00019 {   
00020     _setPoints = _navigationController.getSetPoint();
00021     _angle = _sensors.getAngle();
00022     _rate = _sensors.getRate();   
00023         
00024     //_stabPidOutputs.yaw = _yawStabPidController.compute(_setPoints.yawTarget, _angle.yaw);
00025     _stabPidOutputs.yaw = _yawStabPidController.compute(_setPoints.yawDifference, 0);
00026     _stabPidOutputs.pitch = _pitchStabPidController.compute(_setPoints.pitch, _angle.pitch);
00027     _stabPidOutputs.roll = _rollStabPidController.compute(_setPoints.roll, _angle.roll);
00028 
00029     //If pilot commanding yaw
00030     if(abs(_setPoints.yaw) >= 5) _stabPidOutputs.yaw = _setPoints.yaw;  //Feed to rate PID (overwriting stab PID output)
00031     
00032     _ratePidOutputs.yaw = _yawRatePidController.compute(_stabPidOutputs.yaw, _rate.yaw);
00033     _ratePidOutputs.pitch = _pitchRatePidController.compute(_stabPidOutputs.pitch, _rate.pitch);
00034     _ratePidOutputs.roll = _rollRatePidController.compute(_stabPidOutputs.roll, _rate.roll);
00035     
00036     return _ratePidOutputs;
00037 }
00038 
00039 PidWrapper::FlightControllerPidParameters StabController::getPidParameters()
00040 {
00041     PidWrapper::FlightControllerPidParameters allPidParameters;
00042     allPidParameters.yawStab = _yawStabPidController.getPidParameters();
00043     allPidParameters.pitchStab = _pitchStabPidController.getPidParameters();
00044     allPidParameters.rollStab = _rollStabPidController.getPidParameters();
00045     allPidParameters.yawRate = _yawRatePidController.getPidParameters();
00046     allPidParameters.pitchRate = _pitchRatePidController.getPidParameters();
00047     allPidParameters.rollRate = _rollRatePidController.getPidParameters();
00048     return allPidParameters;
00049 }
00050 
00051 void StabController::reset()
00052 {
00053     _yawRatePidController.reset();
00054     _pitchRatePidController.reset();
00055     _rollRatePidController.reset();
00056     _yawStabPidController.reset();
00057     _pitchStabPidController.reset();
00058     _rollStabPidController.reset();
00059 }
00060 
00061 void StabController::setYawStabPidParameters(PidWrapper::PidParameter pidParameters)
00062 {
00063     _yawStabPidController.setPidParameters(pidParameters);
00064 }
00065 
00066 void StabController::setPitchStabPidParameters(PidWrapper::PidParameter pidParameters)
00067 {
00068     _pitchStabPidController.setPidParameters(pidParameters);
00069 }
00070 
00071 void StabController::setRollStabPidParameters(PidWrapper::PidParameter pidParameters)
00072 {
00073     _rollStabPidController.setPidParameters(pidParameters);
00074 }
00075 
00076 void StabController::setYawRatePidParameters(PidWrapper::PidParameter pidParameters)
00077 {
00078     _yawRatePidController.setPidParameters(pidParameters);
00079 }
00080 
00081 void StabController::setPitchRatePidParameters(PidWrapper::PidParameter pidParameters)
00082 {
00083     _pitchRatePidController.setPidParameters(pidParameters);
00084 }
00085 
00086 void StabController::setRollRatePidParameters(PidWrapper::PidParameter pidParameters)
00087 {
00088     _rollRatePidController.setPidParameters(pidParameters);
00089 }
00090 
00091 PidWrapper::StabPidState StabController::getStabPidState()
00092 {
00093     PidWrapper::StabPidState stabPidState;
00094     PidWrapper::PidState yawRatePidState;
00095     PidWrapper::PidState pitchRatePidState;
00096     PidWrapper::PidState rollRatePidState;
00097     PidWrapper::PidState yawStabPidState;
00098     PidWrapper::PidState pitchStabPidState;
00099     PidWrapper::PidState rollStabPidState;
00100     
00101     yawRatePidState.setPoint = _stabPidOutputs.yaw;
00102     yawRatePidState.processValue = _rate.yaw;
00103     yawRatePidState.output = _ratePidOutputs.yaw;
00104     
00105     pitchRatePidState.setPoint = _stabPidOutputs.pitch;
00106     pitchRatePidState.processValue = _rate.pitch;
00107     pitchRatePidState.output = _ratePidOutputs.pitch;
00108     
00109     rollRatePidState.setPoint = _stabPidOutputs.roll;
00110     rollRatePidState.processValue = _rate.roll;
00111     rollRatePidState.output = _ratePidOutputs.roll;
00112     
00113     yawStabPidState.setPoint = _setPoints.yawDifference;
00114     yawStabPidState.processValue = 0;
00115     yawStabPidState.output = _stabPidOutputs.yaw;
00116     
00117     pitchStabPidState.setPoint = _setPoints.pitch;
00118     pitchStabPidState.processValue = _angle.pitch;
00119     pitchStabPidState.output = _stabPidOutputs.pitch;
00120     
00121     rollStabPidState.setPoint = _setPoints.roll;
00122     rollStabPidState.processValue = _angle.roll;
00123     rollStabPidState.output = _stabPidOutputs.roll;
00124     
00125     stabPidState.yawRate = yawRatePidState;
00126     stabPidState.pitchRate = pitchRatePidState;
00127     stabPidState.rollRate = rollRatePidState;
00128     stabPidState.yawStab = yawStabPidState;
00129     stabPidState.pitchStab = pitchStabPidState;
00130     stabPidState.rollStab = rollStabPidState;
00131     
00132     return stabPidState;
00133 }