AltitudeController.cpp

00001 #include "AltitudeController.h"
00002 
00003 AltitudeController::AltitudeController(Sensors& sensors, ConfigFileWrapper& configFileWrapper, Status& status) : 
00004 _sensors(sensors), _configFileWrapper(configFileWrapper), _status(status)
00005 {
00006     double updateTime = (1.0 / ((float)FLIGHT_CONTROLLER_FREQUENCY / 10.0));
00007     _altitudeRatePidController.initialise(_configFileWrapper.getAltitudeRateParameters(), MIN_CLIMB_RATE, MAX_CLIMB_RATE, (RC_THRUST_MIN + 0.2), (RC_THRUST_MAX - 0.2), updateTime);
00008     _altitudeRatePidController.setBias(RC_HOVER);
00009     _altitudeStabPidController.initialise(_configFileWrapper.getAltitudeStabParameters(), ALTITUDE_MIN, ALTITUDE_MAX, MIN_CLIMB_RATE, MAX_CLIMB_RATE, updateTime );
00010     _altitudeStabPidController.setBias(RC_HOVER);
00011     //_setPoints = new NavigationController::SetPoint();
00012     
00013     DEBUG("Altitude controller initialised\r\n");
00014 }
00015 
00016 AltitudeController::~AltitudeController(){}
00017 
00018 double AltitudeController::compute(double targetAltitude, double climbRate)
00019 {   
00020     _altitude = _sensors.getAltitude();
00021     Imu::Velocity velocity = _sensors.getImuVelocity();
00022     
00023     float altitudeStabPidOutput = _altitudeStabPidController.compute(targetAltitude, _altitude.computed);
00024     
00025     //If pilot commanding climb rate
00026     if(_status.getDeadZone() == false) altitudeStabPidOutput = climbRate; //Feed to rate PID (overwriting stab PID output)
00027     
00028     float altitudeRatePidOutput = _altitudeRatePidController.compute(altitudeStabPidOutput, velocity.z);
00029     
00030     return altitudeRatePidOutput;
00031 }
00032 
00033 void AltitudeController::reset()
00034 {
00035     _altitudeRatePidController.reset();
00036     _altitudeStabPidController.reset();
00037 }
00038 
00039 PidWrapper::NavigationControllerPidParameters AltitudeController::getPidParameters()
00040 {
00041     PidWrapper::NavigationControllerPidParameters allPidParameters;
00042     allPidParameters.altitudeRate = _altitudeRatePidController.getPidParameters();
00043     allPidParameters.altitudeStab = _altitudeStabPidController.getPidParameters();
00044     return allPidParameters;
00045 }
00046 
00047 void AltitudeController::setAltitudeRatePidParameters(PidWrapper::PidParameter pidParameters)
00048 {
00049     _altitudeRatePidController.setPidParameters(pidParameters);
00050 }
00051 
00052 void AltitudeController::setAltitudeStabPidParameters(PidWrapper::PidParameter pidParameters)
00053 {
00054     _altitudeStabPidController.setPidParameters(pidParameters);
00055 }