NavigationController.cpp

00001 #include "NavigationController.h"
00002 
00003 NavigationController::NavigationController(Status& status, Sensors& sensors, Rc& rc, ConfigFileWrapper& configFileWrapper) : 
00004 _status(status), _sensors(sensors), _rc(rc), _configFileWrapper(configFileWrapper)
00005 {
00006     _altitudeController = new AltitudeController(_sensors, _configFileWrapper, _status);
00007     _altitudeHoldPidOutput = 0;
00008     
00009     _thread = new Thread(&NavigationController::threadStarter, this, osPriorityHigh);
00010 
00011     DEBUG("Navigation controller initialised\r\n");    
00012 }
00013 
00014 NavigationController::~NavigationController(){}
00015 
00016 void NavigationController::threadStarter(void const *p)
00017 {
00018     NavigationController *instance = (NavigationController*)p;
00019     instance->threadWorker();
00020 }
00021 
00022 void NavigationController::threadWorker()
00023 {
00024     while(true)
00025     {
00026         //Get latest sensor readings
00027         _sensors.update();
00028         
00029         //Get state
00030         _state = _status.getState();
00031         
00032         //Get navigation mode
00033         _navigationMode = _status.getNavigationMode();
00034         
00035         //Get Rc commands
00036         _mappedRc = _rc.getMappedRc();
00037         
00038         //Get angle to calculate yaw
00039         _angle = _sensors.getAngle();
00040         
00041         //Reset accel data if not flying
00042         if(_state == Status::PREFLIGHT || _state == Status::STANDBY)
00043         {
00044             //Reset accel
00045             _sensors.zeroAccel();
00046             
00047             //Reset Gps
00048             _sensors.zeroPos();
00049         }
00050         
00051         //Update yaw target
00052         if(abs(_mappedRc.yaw) >= 5 || _state == Status::PREFLIGHT || _state == Status::STANDBY) updateYawTarget();
00053              
00054         //Make sure we are initialised         
00055         if(_state != Status::PREFLIGHT)
00056         {            
00057             //Update yaw difference
00058             _setPoints.yawDifference = MOD(_setPoints.yawTarget - _angle.yaw); 
00059             
00060             if(_navigationMode == Status::NONE)
00061             {
00062                 //Motors are directly controlled by rc remote
00063                 if(_mappedRc.throttle >= RC_DEAD_ZONE) _status.setMotorsSpinning(true);
00064                 else _status.setMotorsSpinning(false);
00065                 
00066                 //Update target altitude 
00067                 _setPoints.climbRate = 0;
00068                 updateAltitudeTarget();
00069             }
00070             else if(_navigationMode == Status::ALTITUDE_HOLD)
00071             {
00072                 //Motors are directly controlled by rc remote
00073                 //if(_mappedRc.throttle >= RC_DEAD_ZONE) _status.setMotorsSpinning(true);
00074                 //else _status.setMotorsSpinning(false);
00075                 
00076                 //Check if throttle is in dead zone
00077                 if(_status.getDeadZone() == true) _setPoints.climbRate = 0;
00078                 else
00079                 {
00080                     //Throttle not in dead zone so map to climb rate
00081                     _setPoints.climbRate = map(_mappedRc.throttle, RC_THRUST_MIN, RC_THRUST_MAX, MIN_CLIMB_RATE, MAX_CLIMB_RATE);
00082                     
00083                     //float target = _setPoints.targetAltitude + (_setPoints.climbRate * 0.02);
00084                     
00085                     //Update target altitude
00086                     updateAltitudeTarget();
00087                 }
00088                 
00089                 //If altitude is less than 10cm the directly map the rc throttle stick to the throttle
00090                 //else use the throttle from the altitude PID controller
00091                 if(_altitude.computed > 10) _altitudeHoldPidOutput = _altitudeController->compute(_setPoints.targetAltitude, _setPoints.climbRate);
00092                 else _status.setMotorsSpinning(false);
00093             }
00094            /* else if(_navigationMode == Status::AUTO_LAND)
00095             {
00096                 //Motors are directly controlled by rc remote
00097                 if(_mappedRc.throttle >= RC_DEAD_ZONE) _status.setMotorsSpinning(true);
00098                 else _status.setMotorsSpinning(false);
00099                 
00100                 //Get altitude
00101                 _altitude = _sensors.getAltitude();
00102                 
00103                 if(_altitude.computed > 1000) _setPoints.targetAltitude = 300;
00104                 else if(_altitude.computed < 600) _setPoints.targetAltitude = 100;
00105                 else if(_altitude.computed < 300) _setPoints.targetAltitude = 0;
00106                 
00107                 //If altitude is less than 10 the directly map the rc throttle stick to the throttle
00108                 //else use the throttle from the altitude PID controller
00109                 if(_altitude.computed > 10) _altitudeHoldPidOutput = _altitudeController->compute(_setPoints.targetAltitude, _setPoints.climbRate);
00110                 else _altitudeHoldPidOutput = _mappedRc.throttle;
00111             }*/
00112             
00113             if(_state == Status::STANDBY)  
00114             {
00115                 _setPoints.yaw = 0;
00116                 _setPoints.pitch = 0;
00117                 _setPoints.roll = 0;
00118                 _setPoints.throttle = 0; 
00119                 
00120                 _altitudeController->reset();
00121             }
00122             else if(_state == Status::GROUND_READY)  
00123             {
00124                 _setPoints.yaw = _mappedRc.yaw;
00125                 _setPoints.pitch = _mappedRc.pitch;
00126                 _setPoints.roll = _mappedRc.roll;
00127                 _setPoints.throttle = _mappedRc.throttle; 
00128                 
00129                 _altitudeController->reset();
00130             }
00131             else if(_state == Status::MANUAL)  
00132             {
00133                 _setPoints.yaw = _mappedRc.yaw;
00134                 _setPoints.pitch = _mappedRc.pitch;
00135                 _setPoints.roll = _mappedRc.roll;
00136                 _setPoints.throttle = _mappedRc.throttle; 
00137                 
00138                 _altitudeController->reset();
00139             }
00140             else if(_state == Status::STABILISED)
00141             {
00142                 _setPoints.yaw = _mappedRc.yaw;
00143                 _setPoints.pitch = _mappedRc.pitch;
00144                 _setPoints.roll = _mappedRc.roll;
00145                 _setPoints.throttle = _altitudeHoldPidOutput;
00146             }
00147             else if(_state == Status::AUTO)
00148             {
00149                 //Waypoint navigation
00150             }
00151         }
00152         //Not initialised
00153         else
00154         {
00155             _setPoints.yaw = 0;
00156             _setPoints.pitch = 0;
00157             _setPoints.roll = 0;
00158             _setPoints.throttle = 0;
00159             
00160             _altitudeController->reset();
00161         }
00162         
00163         Thread::wait(20);
00164     }
00165 }
00166 
00167 NavigationController::SetPoint NavigationController::getSetPoint()
00168 {
00169     return _setPoints;
00170 }
00171 
00172 void NavigationController::updateYawTarget()
00173 {
00174    _setPoints.yawTarget = _angle.yaw;
00175 }
00176 
00177 void NavigationController::updateAltitudeTarget()
00178 {
00179     _altitude = _sensors.getAltitude();
00180     _setPoints.targetAltitude = _altitude.computed;
00181     if(_setPoints.targetAltitude <= ALTITUDE_MIN) _setPoints.targetAltitude = ALTITUDE_MIN;
00182     else if(_setPoints.targetAltitude >= ALTITUDE_MAX) _setPoints.targetAltitude = ALTITUDE_MAX;
00183 }
00184   
00185 double NavigationController::map(double input, double inputMin, double inputMax, double outputMin, double outputMax)
00186 {
00187     return (input - inputMin) * (outputMax - outputMin) / (inputMax - inputMin) + outputMin;
00188 }  
00189 
00190 PidWrapper::NavigationControllerPidParameters NavigationController::getPidParameters()
00191 {
00192     return _altitudeController->getPidParameters();
00193 }
00194 
00195 void NavigationController::setAltitudeRatePidParameters(PidWrapper::PidParameter pidParameters)
00196 {
00197     _altitudeController->setAltitudeRatePidParameters(pidParameters);
00198     _configFileWrapper.setAltitudeRateParameters(pidParameters);
00199     saveSettings();
00200 }
00201 
00202 void NavigationController::setAltitudeStabPidParameters(PidWrapper::PidParameter pidParameters)
00203 {
00204     _altitudeController->setAltitudeStabPidParameters(pidParameters);
00205     _configFileWrapper.setAltitudeStabParameters(pidParameters);
00206     saveSettings();
00207 }
00208 
00209 void NavigationController::saveSettings()
00210 {
00211     Status::State state = _status.getState(); 
00212     if(state == Status::STANDBY || state == Status::PREFLIGHT)
00213     {
00214         _sensors.enable(false);
00215         _configFileWrapper.saveSettings();
00216         _sensors.enable(true);
00217     }   
00218 }