Joseph Roberts / Mbed 2 deprecated Quadcopter_mk2

New version of quadcopter software written to OO principles

Dependencies:   mbed MODSERIAL filter mbed-rtos ConfigFile PID PPM FreeIMU_external_magnetometer TinyGPS

FlightController/RateController/RateController.cpp@4:9ffbf9101992, 2015-05-08 (annotated)

Committer:
joe4465
Date:
Fri May 08 09:07:38 2015 +0000
Revision:
4:9ffbf9101992
Parent:
2:969dfa4f2436 
End of FYP

Who changed what in which revision?

UserRevisionLine numberNew contents of line
joe4465 0:c6a85bb2a827 1 #include "RateController.h"
joe4465 0:c6a85bb2a827 2
joe4465 2:969dfa4f2436 3 RateController::RateController(Sensors& sensors, NavigationController& navigationController, ConfigFileWrapper& configFileWrapper) : _sensors(sensors), _navigationController(navigationController), _configFileWrapper(configFileWrapper)
joe4465 2:969dfa4f2436 4 {
joe4465 2:969dfa4f2436 5 float updateTime = 1.0 / (float)FLIGHT_CONTROLLER_FREQUENCY;
joe4465 2:969dfa4f2436 6 _yawRatePidController.initialise(_configFileWrapper.getYawRateParameters(), IMU_YAW_RATE_MIN, IMU_YAW_RATE_MAX, RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX, updateTime);
joe4465 2:969dfa4f2436 7 _pitchRatePidController.initialise(_configFileWrapper.getPitchRateParameters(), IMU_PITCH_RATE_MIN, IMU_PITCH_RATE_MAX, RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX, updateTime);
joe4465 2:969dfa4f2436 8 _rollRatePidController.initialise(_configFileWrapper.getRollRateParameters(), IMU_ROLL_RATE_MIN, IMU_ROLL_RATE_MAX, RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX, updateTime);
joe4465 2:969dfa4f2436 9
joe4465 2:969dfa4f2436 10 DEBUG("Rate controller initialised\r\n");
joe4465 2:969dfa4f2436 11 }
joe4465 0:c6a85bb2a827 12
joe4465 0:c6a85bb2a827 13 RateController::~RateController(){}
joe4465 0:c6a85bb2a827 14
joe4465 2:969dfa4f2436 15 PidWrapper::PidOutput RateController::compute()
joe4465 2:969dfa4f2436 16 {
joe4465 2:969dfa4f2436 17 _setPoints = _navigationController.getSetPoint();
joe4465 2:969dfa4f2436 18 _rate = _sensors.getRate();
joe4465 0:c6a85bb2a827 19
joe4465 2:969dfa4f2436 20 _pidOutputs.yaw = _yawRatePidController.compute(_setPoints.yaw, _rate.yaw);
joe4465 2:969dfa4f2436 21 _pidOutputs.pitch = _pitchRatePidController.compute(_setPoints.pitch, _rate.pitch);
joe4465 2:969dfa4f2436 22 _pidOutputs.roll = _rollRatePidController.compute(_setPoints.roll, _rate.roll);
joe4465 2:969dfa4f2436 23
joe4465 2:969dfa4f2436 24 return _pidOutputs;
joe4465 2:969dfa4f2436 25 }
joe4465 0:c6a85bb2a827 26
joe4465 2:969dfa4f2436 27 void RateController::reset()
joe4465 2:969dfa4f2436 28 {
joe4465 2:969dfa4f2436 29 _yawRatePidController.reset();
joe4465 2:969dfa4f2436 30 _pitchRatePidController.reset();
joe4465 2:969dfa4f2436 31 _rollRatePidController.reset();
joe4465 2:969dfa4f2436 32 }
joe4465 2:969dfa4f2436 33
joe4465 2:969dfa4f2436 34
joe4465 2:969dfa4f2436 35 void RateController::setYawRatePidParameters(PidWrapper::PidParameter pidParameters)
joe4465 2:969dfa4f2436 36 {
joe4465 2:969dfa4f2436 37 _yawRatePidController.setPidParameters(pidParameters);
joe4465 2:969dfa4f2436 38 }
joe4465 2:969dfa4f2436 39
joe4465 2:969dfa4f2436 40 void RateController::setPitchRatePidParameters(PidWrapper::PidParameter pidParameters)
joe4465 2:969dfa4f2436 41 {
joe4465 2:969dfa4f2436 42 _pitchRatePidController.setPidParameters(pidParameters);
joe4465 0:c6a85bb2a827 43 }
joe4465 0:c6a85bb2a827 44
joe4465 2:969dfa4f2436 45 void RateController::setRollRatePidParameters(PidWrapper::PidParameter pidParameters)
joe4465 0:c6a85bb2a827 46 {
joe4465 2:969dfa4f2436 47 _rollRatePidController.setPidParameters(pidParameters);
joe4465 2:969dfa4f2436 48 }
joe4465 2:969dfa4f2436 49
joe4465 2:969dfa4f2436 50 PidWrapper::RatePidState RateController::getRatePidState()
joe4465 2:969dfa4f2436 51 {
joe4465 2:969dfa4f2436 52 PidWrapper::RatePidState ratePidState;
joe4465 2:969dfa4f2436 53 PidWrapper::PidState yawRatePidState;
joe4465 2:969dfa4f2436 54 PidWrapper::PidState pitchRatePidState;
joe4465 2:969dfa4f2436 55 PidWrapper::PidState rollRatePidState;
joe4465 0:c6a85bb2a827 56
joe4465 2:969dfa4f2436 57 yawRatePidState.setPoint = _setPoints.yaw;
joe4465 2:969dfa4f2436 58 yawRatePidState.processValue = _rate.yaw;
joe4465 2:969dfa4f2436 59 yawRatePidState.output = _pidOutputs.yaw;
joe4465 2:969dfa4f2436 60
joe4465 2:969dfa4f2436 61 pitchRatePidState.setPoint = _setPoints.pitch;
joe4465 2:969dfa4f2436 62 pitchRatePidState.processValue = _rate.pitch;
joe4465 2:969dfa4f2436 63 pitchRatePidState.output = _pidOutputs.pitch;
joe4465 0:c6a85bb2a827 64
joe4465 2:969dfa4f2436 65 rollRatePidState.setPoint = _setPoints.roll;
joe4465 2:969dfa4f2436 66 rollRatePidState.processValue = _rate.roll;
joe4465 2:969dfa4f2436 67 rollRatePidState.output = _pidOutputs.roll;
joe4465 2:969dfa4f2436 68
joe4465 2:969dfa4f2436 69 ratePidState.yawRate = yawRatePidState;
joe4465 2:969dfa4f2436 70 ratePidState.pitchRate = pitchRatePidState;
joe4465 2:969dfa4f2436 71 ratePidState.rollRate = rollRatePidState;
joe4465 2:969dfa4f2436 72
joe4465 2:969dfa4f2436 73 return ratePidState;
joe4465 0:c6a85bb2a827 74 }