Status.cpp

00001 #include "Status.h"
00002 
00003 Status::Status()
00004 {
00005     setFlightMode(STAB);
00006     setBaseStationMode(STATUS);
00007     setNavigationMode(NONE);
00008     setBatteryLevel(0);
00009     setRcConnected(false);
00010     setArmed(false);
00011     setMotorsSpinning(false);
00012     setDeadZone(false);
00013     setInitialised(false);
00014     DEBUG("Status initialised\r\n");    
00015 }
00016 
00017 Status::~Status(){}
00018 
00019 bool Status::update()
00020 {
00021          if(getInitialised() == false && getRcConnected() == false) setState(Status::PREFLIGHT);
00022     else if(getInitialised() == true && getRcConnected() == false) setState(Status::PREFLIGHT);
00023     else if(getInitialised() == true && getRcConnected() == true && getArmed() == false) setState(Status::STANDBY);
00024     else if(getInitialised() == true && getRcConnected() == true && getArmed() == true && getMotorsSpinning() == false) setState(Status::GROUND_READY);
00025     else if(getInitialised() == true && getRcConnected() == true && getArmed() == true && getMotorsSpinning() == true && getNavigationMode() == Status::NONE) setState(Status::MANUAL);
00026     else if(getInitialised() == true && getRcConnected() == true && getArmed() == true && getMotorsSpinning() == true && getNavigationMode() == Status::POSITION_HOLD) setState(Status::AUTO);
00027     else if(getInitialised() == true && getRcConnected() == true && getArmed() == true && getMotorsSpinning() == true && getNavigationMode() == Status::ALTITUDE_HOLD) setState(Status::STABILISED);
00028 
00029     //else setState(Status::ERROR); 
00030     
00031     return true;
00032 }
00033 
00034 bool Status::setState(State state)
00035 {
00036     switch(state)
00037     {
00038         case PREFLIGHT:
00039             if(_state != Status::PREFLIGHT)
00040             {
00041                 _state = PREFLIGHT;
00042                 _statusLights.clear();
00043                 DEBUG("State set to PREFLIGHT\r\n");
00044                 return true;
00045             }
00046             _statusLights.preFlight();
00047             return true;
00048               
00049         case STANDBY:
00050             if(_state != Status::STANDBY)
00051             {
00052                 _state = STANDBY;
00053                 _statusLights.clear();
00054                 DEBUG("State set to STANDBY\r\n");
00055                 return true;
00056             }
00057             _statusLights.standby();
00058             return true;
00059             
00060         case GROUND_READY:
00061             if(_state != Status::GROUND_READY)
00062             {
00063                 _state = GROUND_READY;
00064                 _statusLights.clear();
00065                 DEBUG("State set to GROUND_READY\r\n");
00066                 return true;
00067             }
00068             _statusLights.groundReady();
00069             return true;
00070                  
00071         case MANUAL:
00072             if(_state != Status::MANUAL)
00073             {
00074                 _state = MANUAL;
00075                 _statusLights.clear();
00076                 DEBUG("State set to MANUAL\r\n");
00077                 return true;   
00078             }
00079             _statusLights.flying();
00080             return true;              
00081         
00082         case STABILISED:
00083             if(_state != Status::STABILISED)
00084             {
00085                 _state = STABILISED;
00086                 _statusLights.clear();
00087                 DEBUG("State set to STABILISED\r\n");
00088                 return true;   
00089             }
00090             _statusLights.flying();
00091             return true;  
00092               
00093         
00094         case AUTO:
00095             
00096             return true;
00097               
00098             
00099         case ERROR:
00100             if(_state != Status::ERROR)
00101             {
00102                 _state = Status::ERROR;
00103                 _statusLights.clear();
00104                 DEBUG("State set to ERROR\r\n");
00105                 return true;
00106             }
00107             _statusLights.error();
00108             return true;
00109               
00110         default:
00111             
00112             return false;
00113               
00114     }    
00115 }
00116 
00117 Status::State Status::getState()
00118 {
00119     return _state;    
00120 }
00121 
00122 bool Status::setFlightMode(FlightMode flightMode)
00123 {
00124     if(flightMode != _flightMode)
00125     {
00126         _flightMode = flightMode;
00127         DEBUG("Flight mode set to %d\r\n", _flightMode);
00128         return true;
00129     }
00130     else return false;
00131 }
00132 
00133 Status::FlightMode Status::getFlightMode()
00134 {
00135     return _flightMode;
00136 }
00137 
00138 bool Status::setNavigationMode(NavigationMode navigationMode)
00139 {
00140     if(navigationMode != _navigationMode)
00141     {
00142         _navigationMode = navigationMode;
00143         DEBUG("Navigation mode set to %d\r\n", _navigationMode);
00144         return true;
00145     }
00146     else return false;
00147 }
00148 
00149 Status::NavigationMode Status::getNavigationMode()
00150 {
00151     return _navigationMode;
00152 }
00153 
00154 bool Status::setBaseStationMode(BaseStationMode baseStationMode)
00155 {
00156     if(baseStationMode != _baseStationMode)
00157     {
00158         _baseStationMode = baseStationMode;
00159         DEBUG("Base station mode set to %d\r\n", _baseStationMode);
00160         return true;
00161     }
00162     return false;
00163 }
00164 
00165 Status::BaseStationMode Status::getBaseStationMode()
00166 {
00167     return _baseStationMode;
00168 }
00169 
00170 bool Status::setBatteryLevel(double batteryLevel)
00171 {
00172     _batteryLevel = batteryLevel;
00173     return true;
00174 }
00175 
00176 double Status::getBatteryLevel()
00177 {
00178     return _batteryLevel;
00179 }
00180 
00181 bool Status::setArmed(bool armed)
00182 {
00183     if(armed != _armed)
00184     {
00185         if(armed == false)
00186         {
00187             _armed = armed;
00188             DEBUG("Armed set to %d\r\n", _armed);
00189             return true;
00190         }
00191         else if (armed == true && _navigationMode == Status::NONE && getMotorsSpinning() == false)
00192         {
00193             _armed = armed;
00194             DEBUG("Armed set to %d\r\n", _armed);
00195             return true;
00196         }
00197         else if (armed == true && _navigationMode == Status::ALTITUDE_HOLD && getMotorsSpinning() == false)
00198         {
00199             _armed = armed;
00200             DEBUG("Armed set to %d\r\n", _armed);
00201             return true;
00202         }
00203     }
00204     return false;
00205 }
00206 
00207 bool Status::getArmed()
00208 {
00209     return _armed;
00210 }
00211 
00212 bool Status::setInitialised(bool initialised)
00213 {
00214     if(initialised != _initialised)
00215     {
00216         _initialised = initialised;
00217         DEBUG("Initialised set to %d\r\n", _initialised);
00218         return true;
00219     }
00220     else return false;
00221 }
00222 
00223 bool Status::getInitialised()
00224 {
00225     return _initialised;
00226 }
00227 
00228 bool Status::setRcConnected(bool rcConnected)
00229 {
00230     if(rcConnected != _rcConnected)
00231     {
00232         _rcConnected = rcConnected;
00233         if(_rcConnected == false)
00234         {
00235             setArmed(false);
00236             setMotorsSpinning(false);
00237             setDeadZone(false);
00238         }
00239         DEBUG("Rc connected set to %d\r\n", _rcConnected);
00240         return true;
00241     }
00242     else return false;
00243 }
00244 
00245 bool Status::getRcConnected()
00246 {
00247     return _rcConnected;
00248 }
00249 
00250 bool Status::setMotorsSpinning(bool flying)
00251 {
00252     if(flying != _flying)
00253     {
00254         _flying = flying;
00255         DEBUG("Flying set to %d\r\n", _flying);
00256         return true;
00257     }
00258     else return false;
00259 }
00260 
00261 bool Status::getMotorsSpinning()
00262 {
00263     return _flying;
00264 }
00265 
00266 bool Status::setDeadZone(bool deadZone)
00267 {
00268     if(deadZone != _deadZone)
00269     {
00270         _deadZone = deadZone;
00271         DEBUG("Dead zone set to %d\r\n", _deadZone);
00272         return true;
00273     }
00274     else return false;
00275 }
00276 
00277 bool Status::getDeadZone()
00278 {
00279     return _deadZone;
00280 }