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/FlightController.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 "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 }