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@2:969dfa4f2436, 2015-04-01 (annotated)

Committer:
joe4465
Date:
Wed Apr 01 11:19:21 2015 +0000
Revision:
2:969dfa4f2436
Parent:
0:c6a85bb2a827
Child:
3:4823d6750629
Altitude hold with 2 PID's working. Exported to offline compiler

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 2:969dfa4f2436 38 //Get angle
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 2:969dfa4f2436 46 }
joe4465 2:969dfa4f2436 47
joe4465 2:969dfa4f2436 48 //Update yaw target
joe4465 2:969dfa4f2436 49 if(abs(_mappedRc.yaw) >= 5 || _state == Status::PREFLIGHT || _state == Status::STANDBY) updateYawTarget();
joe4465 2:969dfa4f2436 50
joe4465 2:969dfa4f2436 51 //Make sure we are initialised
joe4465 2:969dfa4f2436 52 if(_state != Status::PREFLIGHT)
joe4465 2:969dfa4f2436 53 {
joe4465 2:969dfa4f2436 54 //Update yaw difference
joe4465 2:969dfa4f2436 55 _setPoints.yawDifference = MOD(_setPoints.yawTarget - _angle.yaw);
joe4465 2:969dfa4f2436 56
joe4465 2:969dfa4f2436 57 if(_navigationMode == Status::NONE)
joe4465 2:969dfa4f2436 58 {
joe4465 2:969dfa4f2436 59 //Motors are directly controlled by rc remote
joe4465 2:969dfa4f2436 60 if(_mappedRc.throttle >= RC_DEAD_ZONE) _status.setMotorsSpinning(true);
joe4465 2:969dfa4f2436 61 else _status.setMotorsSpinning(false);
joe4465 2:969dfa4f2436 62
joe4465 2:969dfa4f2436 63 //Update target altitude
joe4465 2:969dfa4f2436 64 updateAltitudeTarget();
joe4465 2:969dfa4f2436 65 }
joe4465 2:969dfa4f2436 66 else if(_navigationMode == Status::ALTITUDE_HOLD)
joe4465 2:969dfa4f2436 67 {
joe4465 2:969dfa4f2436 68 //Check if throttle is in dead zone
joe4465 2:969dfa4f2436 69 if(_status.getDeadZone() == true) _setPoints.climbRate = 0;
joe4465 2:969dfa4f2436 70 else
joe4465 2:969dfa4f2436 71 {
joe4465 2:969dfa4f2436 72 //Throttle not in dead zone so map to climb rate
joe4465 2:969dfa4f2436 73 _setPoints.climbRate = map(_mappedRc.throttle, RC_THRUST_MIN, RC_THRUST_MAX, MIN_CLIMB_RATE, MAX_CLIMB_RATE);
joe4465 2:969dfa4f2436 74
joe4465 2:969dfa4f2436 75 //Update target altitude
joe4465 2:969dfa4f2436 76 updateAltitudeTarget();
joe4465 2:969dfa4f2436 77 }
joe4465 2:969dfa4f2436 78
joe4465 2:969dfa4f2436 79 //Get PID output
joe4465 2:969dfa4f2436 80 _altitudeHoldPidOutput = _altitudeController->compute(_setPoints.targetAltitude, _setPoints.climbRate);
joe4465 2:969dfa4f2436 81 }
joe4465 2:969dfa4f2436 82
joe4465 2:969dfa4f2436 83 if(_state == Status::STANDBY)
joe4465 2:969dfa4f2436 84 {
joe4465 2:969dfa4f2436 85 _setPoints.yaw = 0;
joe4465 2:969dfa4f2436 86 _setPoints.pitch = 0;
joe4465 2:969dfa4f2436 87 _setPoints.roll = 0;
joe4465 2:969dfa4f2436 88 _setPoints.throttle = 0;
joe4465 2:969dfa4f2436 89
joe4465 2:969dfa4f2436 90 _altitudeController->reset();
joe4465 2:969dfa4f2436 91 }
joe4465 2:969dfa4f2436 92 else if(_state == Status::GROUND_READY)
joe4465 2:969dfa4f2436 93 {
joe4465 2:969dfa4f2436 94 _setPoints.yaw = _mappedRc.yaw;
joe4465 2:969dfa4f2436 95 _setPoints.pitch = _mappedRc.pitch;
joe4465 2:969dfa4f2436 96 _setPoints.roll = _mappedRc.roll;
joe4465 2:969dfa4f2436 97 _setPoints.throttle = _mappedRc.throttle;
joe4465 2:969dfa4f2436 98
joe4465 2:969dfa4f2436 99 _altitudeController->reset();
joe4465 2:969dfa4f2436 100 }
joe4465 2:969dfa4f2436 101 else if(_state == Status::MANUAL)
joe4465 2:969dfa4f2436 102 {
joe4465 2:969dfa4f2436 103 _setPoints.yaw = _mappedRc.yaw;
joe4465 2:969dfa4f2436 104 _setPoints.pitch = _mappedRc.pitch;
joe4465 2:969dfa4f2436 105 _setPoints.roll = _mappedRc.roll;
joe4465 2:969dfa4f2436 106 _setPoints.throttle = _mappedRc.throttle;
joe4465 2:969dfa4f2436 107
joe4465 2:969dfa4f2436 108 _altitudeController->reset();
joe4465 2:969dfa4f2436 109 }
joe4465 2:969dfa4f2436 110 else if(_state == Status::STABILISED)
joe4465 2:969dfa4f2436 111 {
joe4465 2:969dfa4f2436 112 _setPoints.yaw = _mappedRc.yaw;
joe4465 2:969dfa4f2436 113 _setPoints.pitch = _mappedRc.pitch;
joe4465 2:969dfa4f2436 114 _setPoints.roll = _mappedRc.roll;
joe4465 2:969dfa4f2436 115 _setPoints.throttle = _altitudeHoldPidOutput;
joe4465 2:969dfa4f2436 116 }
joe4465 2:969dfa4f2436 117 else if(_state == Status::AUTO)
joe4465 2:969dfa4f2436 118 {
joe4465 2:969dfa4f2436 119 //Waypoint navigation
joe4465 2:969dfa4f2436 120 }
joe4465 2:969dfa4f2436 121 }
joe4465 2:969dfa4f2436 122 //Not initialised
joe4465 2:969dfa4f2436 123 else
joe4465 2:969dfa4f2436 124 {
joe4465 2:969dfa4f2436 125 _setPoints.yaw = 0;
joe4465 2:969dfa4f2436 126 _setPoints.pitch = 0;
joe4465 2:969dfa4f2436 127 _setPoints.roll = 0;
joe4465 2:969dfa4f2436 128 _setPoints.throttle = 0;
joe4465 2:969dfa4f2436 129
joe4465 2:969dfa4f2436 130 _altitudeController->reset();
joe4465 2:969dfa4f2436 131 }
joe4465 2:969dfa4f2436 132
joe4465 2:969dfa4f2436 133 Thread::wait(20);
joe4465 2:969dfa4f2436 134 }
joe4465 2:969dfa4f2436 135 }
joe4465 2:969dfa4f2436 136
joe4465 2:969dfa4f2436 137 NavigationController::SetPoint NavigationController::getSetPoint()
joe4465 2:969dfa4f2436 138 {
joe4465 2:969dfa4f2436 139 return _setPoints;
joe4465 2:969dfa4f2436 140 }
joe4465 2:969dfa4f2436 141
joe4465 2:969dfa4f2436 142 void NavigationController::updateYawTarget()
joe4465 2:969dfa4f2436 143 {
joe4465 2:969dfa4f2436 144 _setPoints.yawTarget = _angle.yaw;
joe4465 2:969dfa4f2436 145 }
joe4465 2:969dfa4f2436 146
joe4465 2:969dfa4f2436 147 void NavigationController::updateAltitudeTarget()
joe4465 2:969dfa4f2436 148 {
joe4465 2:969dfa4f2436 149 _altitude = _sensors.getAltitude();
joe4465 2:969dfa4f2436 150 _setPoints.targetAltitude = _altitude.computed;
joe4465 2:969dfa4f2436 151
joe4465 2:969dfa4f2436 152 if(_setPoints.targetAltitude <= ALTITUDE_MIN) _setPoints.targetAltitude = ALTITUDE_MIN;
joe4465 2:969dfa4f2436 153 else if(_setPoints.targetAltitude >= ALTITUDE_MAX) _setPoints.targetAltitude = ALTITUDE_MAX;
joe4465 2:969dfa4f2436 154 }
joe4465 2:969dfa4f2436 155
joe4465 2:969dfa4f2436 156 double NavigationController::map(double input, double inputMin, double inputMax, double outputMin, double outputMax)
joe4465 2:969dfa4f2436 157 {
joe4465 2:969dfa4f2436 158 return (input - inputMin) * (outputMax - outputMin) / (inputMax - inputMin) + outputMin;
joe4465 2:969dfa4f2436 159 }
joe4465 2:969dfa4f2436 160
joe4465 2:969dfa4f2436 161 PidWrapper::NavigationControllerPidParameters NavigationController::getPidParameters()
joe4465 2:969dfa4f2436 162 {
joe4465 2:969dfa4f2436 163 return _altitudeController->getPidParameters();
joe4465 2:969dfa4f2436 164 }
joe4465 2:969dfa4f2436 165
joe4465 2:969dfa4f2436 166 void NavigationController::setAltitudeRatePidParameters(PidWrapper::PidParameter pidParameters)
joe4465 2:969dfa4f2436 167 {
joe4465 2:969dfa4f2436 168 _altitudeController->setAltitudeRatePidParameters(pidParameters);
joe4465 2:969dfa4f2436 169 _configFileWrapper.setAltitudeRateParameters(pidParameters);
joe4465 2:969dfa4f2436 170 saveSettings();
joe4465 2:969dfa4f2436 171 }
joe4465 2:969dfa4f2436 172
joe4465 2:969dfa4f2436 173 void NavigationController::setAltitudeStabPidParameters(PidWrapper::PidParameter pidParameters)
joe4465 2:969dfa4f2436 174 {
joe4465 2:969dfa4f2436 175 _altitudeController->setAltitudeStabPidParameters(pidParameters);
joe4465 2:969dfa4f2436 176 _configFileWrapper.setAltitudeStabParameters(pidParameters);
joe4465 2:969dfa4f2436 177 saveSettings();
joe4465 2:969dfa4f2436 178 }
joe4465 2:969dfa4f2436 179
joe4465 2:969dfa4f2436 180 void NavigationController::saveSettings()
joe4465 2:969dfa4f2436 181 {
joe4465 2:969dfa4f2436 182 Status::State state = _status.getState();
joe4465 2:969dfa4f2436 183 if(state == Status::STANDBY || state == Status::PREFLIGHT)
joe4465 2:969dfa4f2436 184 {
joe4465 2:969dfa4f2436 185 _sensors.enable(false);
joe4465 2:969dfa4f2436 186 _configFileWrapper.saveSettings();
joe4465 2:969dfa4f2436 187 _sensors.enable(true);
joe4465 2:969dfa4f2436 188 }
joe4465 2:969dfa4f2436 189 }