FlightController.cpp

00001 #include "FlightController.h"
00002 
00003 FlightController::FlightController(Status& status, Sensors& sensors, NavigationController& navigationController, ConfigFileWrapper& configFileWrapper, PinName motor1, PinName motor2, PinName motor3, PinName motor4)
00004     : _status(status), _sensors(sensors), _navigationController(navigationController), _configFileWrapper(configFileWrapper)
00005 {
00006     _motorMixer = new MotorMixer(motor1, motor2, motor3, motor4);
00007     _rateController = new RateController(_sensors, _navigationController, _configFileWrapper);
00008     _stabController = new StabController(_sensors, _navigationController, _configFileWrapper);   
00009 
00010     _rtosTimer = new RtosTimer(&FlightController::threadStarter, osTimerPeriodic, (void*)this);
00011     int updateTime = (1.0 / (float)FLIGHT_CONTROLLER_FREQUENCY) * 1000;
00012     _rtosTimer->start(updateTime);
00013     
00014     DEBUG("Flight controller initialised\r\n");
00015 }
00016 
00017 FlightController::~FlightController(){}
00018 
00019 void FlightController::threadStarter(void const *p)
00020 {
00021     FlightController *instance = (FlightController*)p;
00022     instance->threadWorker();
00023 }
00024 
00025 void FlightController::threadWorker()
00026 {   
00027     Status::FlightMode flightMode = _status.getFlightMode();
00028     
00029     _sensors.updateImu();
00030     
00031     if(flightMode == Status::RATE)
00032     {
00033         //Rate mode  
00034         _pidOutputs = _rateController->compute(); 
00035     }
00036     else if (flightMode == Status::STAB)
00037     {
00038         //Stab mode
00039         _pidOutputs = _stabController->compute();  
00040     }
00041       
00042     Status::State state = _status.getState();
00043     NavigationController::SetPoint setPoints = _navigationController.getSetPoint();
00044     
00045     if(state == Status::MANUAL || state == Status::STABILISED || state == Status::AUTO)  
00046     {
00047         _motorMixer->computePower(_pidOutputs, setPoints.throttle);
00048     }      
00049     else if(state == Status::GROUND_READY) 
00050     {
00051         _motorMixer->setPower(MOTORS_ARMED);
00052         _rateController->reset();
00053         _stabController->reset(); 
00054     }
00055     else if(state == Status::STANDBY)
00056     {
00057         _motorMixer->setPower(MOTORS_OFF);
00058         _rateController->reset();
00059         _stabController->reset();
00060     }
00061     //Disable motors if state is not valid
00062     else _motorMixer->setPower(MOTORS_OFF);
00063 }
00064 
00065 MotorMixer::MotorPower FlightController::getMotorPower()
00066 {
00067     return _motorMixer->getMotorPower();
00068 }
00069 
00070 PidWrapper::PidOutput FlightController::getPidOutputs()
00071 {
00072     return _pidOutputs;  
00073 }
00074 
00075 PidWrapper::FlightControllerPidParameters FlightController::getPidParameters()
00076 {
00077     return _stabController->getPidParameters();
00078 }
00079 
00080 void FlightController::setYawStabPidParameters(PidWrapper::PidParameter pidParameters)
00081 {
00082     _stabController->setYawStabPidParameters(pidParameters);
00083     _configFileWrapper.setYawStabParameters(pidParameters); 
00084     Status::State state = _status.getState(); 
00085     saveSettings();
00086 }
00087 
00088 void FlightController::setPitchStabPidParameters(PidWrapper::PidParameter pidParameters)
00089 {
00090     _stabController->setPitchStabPidParameters(pidParameters);
00091     _configFileWrapper.setPitchStabParameters(pidParameters);
00092     saveSettings();
00093 }
00094 
00095 void FlightController::setRollStabPidParameters(PidWrapper::PidParameter pidParameters)
00096 {
00097     _stabController->setRollStabPidParameters(pidParameters);
00098     _configFileWrapper.setRollStabParameters(pidParameters);
00099     saveSettings();
00100 }
00101 
00102 void FlightController::setYawRatePidParameters(PidWrapper::PidParameter pidParameters)
00103 {
00104     _stabController->setYawRatePidParameters(pidParameters);
00105     _rateController->setYawRatePidParameters(pidParameters);
00106     _configFileWrapper.setYawRateParameters(pidParameters);
00107     saveSettings();
00108 }
00109 
00110 void FlightController::setPitchRatePidParameters(PidWrapper::PidParameter pidParameters)
00111 {
00112     _stabController->setPitchRatePidParameters(pidParameters);
00113     _rateController->setPitchRatePidParameters(pidParameters);
00114     _configFileWrapper.setPitchRateParameters(pidParameters);
00115     saveSettings();
00116 }
00117 
00118 void FlightController::setRollRatePidParameters(PidWrapper::PidParameter pidParameters)
00119 {
00120     _stabController->setRollRatePidParameters(pidParameters);
00121     _rateController->setRollRatePidParameters(pidParameters);
00122     _configFileWrapper.setRollRateParameters(pidParameters);
00123     saveSettings();
00124 }
00125 
00126 void FlightController::saveSettings()
00127 {
00128     Status::State state = _status.getState(); 
00129     if(state == Status::STANDBY || state == Status::PREFLIGHT)
00130     {
00131         _sensors.enable(false);
00132         _configFileWrapper.saveSettings();
00133         _sensors.enable(true);
00134     }   
00135 }
00136 
00137 PidWrapper::RatePidState FlightController::getRatePidState()
00138 {
00139     return _rateController->getRatePidState();
00140 }
00141 
00142 PidWrapper::StabPidState FlightController::getStabPidState()
00143 {
00144     return _stabController->getStabPidState();
00145 }