RateController.cpp

00001 #include "RateController.h"
00002 
00003 RateController::RateController(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     
00010     DEBUG("Rate controller initialised\r\n");
00011 }
00012 
00013 RateController::~RateController(){}
00014 
00015 PidWrapper::PidOutput RateController::compute()
00016 {   
00017     _setPoints = _navigationController.getSetPoint();
00018     _rate = _sensors.getRate();
00019     
00020     _pidOutputs.yaw = _yawRatePidController.compute(_setPoints.yaw, _rate.yaw);
00021     _pidOutputs.pitch = _pitchRatePidController.compute(_setPoints.pitch, _rate.pitch);
00022     _pidOutputs.roll = _rollRatePidController.compute(_setPoints.roll, _rate.roll);
00023 
00024     return _pidOutputs;
00025 }
00026 
00027 void RateController::reset()
00028 {
00029     _yawRatePidController.reset();
00030     _pitchRatePidController.reset();
00031     _rollRatePidController.reset();
00032 }
00033 
00034 
00035 void RateController::setYawRatePidParameters(PidWrapper::PidParameter pidParameters)
00036 {
00037     _yawRatePidController.setPidParameters(pidParameters);
00038 }
00039 
00040 void RateController::setPitchRatePidParameters(PidWrapper::PidParameter pidParameters)
00041 {
00042     _pitchRatePidController.setPidParameters(pidParameters);
00043 }
00044 
00045 void RateController::setRollRatePidParameters(PidWrapper::PidParameter pidParameters)
00046 {
00047     _rollRatePidController.setPidParameters(pidParameters);
00048 }
00049 
00050 PidWrapper::RatePidState RateController::getRatePidState()
00051 {
00052     PidWrapper::RatePidState ratePidState;
00053     PidWrapper::PidState yawRatePidState;
00054     PidWrapper::PidState pitchRatePidState;
00055     PidWrapper::PidState rollRatePidState;
00056     
00057     yawRatePidState.setPoint = _setPoints.yaw;
00058     yawRatePidState.processValue = _rate.yaw;
00059     yawRatePidState.output = _pidOutputs.yaw;
00060     
00061     pitchRatePidState.setPoint = _setPoints.pitch;
00062     pitchRatePidState.processValue = _rate.pitch;
00063     pitchRatePidState.output = _pidOutputs.pitch;
00064     
00065     rollRatePidState.setPoint = _setPoints.roll;
00066     rollRatePidState.processValue = _rate.roll;
00067     rollRatePidState.output = _pidOutputs.roll;
00068     
00069     ratePidState.yawRate = yawRatePidState;
00070     ratePidState.pitchRate = pitchRatePidState;
00071     ratePidState.rollRate = rollRatePidState;
00072     
00073     return ratePidState;
00074 }