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/StabController/StabController.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 2:969dfa4f2436 1 #include "StabController.h"
joe4465 2:969dfa4f2436 2
joe4465 2:969dfa4f2436 3 StabController::StabController(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 _yawStabPidController.initialise(_configFileWrapper.getYawStabParameters(), IMU_YAW_ANGLE_MIN, IMU_YAW_ANGLE_MAX, IMU_YAW_RATE_MIN, IMU_YAW_RATE_MAX, updateTime);
joe4465 2:969dfa4f2436 10 _pitchStabPidController.initialise(_configFileWrapper.getPitchStabParameters(), IMU_PITCH_ANGLE_MIN, IMU_PITCH_ANGLE_MAX, IMU_PITCH_RATE_MIN, IMU_PITCH_RATE_MAX, updateTime);
joe4465 2:969dfa4f2436 11 _rollStabPidController.initialise(_configFileWrapper.getRollStabParameters(), IMU_ROLL_ANGLE_MIN, IMU_ROLL_ANGLE_MAX, IMU_ROLL_RATE_MIN, IMU_ROLL_RATE_MAX, updateTime);
joe4465 2:969dfa4f2436 12
joe4465 2:969dfa4f2436 13 DEBUG("Stab controller initialised\r\n");
joe4465 2:969dfa4f2436 14 }
joe4465 2:969dfa4f2436 15
joe4465 2:969dfa4f2436 16 StabController::~StabController(){}
joe4465 2:969dfa4f2436 17
joe4465 2:969dfa4f2436 18 PidWrapper::PidOutput StabController::compute()
joe4465 2:969dfa4f2436 19 {
joe4465 2:969dfa4f2436 20 _setPoints = _navigationController.getSetPoint();
joe4465 2:969dfa4f2436 21 _angle = _sensors.getAngle();
joe4465 2:969dfa4f2436 22 _rate = _sensors.getRate();
joe4465 2:969dfa4f2436 23
joe4465 2:969dfa4f2436 24 //_stabPidOutputs.yaw = _yawStabPidController.compute(_setPoints.yawTarget, _angle.yaw);
joe4465 2:969dfa4f2436 25 _stabPidOutputs.yaw = _yawStabPidController.compute(_setPoints.yawDifference, 0);
joe4465 2:969dfa4f2436 26 _stabPidOutputs.pitch = _pitchStabPidController.compute(_setPoints.pitch, _angle.pitch);
joe4465 2:969dfa4f2436 27 _stabPidOutputs.roll = _rollStabPidController.compute(_setPoints.roll, _angle.roll);
joe4465 2:969dfa4f2436 28
joe4465 2:969dfa4f2436 29 //If pilot commanding yaw
joe4465 2:969dfa4f2436 30 if(abs(_setPoints.yaw) >= 5) _stabPidOutputs.yaw = _setPoints.yaw; //Feed to rate PID (overwriting stab PID output)
joe4465 2:969dfa4f2436 31
joe4465 2:969dfa4f2436 32 _ratePidOutputs.yaw = _yawRatePidController.compute(_stabPidOutputs.yaw, _rate.yaw);
joe4465 2:969dfa4f2436 33 _ratePidOutputs.pitch = _pitchRatePidController.compute(_stabPidOutputs.pitch, _rate.pitch);
joe4465 2:969dfa4f2436 34 _ratePidOutputs.roll = _rollRatePidController.compute(_stabPidOutputs.roll, _rate.roll);
joe4465 2:969dfa4f2436 35
joe4465 2:969dfa4f2436 36 return _ratePidOutputs;
joe4465 2:969dfa4f2436 37 }
joe4465 2:969dfa4f2436 38
joe4465 2:969dfa4f2436 39 PidWrapper::FlightControllerPidParameters StabController::getPidParameters()
joe4465 2:969dfa4f2436 40 {
joe4465 2:969dfa4f2436 41 PidWrapper::FlightControllerPidParameters allPidParameters;
joe4465 2:969dfa4f2436 42 allPidParameters.yawStab = _yawStabPidController.getPidParameters();
joe4465 2:969dfa4f2436 43 allPidParameters.pitchStab = _pitchStabPidController.getPidParameters();
joe4465 2:969dfa4f2436 44 allPidParameters.rollStab = _rollStabPidController.getPidParameters();
joe4465 2:969dfa4f2436 45 allPidParameters.yawRate = _yawRatePidController.getPidParameters();
joe4465 2:969dfa4f2436 46 allPidParameters.pitchRate = _pitchRatePidController.getPidParameters();
joe4465 2:969dfa4f2436 47 allPidParameters.rollRate = _rollRatePidController.getPidParameters();
joe4465 2:969dfa4f2436 48 return allPidParameters;
joe4465 2:969dfa4f2436 49 }
joe4465 2:969dfa4f2436 50
joe4465 2:969dfa4f2436 51 void StabController::reset()
joe4465 2:969dfa4f2436 52 {
joe4465 2:969dfa4f2436 53 _yawRatePidController.reset();
joe4465 2:969dfa4f2436 54 _pitchRatePidController.reset();
joe4465 2:969dfa4f2436 55 _rollRatePidController.reset();
joe4465 2:969dfa4f2436 56 _yawStabPidController.reset();
joe4465 2:969dfa4f2436 57 _pitchStabPidController.reset();
joe4465 2:969dfa4f2436 58 _rollStabPidController.reset();
joe4465 2:969dfa4f2436 59 }
joe4465 2:969dfa4f2436 60
joe4465 2:969dfa4f2436 61 void StabController::setYawStabPidParameters(PidWrapper::PidParameter pidParameters)
joe4465 2:969dfa4f2436 62 {
joe4465 2:969dfa4f2436 63 _yawStabPidController.setPidParameters(pidParameters);
joe4465 2:969dfa4f2436 64 }
joe4465 2:969dfa4f2436 65
joe4465 2:969dfa4f2436 66 void StabController::setPitchStabPidParameters(PidWrapper::PidParameter pidParameters)
joe4465 2:969dfa4f2436 67 {
joe4465 2:969dfa4f2436 68 _pitchStabPidController.setPidParameters(pidParameters);
joe4465 2:969dfa4f2436 69 }
joe4465 2:969dfa4f2436 70
joe4465 2:969dfa4f2436 71 void StabController::setRollStabPidParameters(PidWrapper::PidParameter pidParameters)
joe4465 2:969dfa4f2436 72 {
joe4465 2:969dfa4f2436 73 _rollStabPidController.setPidParameters(pidParameters);
joe4465 2:969dfa4f2436 74 }
joe4465 2:969dfa4f2436 75
joe4465 2:969dfa4f2436 76 void StabController::setYawRatePidParameters(PidWrapper::PidParameter pidParameters)
joe4465 2:969dfa4f2436 77 {
joe4465 2:969dfa4f2436 78 _yawRatePidController.setPidParameters(pidParameters);
joe4465 2:969dfa4f2436 79 }
joe4465 2:969dfa4f2436 80
joe4465 2:969dfa4f2436 81 void StabController::setPitchRatePidParameters(PidWrapper::PidParameter pidParameters)
joe4465 2:969dfa4f2436 82 {
joe4465 2:969dfa4f2436 83 _pitchRatePidController.setPidParameters(pidParameters);
joe4465 2:969dfa4f2436 84 }
joe4465 2:969dfa4f2436 85
joe4465 2:969dfa4f2436 86 void StabController::setRollRatePidParameters(PidWrapper::PidParameter pidParameters)
joe4465 2:969dfa4f2436 87 {
joe4465 2:969dfa4f2436 88 _rollRatePidController.setPidParameters(pidParameters);
joe4465 2:969dfa4f2436 89 }
joe4465 2:969dfa4f2436 90
joe4465 2:969dfa4f2436 91 PidWrapper::StabPidState StabController::getStabPidState()
joe4465 2:969dfa4f2436 92 {
joe4465 2:969dfa4f2436 93 PidWrapper::StabPidState stabPidState;
joe4465 2:969dfa4f2436 94 PidWrapper::PidState yawRatePidState;
joe4465 2:969dfa4f2436 95 PidWrapper::PidState pitchRatePidState;
joe4465 2:969dfa4f2436 96 PidWrapper::PidState rollRatePidState;
joe4465 2:969dfa4f2436 97 PidWrapper::PidState yawStabPidState;
joe4465 2:969dfa4f2436 98 PidWrapper::PidState pitchStabPidState;
joe4465 2:969dfa4f2436 99 PidWrapper::PidState rollStabPidState;
joe4465 2:969dfa4f2436 100
joe4465 2:969dfa4f2436 101 yawRatePidState.setPoint = _stabPidOutputs.yaw;
joe4465 2:969dfa4f2436 102 yawRatePidState.processValue = _rate.yaw;
joe4465 2:969dfa4f2436 103 yawRatePidState.output = _ratePidOutputs.yaw;
joe4465 2:969dfa4f2436 104
joe4465 2:969dfa4f2436 105 pitchRatePidState.setPoint = _stabPidOutputs.pitch;
joe4465 2:969dfa4f2436 106 pitchRatePidState.processValue = _rate.pitch;
joe4465 2:969dfa4f2436 107 pitchRatePidState.output = _ratePidOutputs.pitch;
joe4465 2:969dfa4f2436 108
joe4465 2:969dfa4f2436 109 rollRatePidState.setPoint = _stabPidOutputs.roll;
joe4465 2:969dfa4f2436 110 rollRatePidState.processValue = _rate.roll;
joe4465 2:969dfa4f2436 111 rollRatePidState.output = _ratePidOutputs.roll;
joe4465 2:969dfa4f2436 112
joe4465 2:969dfa4f2436 113 yawStabPidState.setPoint = _setPoints.yawDifference;
joe4465 2:969dfa4f2436 114 yawStabPidState.processValue = 0;
joe4465 2:969dfa4f2436 115 yawStabPidState.output = _stabPidOutputs.yaw;
joe4465 2:969dfa4f2436 116
joe4465 2:969dfa4f2436 117 pitchStabPidState.setPoint = _setPoints.pitch;
joe4465 2:969dfa4f2436 118 pitchStabPidState.processValue = _angle.pitch;
joe4465 2:969dfa4f2436 119 pitchStabPidState.output = _stabPidOutputs.pitch;
joe4465 2:969dfa4f2436 120
joe4465 2:969dfa4f2436 121 rollStabPidState.setPoint = _setPoints.roll;
joe4465 2:969dfa4f2436 122 rollStabPidState.processValue = _angle.roll;
joe4465 2:969dfa4f2436 123 rollStabPidState.output = _stabPidOutputs.roll;
joe4465 2:969dfa4f2436 124
joe4465 2:969dfa4f2436 125 stabPidState.yawRate = yawRatePidState;
joe4465 2:969dfa4f2436 126 stabPidState.pitchRate = pitchRatePidState;
joe4465 2:969dfa4f2436 127 stabPidState.rollRate = rollRatePidState;
joe4465 2:969dfa4f2436 128 stabPidState.yawStab = yawStabPidState;
joe4465 2:969dfa4f2436 129 stabPidState.pitchStab = pitchStabPidState;
joe4465 2:969dfa4f2436 130 stabPidState.rollStab = rollStabPidState;
joe4465 2:969dfa4f2436 131
joe4465 2:969dfa4f2436 132 return stabPidState;
joe4465 2:969dfa4f2436 133 }