New version of quadcopter software written to OO principles
Dependencies: mbed MODSERIAL filter mbed-rtos ConfigFile PID PPM FreeIMU_external_magnetometer TinyGPS
FlightController/FlightController.cpp@2:969dfa4f2436, 2015-04-01 (annotated)
- Committer:
- joe4465
- Date:
- Wed Apr 01 11:19:21 2015 +0000
- Revision:
- 2:969dfa4f2436
- Parent:
- 0:c6a85bb2a827
Altitude hold with 2 PID's working. Exported to offline compiler
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
joe4465 | 0:c6a85bb2a827 | 1 | #include "FlightController.h" |
joe4465 | 0:c6a85bb2a827 | 2 | |
joe4465 | 2:969dfa4f2436 | 3 | FlightController::FlightController(Status& status, Sensors& sensors, NavigationController& navigationController, ConfigFileWrapper& configFileWrapper, PinName motor1, PinName motor2, PinName motor3, PinName motor4) |
joe4465 | 2:969dfa4f2436 | 4 | : _status(status), _sensors(sensors), _navigationController(navigationController), _configFileWrapper(configFileWrapper) |
joe4465 | 2:969dfa4f2436 | 5 | { |
joe4465 | 2:969dfa4f2436 | 6 | _motorMixer = new MotorMixer(motor1, motor2, motor3, motor4); |
joe4465 | 2:969dfa4f2436 | 7 | _rateController = new RateController(_sensors, _navigationController, _configFileWrapper); |
joe4465 | 2:969dfa4f2436 | 8 | _stabController = new StabController(_sensors, _navigationController, _configFileWrapper); |
joe4465 | 2:969dfa4f2436 | 9 | |
joe4465 | 2:969dfa4f2436 | 10 | _rtosTimer = new RtosTimer(&FlightController::threadStarter, osTimerPeriodic, (void*)this); |
joe4465 | 2:969dfa4f2436 | 11 | int updateTime = (1.0 / (float)FLIGHT_CONTROLLER_FREQUENCY) * 1000; |
joe4465 | 2:969dfa4f2436 | 12 | _rtosTimer->start(updateTime); |
joe4465 | 2:969dfa4f2436 | 13 | |
joe4465 | 2:969dfa4f2436 | 14 | DEBUG("Flight controller initialised\r\n"); |
joe4465 | 2:969dfa4f2436 | 15 | } |
joe4465 | 0:c6a85bb2a827 | 16 | |
joe4465 | 0:c6a85bb2a827 | 17 | FlightController::~FlightController(){} |
joe4465 | 0:c6a85bb2a827 | 18 | |
joe4465 | 0:c6a85bb2a827 | 19 | void FlightController::threadStarter(void const *p) |
joe4465 | 0:c6a85bb2a827 | 20 | { |
joe4465 | 0:c6a85bb2a827 | 21 | FlightController *instance = (FlightController*)p; |
joe4465 | 0:c6a85bb2a827 | 22 | instance->threadWorker(); |
joe4465 | 0:c6a85bb2a827 | 23 | } |
joe4465 | 0:c6a85bb2a827 | 24 | |
joe4465 | 0:c6a85bb2a827 | 25 | void FlightController::threadWorker() |
joe4465 | 2:969dfa4f2436 | 26 | { |
joe4465 | 2:969dfa4f2436 | 27 | Status::FlightMode flightMode = _status.getFlightMode(); |
joe4465 | 2:969dfa4f2436 | 28 | |
joe4465 | 2:969dfa4f2436 | 29 | _sensors.updateImu(); |
joe4465 | 0:c6a85bb2a827 | 30 | |
joe4465 | 2:969dfa4f2436 | 31 | if(flightMode == Status::RATE) |
joe4465 | 2:969dfa4f2436 | 32 | { |
joe4465 | 2:969dfa4f2436 | 33 | //Rate mode |
joe4465 | 2:969dfa4f2436 | 34 | _pidOutputs = _rateController->compute(); |
joe4465 | 2:969dfa4f2436 | 35 | } |
joe4465 | 2:969dfa4f2436 | 36 | else if (flightMode == Status::STAB) |
joe4465 | 2:969dfa4f2436 | 37 | { |
joe4465 | 2:969dfa4f2436 | 38 | //Stab mode |
joe4465 | 2:969dfa4f2436 | 39 | _pidOutputs = _stabController->compute(); |
joe4465 | 2:969dfa4f2436 | 40 | } |
joe4465 | 2:969dfa4f2436 | 41 | |
joe4465 | 0:c6a85bb2a827 | 42 | Status::State state = _status.getState(); |
joe4465 | 2:969dfa4f2436 | 43 | NavigationController::SetPoint setPoints = _navigationController.getSetPoint(); |
joe4465 | 2:969dfa4f2436 | 44 | |
joe4465 | 0:c6a85bb2a827 | 45 | if(state == Status::MANUAL || state == Status::STABILISED || state == Status::AUTO) |
joe4465 | 0:c6a85bb2a827 | 46 | { |
joe4465 | 2:969dfa4f2436 | 47 | _motorMixer->computePower(_pidOutputs, setPoints.throttle); |
joe4465 | 2:969dfa4f2436 | 48 | } |
joe4465 | 2:969dfa4f2436 | 49 | else if(state == Status::GROUND_READY) |
joe4465 | 2:969dfa4f2436 | 50 | { |
joe4465 | 2:969dfa4f2436 | 51 | _motorMixer->setPower(MOTORS_ARMED); |
joe4465 | 2:969dfa4f2436 | 52 | _rateController->reset(); |
joe4465 | 2:969dfa4f2436 | 53 | _stabController->reset(); |
joe4465 | 2:969dfa4f2436 | 54 | } |
joe4465 | 2:969dfa4f2436 | 55 | else if(state == Status::STANDBY) |
joe4465 | 2:969dfa4f2436 | 56 | { |
joe4465 | 2:969dfa4f2436 | 57 | _motorMixer->setPower(MOTORS_OFF); |
joe4465 | 2:969dfa4f2436 | 58 | _rateController->reset(); |
joe4465 | 2:969dfa4f2436 | 59 | _stabController->reset(); |
joe4465 | 0:c6a85bb2a827 | 60 | } |
joe4465 | 0:c6a85bb2a827 | 61 | //Disable motors if state is not valid |
joe4465 | 2:969dfa4f2436 | 62 | else _motorMixer->setPower(MOTORS_OFF); |
joe4465 | 2:969dfa4f2436 | 63 | } |
joe4465 | 2:969dfa4f2436 | 64 | |
joe4465 | 2:969dfa4f2436 | 65 | MotorMixer::MotorPower FlightController::getMotorPower() |
joe4465 | 2:969dfa4f2436 | 66 | { |
joe4465 | 2:969dfa4f2436 | 67 | return _motorMixer->getMotorPower(); |
joe4465 | 2:969dfa4f2436 | 68 | } |
joe4465 | 2:969dfa4f2436 | 69 | |
joe4465 | 2:969dfa4f2436 | 70 | PidWrapper::PidOutput FlightController::getPidOutputs() |
joe4465 | 2:969dfa4f2436 | 71 | { |
joe4465 | 2:969dfa4f2436 | 72 | return _pidOutputs; |
joe4465 | 2:969dfa4f2436 | 73 | } |
joe4465 | 2:969dfa4f2436 | 74 | |
joe4465 | 2:969dfa4f2436 | 75 | PidWrapper::FlightControllerPidParameters FlightController::getPidParameters() |
joe4465 | 2:969dfa4f2436 | 76 | { |
joe4465 | 2:969dfa4f2436 | 77 | return _stabController->getPidParameters(); |
joe4465 | 2:969dfa4f2436 | 78 | } |
joe4465 | 2:969dfa4f2436 | 79 | |
joe4465 | 2:969dfa4f2436 | 80 | void FlightController::setYawStabPidParameters(PidWrapper::PidParameter pidParameters) |
joe4465 | 2:969dfa4f2436 | 81 | { |
joe4465 | 2:969dfa4f2436 | 82 | _stabController->setYawStabPidParameters(pidParameters); |
joe4465 | 2:969dfa4f2436 | 83 | _configFileWrapper.setYawStabParameters(pidParameters); |
joe4465 | 2:969dfa4f2436 | 84 | Status::State state = _status.getState(); |
joe4465 | 2:969dfa4f2436 | 85 | saveSettings(); |
joe4465 | 2:969dfa4f2436 | 86 | } |
joe4465 | 2:969dfa4f2436 | 87 | |
joe4465 | 2:969dfa4f2436 | 88 | void FlightController::setPitchStabPidParameters(PidWrapper::PidParameter pidParameters) |
joe4465 | 2:969dfa4f2436 | 89 | { |
joe4465 | 2:969dfa4f2436 | 90 | _stabController->setPitchStabPidParameters(pidParameters); |
joe4465 | 2:969dfa4f2436 | 91 | _configFileWrapper.setPitchStabParameters(pidParameters); |
joe4465 | 2:969dfa4f2436 | 92 | saveSettings(); |
joe4465 | 2:969dfa4f2436 | 93 | } |
joe4465 | 2:969dfa4f2436 | 94 | |
joe4465 | 2:969dfa4f2436 | 95 | void FlightController::setRollStabPidParameters(PidWrapper::PidParameter pidParameters) |
joe4465 | 2:969dfa4f2436 | 96 | { |
joe4465 | 2:969dfa4f2436 | 97 | _stabController->setRollStabPidParameters(pidParameters); |
joe4465 | 2:969dfa4f2436 | 98 | _configFileWrapper.setRollStabParameters(pidParameters); |
joe4465 | 2:969dfa4f2436 | 99 | saveSettings(); |
joe4465 | 0:c6a85bb2a827 | 100 | } |
joe4465 | 2:969dfa4f2436 | 101 | |
joe4465 | 2:969dfa4f2436 | 102 | void FlightController::setYawRatePidParameters(PidWrapper::PidParameter pidParameters) |
joe4465 | 2:969dfa4f2436 | 103 | { |
joe4465 | 2:969dfa4f2436 | 104 | _stabController->setYawRatePidParameters(pidParameters); |
joe4465 | 2:969dfa4f2436 | 105 | _rateController->setYawRatePidParameters(pidParameters); |
joe4465 | 2:969dfa4f2436 | 106 | _configFileWrapper.setYawRateParameters(pidParameters); |
joe4465 | 2:969dfa4f2436 | 107 | saveSettings(); |
joe4465 | 2:969dfa4f2436 | 108 | } |
joe4465 | 2:969dfa4f2436 | 109 | |
joe4465 | 2:969dfa4f2436 | 110 | void FlightController::setPitchRatePidParameters(PidWrapper::PidParameter pidParameters) |
joe4465 | 2:969dfa4f2436 | 111 | { |
joe4465 | 2:969dfa4f2436 | 112 | _stabController->setPitchRatePidParameters(pidParameters); |
joe4465 | 2:969dfa4f2436 | 113 | _rateController->setPitchRatePidParameters(pidParameters); |
joe4465 | 2:969dfa4f2436 | 114 | _configFileWrapper.setPitchRateParameters(pidParameters); |
joe4465 | 2:969dfa4f2436 | 115 | saveSettings(); |
joe4465 | 2:969dfa4f2436 | 116 | } |
joe4465 | 2:969dfa4f2436 | 117 | |
joe4465 | 2:969dfa4f2436 | 118 | void FlightController::setRollRatePidParameters(PidWrapper::PidParameter pidParameters) |
joe4465 | 2:969dfa4f2436 | 119 | { |
joe4465 | 2:969dfa4f2436 | 120 | _stabController->setRollRatePidParameters(pidParameters); |
joe4465 | 2:969dfa4f2436 | 121 | _rateController->setRollRatePidParameters(pidParameters); |
joe4465 | 2:969dfa4f2436 | 122 | _configFileWrapper.setRollRateParameters(pidParameters); |
joe4465 | 2:969dfa4f2436 | 123 | saveSettings(); |
joe4465 | 2:969dfa4f2436 | 124 | } |
joe4465 | 2:969dfa4f2436 | 125 | |
joe4465 | 2:969dfa4f2436 | 126 | void FlightController::saveSettings() |
joe4465 | 2:969dfa4f2436 | 127 | { |
joe4465 | 2:969dfa4f2436 | 128 | Status::State state = _status.getState(); |
joe4465 | 2:969dfa4f2436 | 129 | if(state == Status::STANDBY || state == Status::PREFLIGHT) |
joe4465 | 2:969dfa4f2436 | 130 | { |
joe4465 | 2:969dfa4f2436 | 131 | _sensors.enable(false); |
joe4465 | 2:969dfa4f2436 | 132 | _configFileWrapper.saveSettings(); |
joe4465 | 2:969dfa4f2436 | 133 | _sensors.enable(true); |
joe4465 | 2:969dfa4f2436 | 134 | } |
joe4465 | 2:969dfa4f2436 | 135 | } |
joe4465 | 2:969dfa4f2436 | 136 | |
joe4465 | 2:969dfa4f2436 | 137 | PidWrapper::RatePidState FlightController::getRatePidState() |
joe4465 | 2:969dfa4f2436 | 138 | { |
joe4465 | 2:969dfa4f2436 | 139 | return _rateController->getRatePidState(); |
joe4465 | 2:969dfa4f2436 | 140 | } |
joe4465 | 2:969dfa4f2436 | 141 | |
joe4465 | 2:969dfa4f2436 | 142 | PidWrapper::StabPidState FlightController::getStabPidState() |
joe4465 | 2:969dfa4f2436 | 143 | { |
joe4465 | 2:969dfa4f2436 | 144 | return _stabController->getStabPidState(); |
joe4465 | 2:969dfa4f2436 | 145 | } |