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

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

Committer:
joe4465
Date:
Fri May 08 09:07:38 2015 +0000
Revision:
4:9ffbf9101992
Parent:
3:4823d6750629 
End of FYP

Who changed what in which revision?

UserRevisionLine numberNew contents of line
joe4465 2:969dfa4f2436 1 #include "NavigationController.h"
joe4465 2:969dfa4f2436 2
joe4465 2:969dfa4f2436 3 NavigationController::NavigationController(Status& status, Sensors& sensors, Rc& rc, ConfigFileWrapper& configFileWrapper) :
joe4465 2:969dfa4f2436 4 _status(status), _sensors(sensors), _rc(rc), _configFileWrapper(configFileWrapper)
joe4465 2:969dfa4f2436 5 {
joe4465 2:969dfa4f2436 6 _altitudeController = new AltitudeController(_sensors, _configFileWrapper, _status);
joe4465 2:969dfa4f2436 7 _altitudeHoldPidOutput = 0;
joe4465 2:969dfa4f2436 8
joe4465 2:969dfa4f2436 9 _thread = new Thread(&NavigationController::threadStarter, this, osPriorityHigh);
joe4465 2:969dfa4f2436 10
joe4465 2:969dfa4f2436 11 DEBUG("Navigation controller initialised\r\n");
joe4465 2:969dfa4f2436 12 }
joe4465 2:969dfa4f2436 13
joe4465 2:969dfa4f2436 14 NavigationController::~NavigationController(){}
joe4465 2:969dfa4f2436 15
joe4465 2:969dfa4f2436 16 void NavigationController::threadStarter(void const *p)
joe4465 2:969dfa4f2436 17 {
joe4465 2:969dfa4f2436 18 NavigationController *instance = (NavigationController*)p;
joe4465 2:969dfa4f2436 19 instance->threadWorker();
joe4465 2:969dfa4f2436 20 }
joe4465 2:969dfa4f2436 21
joe4465 2:969dfa4f2436 22 void NavigationController::threadWorker()
joe4465 2:969dfa4f2436 23 {
joe4465 2:969dfa4f2436 24 while(true)
joe4465 2:969dfa4f2436 25 {
joe4465 2:969dfa4f2436 26 //Get latest sensor readings
joe4465 2:969dfa4f2436 27 _sensors.update();
joe4465 2:969dfa4f2436 28
joe4465 2:969dfa4f2436 29 //Get state
joe4465 2:969dfa4f2436 30 _state = _status.getState();
joe4465 2:969dfa4f2436 31
joe4465 2:969dfa4f2436 32 //Get navigation mode
joe4465 2:969dfa4f2436 33 _navigationMode = _status.getNavigationMode();
joe4465 2:969dfa4f2436 34
joe4465 2:969dfa4f2436 35 //Get Rc commands
joe4465 2:969dfa4f2436 36 _mappedRc = _rc.getMappedRc();
joe4465 2:969dfa4f2436 37
joe4465 3:4823d6750629 38 //Get angle to calculate yaw
joe4465 2:969dfa4f2436 39 _angle = _sensors.getAngle();
joe4465 2:969dfa4f2436 40
joe4465 2:969dfa4f2436 41 //Reset accel data if not flying
joe4465 2:969dfa4f2436 42 if(_state == Status::PREFLIGHT || _state == Status::STANDBY)
joe4465 2:969dfa4f2436 43 {
joe4465 2:969dfa4f2436 44 //Reset accel
joe4465 2:969dfa4f2436 45 _sensors.zeroAccel();
joe4465 3:4823d6750629 46
joe4465 3:4823d6750629 47 //Reset Gps
joe4465 3:4823d6750629 48 _sensors.zeroPos();
joe4465 2:969dfa4f2436 49 }
joe4465 2:969dfa4f2436 50
joe4465 2:969dfa4f2436 51 //Update yaw target
joe4465 2:969dfa4f2436 52 if(abs(_mappedRc.yaw) >= 5 || _state == Status::PREFLIGHT || _state == Status::STANDBY) updateYawTarget();
joe4465 2:969dfa4f2436 53
joe4465 2:969dfa4f2436 54 //Make sure we are initialised
joe4465 2:969dfa4f2436 55 if(_state != Status::PREFLIGHT)
joe4465 2:969dfa4f2436 56 {
joe4465 2:969dfa4f2436 57 //Update yaw difference
joe4465 2:969dfa4f2436 58 _setPoints.yawDifference = MOD(_setPoints.yawTarget - _angle.yaw);
joe4465 2:969dfa4f2436 59
joe4465 2:969dfa4f2436 60 if(_navigationMode == Status::NONE)
joe4465 2:969dfa4f2436 61 {
joe4465 2:969dfa4f2436 62 //Motors are directly controlled by rc remote
joe4465 2:969dfa4f2436 63 if(_mappedRc.throttle >= RC_DEAD_ZONE) _status.setMotorsSpinning(true);
joe4465 2:969dfa4f2436 64 else _status.setMotorsSpinning(false);
joe4465 2:969dfa4f2436 65
joe4465 2:969dfa4f2436 66 //Update target altitude
joe4465 3:4823d6750629 67 _setPoints.climbRate = 0;
joe4465 2:969dfa4f2436 68 updateAltitudeTarget();
joe4465 2:969dfa4f2436 69 }
joe4465 2:969dfa4f2436 70 else if(_navigationMode == Status::ALTITUDE_HOLD)
joe4465 2:969dfa4f2436 71 {
joe4465 3:4823d6750629 72 //Motors are directly controlled by rc remote
joe4465 3:4823d6750629 73 //if(_mappedRc.throttle >= RC_DEAD_ZONE) _status.setMotorsSpinning(true);
joe4465 3:4823d6750629 74 //else _status.setMotorsSpinning(false);
joe4465 3:4823d6750629 75
joe4465 2:969dfa4f2436 76 //Check if throttle is in dead zone
joe4465 2:969dfa4f2436 77 if(_status.getDeadZone() == true) _setPoints.climbRate = 0;
joe4465 2:969dfa4f2436 78 else
joe4465 2:969dfa4f2436 79 {
joe4465 2:969dfa4f2436 80 //Throttle not in dead zone so map to climb rate
joe4465 2:969dfa4f2436 81 _setPoints.climbRate = map(_mappedRc.throttle, RC_THRUST_MIN, RC_THRUST_MAX, MIN_CLIMB_RATE, MAX_CLIMB_RATE);
joe4465 2:969dfa4f2436 82
joe4465 3:4823d6750629 83 //float target = _setPoints.targetAltitude + (_setPoints.climbRate * 0.02);
joe4465 3:4823d6750629 84
joe4465 2:969dfa4f2436 85 //Update target altitude
joe4465 2:969dfa4f2436 86 updateAltitudeTarget();
joe4465 2:969dfa4f2436 87 }
joe4465 2:969dfa4f2436 88
joe4465 3:4823d6750629 89 //If altitude is less than 10cm the directly map the rc throttle stick to the throttle
joe4465 3:4823d6750629 90 //else use the throttle from the altitude PID controller
joe4465 3:4823d6750629 91 if(_altitude.computed > 10) _altitudeHoldPidOutput = _altitudeController->compute(_setPoints.targetAltitude, _setPoints.climbRate);
joe4465 3:4823d6750629 92 else _status.setMotorsSpinning(false);
joe4465 2:969dfa4f2436 93 }
joe4465 3:4823d6750629 94 /* else if(_navigationMode == Status::AUTO_LAND)
joe4465 3:4823d6750629 95 {
joe4465 3:4823d6750629 96 //Motors are directly controlled by rc remote
joe4465 3:4823d6750629 97 if(_mappedRc.throttle >= RC_DEAD_ZONE) _status.setMotorsSpinning(true);
joe4465 3:4823d6750629 98 else _status.setMotorsSpinning(false);
joe4465 3:4823d6750629 99
joe4465 3:4823d6750629 100 //Get altitude
joe4465 3:4823d6750629 101 _altitude = _sensors.getAltitude();
joe4465 3:4823d6750629 102
joe4465 3:4823d6750629 103 if(_altitude.computed > 1000) _setPoints.targetAltitude = 300;
joe4465 3:4823d6750629 104 else if(_altitude.computed < 600) _setPoints.targetAltitude = 100;
joe4465 3:4823d6750629 105 else if(_altitude.computed < 300) _setPoints.targetAltitude = 0;
joe4465 3:4823d6750629 106
joe4465 3:4823d6750629 107 //If altitude is less than 10 the directly map the rc throttle stick to the throttle
joe4465 3:4823d6750629 108 //else use the throttle from the altitude PID controller
joe4465 3:4823d6750629 109 if(_altitude.computed > 10) _altitudeHoldPidOutput = _altitudeController->compute(_setPoints.targetAltitude, _setPoints.climbRate);
joe4465 3:4823d6750629 110 else _altitudeHoldPidOutput = _mappedRc.throttle;
joe4465 3:4823d6750629 111 }*/
joe4465 2:969dfa4f2436 112
joe4465 2:969dfa4f2436 113 if(_state == Status::STANDBY)
joe4465 2:969dfa4f2436 114 {
joe4465 2:969dfa4f2436 115 _setPoints.yaw = 0;
joe4465 2:969dfa4f2436 116 _setPoints.pitch = 0;
joe4465 2:969dfa4f2436 117 _setPoints.roll = 0;
joe4465 2:969dfa4f2436 118 _setPoints.throttle = 0;
joe4465 2:969dfa4f2436 119
joe4465 2:969dfa4f2436 120 _altitudeController->reset();
joe4465 2:969dfa4f2436 121 }
joe4465 2:969dfa4f2436 122 else if(_state == Status::GROUND_READY)
joe4465 2:969dfa4f2436 123 {
joe4465 2:969dfa4f2436 124 _setPoints.yaw = _mappedRc.yaw;
joe4465 2:969dfa4f2436 125 _setPoints.pitch = _mappedRc.pitch;
joe4465 2:969dfa4f2436 126 _setPoints.roll = _mappedRc.roll;
joe4465 2:969dfa4f2436 127 _setPoints.throttle = _mappedRc.throttle;
joe4465 2:969dfa4f2436 128
joe4465 2:969dfa4f2436 129 _altitudeController->reset();
joe4465 2:969dfa4f2436 130 }
joe4465 2:969dfa4f2436 131 else if(_state == Status::MANUAL)
joe4465 2:969dfa4f2436 132 {
joe4465 2:969dfa4f2436 133 _setPoints.yaw = _mappedRc.yaw;
joe4465 2:969dfa4f2436 134 _setPoints.pitch = _mappedRc.pitch;
joe4465 2:969dfa4f2436 135 _setPoints.roll = _mappedRc.roll;
joe4465 2:969dfa4f2436 136 _setPoints.throttle = _mappedRc.throttle;
joe4465 2:969dfa4f2436 137
joe4465 2:969dfa4f2436 138 _altitudeController->reset();
joe4465 2:969dfa4f2436 139 }
joe4465 2:969dfa4f2436 140 else if(_state == Status::STABILISED)
joe4465 2:969dfa4f2436 141 {
joe4465 2:969dfa4f2436 142 _setPoints.yaw = _mappedRc.yaw;
joe4465 2:969dfa4f2436 143 _setPoints.pitch = _mappedRc.pitch;
joe4465 2:969dfa4f2436 144 _setPoints.roll = _mappedRc.roll;
joe4465 2:969dfa4f2436 145 _setPoints.throttle = _altitudeHoldPidOutput;
joe4465 2:969dfa4f2436 146 }
joe4465 2:969dfa4f2436 147 else if(_state == Status::AUTO)
joe4465 2:969dfa4f2436 148 {
joe4465 2:969dfa4f2436 149 //Waypoint navigation
joe4465 2:969dfa4f2436 150 }
joe4465 2:969dfa4f2436 151 }
joe4465 2:969dfa4f2436 152 //Not initialised
joe4465 2:969dfa4f2436 153 else
joe4465 2:969dfa4f2436 154 {
joe4465 2:969dfa4f2436 155 _setPoints.yaw = 0;
joe4465 2:969dfa4f2436 156 _setPoints.pitch = 0;
joe4465 2:969dfa4f2436 157 _setPoints.roll = 0;
joe4465 2:969dfa4f2436 158 _setPoints.throttle = 0;
joe4465 2:969dfa4f2436 159
joe4465 2:969dfa4f2436 160 _altitudeController->reset();
joe4465 2:969dfa4f2436 161 }
joe4465 2:969dfa4f2436 162
joe4465 2:969dfa4f2436 163 Thread::wait(20);
joe4465 2:969dfa4f2436 164 }
joe4465 2:969dfa4f2436 165 }
joe4465 2:969dfa4f2436 166
joe4465 2:969dfa4f2436 167 NavigationController::SetPoint NavigationController::getSetPoint()
joe4465 2:969dfa4f2436 168 {
joe4465 2:969dfa4f2436 169 return _setPoints;
joe4465 2:969dfa4f2436 170 }
joe4465 2:969dfa4f2436 171
joe4465 2:969dfa4f2436 172 void NavigationController::updateYawTarget()
joe4465 2:969dfa4f2436 173 {
joe4465 2:969dfa4f2436 174 _setPoints.yawTarget = _angle.yaw;
joe4465 2:969dfa4f2436 175 }
joe4465 2:969dfa4f2436 176
joe4465 2:969dfa4f2436 177 void NavigationController::updateAltitudeTarget()
joe4465 2:969dfa4f2436 178 {
joe4465 2:969dfa4f2436 179 _altitude = _sensors.getAltitude();
joe4465 2:969dfa4f2436 180 _setPoints.targetAltitude = _altitude.computed;
joe4465 2:969dfa4f2436 181 if(_setPoints.targetAltitude <= ALTITUDE_MIN) _setPoints.targetAltitude = ALTITUDE_MIN;
joe4465 2:969dfa4f2436 182 else if(_setPoints.targetAltitude >= ALTITUDE_MAX) _setPoints.targetAltitude = ALTITUDE_MAX;
joe4465 2:969dfa4f2436 183 }
joe4465 2:969dfa4f2436 184
joe4465 2:969dfa4f2436 185 double NavigationController::map(double input, double inputMin, double inputMax, double outputMin, double outputMax)
joe4465 2:969dfa4f2436 186 {
joe4465 2:969dfa4f2436 187 return (input - inputMin) * (outputMax - outputMin) / (inputMax - inputMin) + outputMin;
joe4465 2:969dfa4f2436 188 }
joe4465 2:969dfa4f2436 189
joe4465 2:969dfa4f2436 190 PidWrapper::NavigationControllerPidParameters NavigationController::getPidParameters()
joe4465 2:969dfa4f2436 191 {
joe4465 2:969dfa4f2436 192 return _altitudeController->getPidParameters();
joe4465 2:969dfa4f2436 193 }
joe4465 2:969dfa4f2436 194
joe4465 2:969dfa4f2436 195 void NavigationController::setAltitudeRatePidParameters(PidWrapper::PidParameter pidParameters)
joe4465 2:969dfa4f2436 196 {
joe4465 2:969dfa4f2436 197 _altitudeController->setAltitudeRatePidParameters(pidParameters);
joe4465 2:969dfa4f2436 198 _configFileWrapper.setAltitudeRateParameters(pidParameters);
joe4465 2:969dfa4f2436 199 saveSettings();
joe4465 2:969dfa4f2436 200 }
joe4465 2:969dfa4f2436 201
joe4465 2:969dfa4f2436 202 void NavigationController::setAltitudeStabPidParameters(PidWrapper::PidParameter pidParameters)
joe4465 2:969dfa4f2436 203 {
joe4465 2:969dfa4f2436 204 _altitudeController->setAltitudeStabPidParameters(pidParameters);
joe4465 2:969dfa4f2436 205 _configFileWrapper.setAltitudeStabParameters(pidParameters);
joe4465 2:969dfa4f2436 206 saveSettings();
joe4465 2:969dfa4f2436 207 }
joe4465 2:969dfa4f2436 208
joe4465 2:969dfa4f2436 209 void NavigationController::saveSettings()
joe4465 2:969dfa4f2436 210 {
joe4465 2:969dfa4f2436 211 Status::State state = _status.getState();
joe4465 2:969dfa4f2436 212 if(state == Status::STANDBY || state == Status::PREFLIGHT)
joe4465 2:969dfa4f2436 213 {
joe4465 2:969dfa4f2436 214 _sensors.enable(false);
joe4465 2:969dfa4f2436 215 _configFileWrapper.saveSettings();
joe4465 2:969dfa4f2436 216 _sensors.enable(true);
joe4465 2:969dfa4f2436 217 }
joe4465 2:969dfa4f2436 218 }