![](/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
- Committer:
- joe4465
- Date:
- 2015-03-04
- Revision:
- 0:c6a85bb2a827
- Child:
- 2:969dfa4f2436
File content as of revision 0:c6a85bb2a827:
#include "FlightController.h" FlightController::FlightController(){} FlightController::~FlightController(){} bool FlightController::initialise(Status& status, Sensors& sensors, NavigationController& navigationController, PinName motor1, PinName motor2, PinName motor3, PinName motor4) { _status = status; _motorMixer.initialise(motor1, motor2, motor3, motor4); _rateController.initialise(sensors, navigationController); _stabController.initialise(sensors, navigationController); _rtosTimer = new RtosTimer(&FlightController::threadStarter, osTimerPeriodic, (void *)0); int updateTime = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000; _rtosTimer->start(updateTime); DEBUG("Flight controller initialised"); return true; } void FlightController::threadStarter(void const *p) { FlightController *instance = (FlightController*)p; instance->threadWorker(); } void FlightController::threadWorker() { //Struct to hold PID outputs PidWrapper::PidOutputs pidOutputs; //Check state is valid Status::State state = _status.getState(); if(state == Status::MANUAL || state == Status::STABILISED || state == Status::AUTO) { //State valid Status::FlightMode flightMode = _status.getFlightMode(); if(flightMode == Status::RATE) { //Rate mode pidOutputs = _rateController.compute(); } else if (flightMode == Status::STAB) { //Stab mode pidOutputs = _stabController.compute(); } else { //Invalid state _status.setState(Status::ABORT); return; } _motorMixer.computePower(pidOutputs, throttle); } //Set motors to armed if state is ground ready else if (state == Status::GROUND_READY) _motorMixer.setPower(MOTORS_ARMED); //Disable motors if state is not valid else _motorMixer.setPower(MOTORS_OFF); }