![](/media/cache/profiles/8158fa3e4de806e614f7ff02e7b22fde.jpg.50x50_q85.png)
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@0:c6a85bb2a827, 2015-03-04 (annotated)
- Committer:
- joe4465
- Date:
- Wed Mar 04 18:50:37 2015 +0000
- Revision:
- 0:c6a85bb2a827
- Child:
- 2:969dfa4f2436
New version of quadcopter software, written following OO principles
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 | 0:c6a85bb2a827 | 3 | FlightController::FlightController(){} |
joe4465 | 0:c6a85bb2a827 | 4 | |
joe4465 | 0:c6a85bb2a827 | 5 | FlightController::~FlightController(){} |
joe4465 | 0:c6a85bb2a827 | 6 | |
joe4465 | 0:c6a85bb2a827 | 7 | bool FlightController::initialise(Status& status, Sensors& sensors, NavigationController& navigationController, PinName motor1, PinName motor2, PinName motor3, PinName motor4) |
joe4465 | 0:c6a85bb2a827 | 8 | { |
joe4465 | 0:c6a85bb2a827 | 9 | _status = status; |
joe4465 | 0:c6a85bb2a827 | 10 | |
joe4465 | 0:c6a85bb2a827 | 11 | _motorMixer.initialise(motor1, motor2, motor3, motor4); |
joe4465 | 0:c6a85bb2a827 | 12 | _rateController.initialise(sensors, navigationController); |
joe4465 | 0:c6a85bb2a827 | 13 | _stabController.initialise(sensors, navigationController); |
joe4465 | 0:c6a85bb2a827 | 14 | |
joe4465 | 0:c6a85bb2a827 | 15 | _rtosTimer = new RtosTimer(&FlightController::threadStarter, osTimerPeriodic, (void *)0); |
joe4465 | 0:c6a85bb2a827 | 16 | int updateTime = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000; |
joe4465 | 0:c6a85bb2a827 | 17 | _rtosTimer->start(updateTime); |
joe4465 | 0:c6a85bb2a827 | 18 | DEBUG("Flight controller initialised"); |
joe4465 | 0:c6a85bb2a827 | 19 | return true; |
joe4465 | 0:c6a85bb2a827 | 20 | } |
joe4465 | 0:c6a85bb2a827 | 21 | |
joe4465 | 0:c6a85bb2a827 | 22 | void FlightController::threadStarter(void const *p) |
joe4465 | 0:c6a85bb2a827 | 23 | { |
joe4465 | 0:c6a85bb2a827 | 24 | FlightController *instance = (FlightController*)p; |
joe4465 | 0:c6a85bb2a827 | 25 | instance->threadWorker(); |
joe4465 | 0:c6a85bb2a827 | 26 | } |
joe4465 | 0:c6a85bb2a827 | 27 | |
joe4465 | 0:c6a85bb2a827 | 28 | void FlightController::threadWorker() |
joe4465 | 0:c6a85bb2a827 | 29 | { |
joe4465 | 0:c6a85bb2a827 | 30 | //Struct to hold PID outputs |
joe4465 | 0:c6a85bb2a827 | 31 | PidWrapper::PidOutputs pidOutputs; |
joe4465 | 0:c6a85bb2a827 | 32 | |
joe4465 | 0:c6a85bb2a827 | 33 | //Check state is valid |
joe4465 | 0:c6a85bb2a827 | 34 | Status::State state = _status.getState(); |
joe4465 | 0:c6a85bb2a827 | 35 | if(state == Status::MANUAL || state == Status::STABILISED || state == Status::AUTO) |
joe4465 | 0:c6a85bb2a827 | 36 | { |
joe4465 | 0:c6a85bb2a827 | 37 | //State valid |
joe4465 | 0:c6a85bb2a827 | 38 | Status::FlightMode flightMode = _status.getFlightMode(); |
joe4465 | 0:c6a85bb2a827 | 39 | if(flightMode == Status::RATE) |
joe4465 | 0:c6a85bb2a827 | 40 | { |
joe4465 | 0:c6a85bb2a827 | 41 | //Rate mode |
joe4465 | 0:c6a85bb2a827 | 42 | pidOutputs = _rateController.compute(); |
joe4465 | 0:c6a85bb2a827 | 43 | } |
joe4465 | 0:c6a85bb2a827 | 44 | else if (flightMode == Status::STAB) |
joe4465 | 0:c6a85bb2a827 | 45 | { |
joe4465 | 0:c6a85bb2a827 | 46 | //Stab mode |
joe4465 | 0:c6a85bb2a827 | 47 | pidOutputs = _stabController.compute(); |
joe4465 | 0:c6a85bb2a827 | 48 | } |
joe4465 | 0:c6a85bb2a827 | 49 | else |
joe4465 | 0:c6a85bb2a827 | 50 | { |
joe4465 | 0:c6a85bb2a827 | 51 | //Invalid state |
joe4465 | 0:c6a85bb2a827 | 52 | _status.setState(Status::ABORT); |
joe4465 | 0:c6a85bb2a827 | 53 | return; |
joe4465 | 0:c6a85bb2a827 | 54 | } |
joe4465 | 0:c6a85bb2a827 | 55 | |
joe4465 | 0:c6a85bb2a827 | 56 | _motorMixer.computePower(pidOutputs, throttle); |
joe4465 | 0:c6a85bb2a827 | 57 | } |
joe4465 | 0:c6a85bb2a827 | 58 | //Set motors to armed if state is ground ready |
joe4465 | 0:c6a85bb2a827 | 59 | else if (state == Status::GROUND_READY) _motorMixer.setPower(MOTORS_ARMED); |
joe4465 | 0:c6a85bb2a827 | 60 | //Disable motors if state is not valid |
joe4465 | 0:c6a85bb2a827 | 61 | else _motorMixer.setPower(MOTORS_OFF); |
joe4465 | 0:c6a85bb2a827 | 62 | } |