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@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?

UserRevisionLine numberNew 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 }