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

Status/Status.cpp@4:9ffbf9101992, 2015-05-08 (annotated)

Committer:
joe4465
Date:
Fri May 08 09:07:38 2015 +0000
Revision:
4:9ffbf9101992
Parent:
3:4823d6750629 
End of FYP

Who changed what in which revision?

UserRevisionLine numberNew contents of line
joe4465 0:c6a85bb2a827 1 #include "Status.h"
joe4465 0:c6a85bb2a827 2
joe4465 2:969dfa4f2436 3 Status::Status()
joe4465 2:969dfa4f2436 4 {
joe4465 2:969dfa4f2436 5 setFlightMode(STAB);
joe4465 2:969dfa4f2436 6 setBaseStationMode(STATUS);
joe4465 2:969dfa4f2436 7 setNavigationMode(NONE);
joe4465 2:969dfa4f2436 8 setBatteryLevel(0);
joe4465 2:969dfa4f2436 9 setRcConnected(false);
joe4465 2:969dfa4f2436 10 setArmed(false);
joe4465 2:969dfa4f2436 11 setMotorsSpinning(false);
joe4465 2:969dfa4f2436 12 setDeadZone(false);
joe4465 2:969dfa4f2436 13 setInitialised(false);
joe4465 2:969dfa4f2436 14 DEBUG("Status initialised\r\n");
joe4465 2:969dfa4f2436 15 }
joe4465 0:c6a85bb2a827 16
joe4465 0:c6a85bb2a827 17 Status::~Status(){}
joe4465 0:c6a85bb2a827 18
joe4465 2:969dfa4f2436 19 bool Status::update()
joe4465 0:c6a85bb2a827 20 {
joe4465 2:969dfa4f2436 21 if(getInitialised() == false && getRcConnected() == false) setState(Status::PREFLIGHT);
joe4465 2:969dfa4f2436 22 else if(getInitialised() == true && getRcConnected() == false) setState(Status::PREFLIGHT);
joe4465 2:969dfa4f2436 23 else if(getInitialised() == true && getRcConnected() == true && getArmed() == false) setState(Status::STANDBY);
joe4465 2:969dfa4f2436 24 else if(getInitialised() == true && getRcConnected() == true && getArmed() == true && getMotorsSpinning() == false) setState(Status::GROUND_READY);
joe4465 2:969dfa4f2436 25 else if(getInitialised() == true && getRcConnected() == true && getArmed() == true && getMotorsSpinning() == true && getNavigationMode() == Status::NONE) setState(Status::MANUAL);
joe4465 2:969dfa4f2436 26 else if(getInitialised() == true && getRcConnected() == true && getArmed() == true && getMotorsSpinning() == true && getNavigationMode() == Status::POSITION_HOLD) setState(Status::AUTO);
joe4465 2:969dfa4f2436 27 else if(getInitialised() == true && getRcConnected() == true && getArmed() == true && getMotorsSpinning() == true && getNavigationMode() == Status::ALTITUDE_HOLD) setState(Status::STABILISED);
joe4465 2:969dfa4f2436 28
joe4465 2:969dfa4f2436 29 //else setState(Status::ERROR);
joe4465 2:969dfa4f2436 30
joe4465 2:969dfa4f2436 31 return true;
joe4465 0:c6a85bb2a827 32 }
joe4465 0:c6a85bb2a827 33
joe4465 0:c6a85bb2a827 34 bool Status::setState(State state)
joe4465 0:c6a85bb2a827 35 {
joe4465 0:c6a85bb2a827 36 switch(state)
joe4465 0:c6a85bb2a827 37 {
joe4465 0:c6a85bb2a827 38 case PREFLIGHT:
joe4465 2:969dfa4f2436 39 if(_state != Status::PREFLIGHT)
joe4465 2:969dfa4f2436 40 {
joe4465 2:969dfa4f2436 41 _state = PREFLIGHT;
joe4465 2:969dfa4f2436 42 _statusLights.clear();
joe4465 2:969dfa4f2436 43 DEBUG("State set to PREFLIGHT\r\n");
joe4465 2:969dfa4f2436 44 return true;
joe4465 2:969dfa4f2436 45 }
joe4465 2:969dfa4f2436 46 _statusLights.preFlight();
joe4465 0:c6a85bb2a827 47 return true;
joe4465 0:c6a85bb2a827 48
joe4465 0:c6a85bb2a827 49 case STANDBY:
joe4465 2:969dfa4f2436 50 if(_state != Status::STANDBY)
joe4465 2:969dfa4f2436 51 {
joe4465 2:969dfa4f2436 52 _state = STANDBY;
joe4465 2:969dfa4f2436 53 _statusLights.clear();
joe4465 2:969dfa4f2436 54 DEBUG("State set to STANDBY\r\n");
joe4465 2:969dfa4f2436 55 return true;
joe4465 2:969dfa4f2436 56 }
joe4465 2:969dfa4f2436 57 _statusLights.standby();
joe4465 0:c6a85bb2a827 58 return true;
joe4465 0:c6a85bb2a827 59
joe4465 0:c6a85bb2a827 60 case GROUND_READY:
joe4465 2:969dfa4f2436 61 if(_state != Status::GROUND_READY)
joe4465 2:969dfa4f2436 62 {
joe4465 2:969dfa4f2436 63 _state = GROUND_READY;
joe4465 2:969dfa4f2436 64 _statusLights.clear();
joe4465 2:969dfa4f2436 65 DEBUG("State set to GROUND_READY\r\n");
joe4465 2:969dfa4f2436 66 return true;
joe4465 2:969dfa4f2436 67 }
joe4465 2:969dfa4f2436 68 _statusLights.groundReady();
joe4465 0:c6a85bb2a827 69 return true;
joe4465 2:969dfa4f2436 70
joe4465 0:c6a85bb2a827 71 case MANUAL:
joe4465 2:969dfa4f2436 72 if(_state != Status::MANUAL)
joe4465 2:969dfa4f2436 73 {
joe4465 2:969dfa4f2436 74 _state = MANUAL;
joe4465 2:969dfa4f2436 75 _statusLights.clear();
joe4465 2:969dfa4f2436 76 DEBUG("State set to MANUAL\r\n");
joe4465 2:969dfa4f2436 77 return true;
joe4465 2:969dfa4f2436 78 }
joe4465 2:969dfa4f2436 79 _statusLights.flying();
joe4465 2:969dfa4f2436 80 return true;
joe4465 0:c6a85bb2a827 81
joe4465 0:c6a85bb2a827 82 case STABILISED:
joe4465 2:969dfa4f2436 83 if(_state != Status::STABILISED)
joe4465 2:969dfa4f2436 84 {
joe4465 2:969dfa4f2436 85 _state = STABILISED;
joe4465 2:969dfa4f2436 86 _statusLights.clear();
joe4465 2:969dfa4f2436 87 DEBUG("State set to STABILISED\r\n");
joe4465 2:969dfa4f2436 88 return true;
joe4465 2:969dfa4f2436 89 }
joe4465 2:969dfa4f2436 90 _statusLights.flying();
joe4465 2:969dfa4f2436 91 return true;
joe4465 0:c6a85bb2a827 92
joe4465 0:c6a85bb2a827 93
joe4465 0:c6a85bb2a827 94 case AUTO:
joe4465 0:c6a85bb2a827 95
joe4465 0:c6a85bb2a827 96 return true;
joe4465 0:c6a85bb2a827 97
joe4465 0:c6a85bb2a827 98
joe4465 2:969dfa4f2436 99 case ERROR:
joe4465 2:969dfa4f2436 100 if(_state != Status::ERROR)
joe4465 2:969dfa4f2436 101 {
joe4465 2:969dfa4f2436 102 _state = Status::ERROR;
joe4465 2:969dfa4f2436 103 _statusLights.clear();
joe4465 2:969dfa4f2436 104 DEBUG("State set to ERROR\r\n");
joe4465 2:969dfa4f2436 105 return true;
joe4465 2:969dfa4f2436 106 }
joe4465 2:969dfa4f2436 107 _statusLights.error();
joe4465 0:c6a85bb2a827 108 return true;
joe4465 0:c6a85bb2a827 109
joe4465 0:c6a85bb2a827 110 default:
joe4465 0:c6a85bb2a827 111
joe4465 0:c6a85bb2a827 112 return false;
joe4465 0:c6a85bb2a827 113
joe4465 0:c6a85bb2a827 114 }
joe4465 0:c6a85bb2a827 115 }
joe4465 0:c6a85bb2a827 116
joe4465 0:c6a85bb2a827 117 Status::State Status::getState()
joe4465 0:c6a85bb2a827 118 {
joe4465 0:c6a85bb2a827 119 return _state;
joe4465 0:c6a85bb2a827 120 }
joe4465 0:c6a85bb2a827 121
joe4465 0:c6a85bb2a827 122 bool Status::setFlightMode(FlightMode flightMode)
joe4465 0:c6a85bb2a827 123 {
joe4465 2:969dfa4f2436 124 if(flightMode != _flightMode)
joe4465 2:969dfa4f2436 125 {
joe4465 2:969dfa4f2436 126 _flightMode = flightMode;
joe4465 2:969dfa4f2436 127 DEBUG("Flight mode set to %d\r\n", _flightMode);
joe4465 2:969dfa4f2436 128 return true;
joe4465 2:969dfa4f2436 129 }
joe4465 2:969dfa4f2436 130 else return false;
joe4465 0:c6a85bb2a827 131 }
joe4465 0:c6a85bb2a827 132
joe4465 0:c6a85bb2a827 133 Status::FlightMode Status::getFlightMode()
joe4465 0:c6a85bb2a827 134 {
joe4465 0:c6a85bb2a827 135 return _flightMode;
joe4465 0:c6a85bb2a827 136 }
joe4465 0:c6a85bb2a827 137
joe4465 2:969dfa4f2436 138 bool Status::setNavigationMode(NavigationMode navigationMode)
joe4465 2:969dfa4f2436 139 {
joe4465 2:969dfa4f2436 140 if(navigationMode != _navigationMode)
joe4465 2:969dfa4f2436 141 {
joe4465 2:969dfa4f2436 142 _navigationMode = navigationMode;
joe4465 2:969dfa4f2436 143 DEBUG("Navigation mode set to %d\r\n", _navigationMode);
joe4465 2:969dfa4f2436 144 return true;
joe4465 2:969dfa4f2436 145 }
joe4465 2:969dfa4f2436 146 else return false;
joe4465 2:969dfa4f2436 147 }
joe4465 2:969dfa4f2436 148
joe4465 2:969dfa4f2436 149 Status::NavigationMode Status::getNavigationMode()
joe4465 2:969dfa4f2436 150 {
joe4465 2:969dfa4f2436 151 return _navigationMode;
joe4465 2:969dfa4f2436 152 }
joe4465 2:969dfa4f2436 153
joe4465 0:c6a85bb2a827 154 bool Status::setBaseStationMode(BaseStationMode baseStationMode)
joe4465 0:c6a85bb2a827 155 {
joe4465 2:969dfa4f2436 156 if(baseStationMode != _baseStationMode)
joe4465 2:969dfa4f2436 157 {
joe4465 2:969dfa4f2436 158 _baseStationMode = baseStationMode;
joe4465 2:969dfa4f2436 159 DEBUG("Base station mode set to %d\r\n", _baseStationMode);
joe4465 2:969dfa4f2436 160 return true;
joe4465 2:969dfa4f2436 161 }
joe4465 2:969dfa4f2436 162 return false;
joe4465 0:c6a85bb2a827 163 }
joe4465 0:c6a85bb2a827 164
joe4465 0:c6a85bb2a827 165 Status::BaseStationMode Status::getBaseStationMode()
joe4465 0:c6a85bb2a827 166 {
joe4465 0:c6a85bb2a827 167 return _baseStationMode;
joe4465 0:c6a85bb2a827 168 }
joe4465 0:c6a85bb2a827 169
joe4465 2:969dfa4f2436 170 bool Status::setBatteryLevel(double batteryLevel)
joe4465 0:c6a85bb2a827 171 {
joe4465 0:c6a85bb2a827 172 _batteryLevel = batteryLevel;
joe4465 0:c6a85bb2a827 173 return true;
joe4465 0:c6a85bb2a827 174 }
joe4465 0:c6a85bb2a827 175
joe4465 2:969dfa4f2436 176 double Status::getBatteryLevel()
joe4465 0:c6a85bb2a827 177 {
joe4465 0:c6a85bb2a827 178 return _batteryLevel;
joe4465 0:c6a85bb2a827 179 }
joe4465 0:c6a85bb2a827 180
joe4465 0:c6a85bb2a827 181 bool Status::setArmed(bool armed)
joe4465 0:c6a85bb2a827 182 {
joe4465 2:969dfa4f2436 183 if(armed != _armed)
joe4465 2:969dfa4f2436 184 {
joe4465 2:969dfa4f2436 185 if(armed == false)
joe4465 2:969dfa4f2436 186 {
joe4465 2:969dfa4f2436 187 _armed = armed;
joe4465 2:969dfa4f2436 188 DEBUG("Armed set to %d\r\n", _armed);
joe4465 2:969dfa4f2436 189 return true;
joe4465 2:969dfa4f2436 190 }
joe4465 2:969dfa4f2436 191 else if (armed == true && _navigationMode == Status::NONE && getMotorsSpinning() == false)
joe4465 2:969dfa4f2436 192 {
joe4465 2:969dfa4f2436 193 _armed = armed;
joe4465 2:969dfa4f2436 194 DEBUG("Armed set to %d\r\n", _armed);
joe4465 2:969dfa4f2436 195 return true;
joe4465 2:969dfa4f2436 196 }
joe4465 3:4823d6750629 197 else if (armed == true && _navigationMode == Status::ALTITUDE_HOLD && getMotorsSpinning() == false)
joe4465 2:969dfa4f2436 198 {
joe4465 2:969dfa4f2436 199 _armed = armed;
joe4465 2:969dfa4f2436 200 DEBUG("Armed set to %d\r\n", _armed);
joe4465 2:969dfa4f2436 201 return true;
joe4465 2:969dfa4f2436 202 }
joe4465 2:969dfa4f2436 203 }
joe4465 2:969dfa4f2436 204 return false;
joe4465 0:c6a85bb2a827 205 }
joe4465 0:c6a85bb2a827 206
joe4465 0:c6a85bb2a827 207 bool Status::getArmed()
joe4465 0:c6a85bb2a827 208 {
joe4465 0:c6a85bb2a827 209 return _armed;
joe4465 0:c6a85bb2a827 210 }
joe4465 0:c6a85bb2a827 211
joe4465 0:c6a85bb2a827 212 bool Status::setInitialised(bool initialised)
joe4465 0:c6a85bb2a827 213 {
joe4465 2:969dfa4f2436 214 if(initialised != _initialised)
joe4465 2:969dfa4f2436 215 {
joe4465 2:969dfa4f2436 216 _initialised = initialised;
joe4465 2:969dfa4f2436 217 DEBUG("Initialised set to %d\r\n", _initialised);
joe4465 2:969dfa4f2436 218 return true;
joe4465 2:969dfa4f2436 219 }
joe4465 2:969dfa4f2436 220 else return false;
joe4465 0:c6a85bb2a827 221 }
joe4465 0:c6a85bb2a827 222
joe4465 0:c6a85bb2a827 223 bool Status::getInitialised()
joe4465 0:c6a85bb2a827 224 {
joe4465 0:c6a85bb2a827 225 return _initialised;
joe4465 0:c6a85bb2a827 226 }
joe4465 0:c6a85bb2a827 227
joe4465 0:c6a85bb2a827 228 bool Status::setRcConnected(bool rcConnected)
joe4465 0:c6a85bb2a827 229 {
joe4465 2:969dfa4f2436 230 if(rcConnected != _rcConnected)
joe4465 2:969dfa4f2436 231 {
joe4465 2:969dfa4f2436 232 _rcConnected = rcConnected;
joe4465 2:969dfa4f2436 233 if(_rcConnected == false)
joe4465 2:969dfa4f2436 234 {
joe4465 2:969dfa4f2436 235 setArmed(false);
joe4465 2:969dfa4f2436 236 setMotorsSpinning(false);
joe4465 2:969dfa4f2436 237 setDeadZone(false);
joe4465 2:969dfa4f2436 238 }
joe4465 2:969dfa4f2436 239 DEBUG("Rc connected set to %d\r\n", _rcConnected);
joe4465 2:969dfa4f2436 240 return true;
joe4465 2:969dfa4f2436 241 }
joe4465 2:969dfa4f2436 242 else return false;
joe4465 0:c6a85bb2a827 243 }
joe4465 0:c6a85bb2a827 244
joe4465 0:c6a85bb2a827 245 bool Status::getRcConnected()
joe4465 0:c6a85bb2a827 246 {
joe4465 0:c6a85bb2a827 247 return _rcConnected;
joe4465 2:969dfa4f2436 248 }
joe4465 2:969dfa4f2436 249
joe4465 2:969dfa4f2436 250 bool Status::setMotorsSpinning(bool flying)
joe4465 2:969dfa4f2436 251 {
joe4465 2:969dfa4f2436 252 if(flying != _flying)
joe4465 2:969dfa4f2436 253 {
joe4465 2:969dfa4f2436 254 _flying = flying;
joe4465 2:969dfa4f2436 255 DEBUG("Flying set to %d\r\n", _flying);
joe4465 2:969dfa4f2436 256 return true;
joe4465 2:969dfa4f2436 257 }
joe4465 2:969dfa4f2436 258 else return false;
joe4465 2:969dfa4f2436 259 }
joe4465 2:969dfa4f2436 260
joe4465 2:969dfa4f2436 261 bool Status::getMotorsSpinning()
joe4465 2:969dfa4f2436 262 {
joe4465 2:969dfa4f2436 263 return _flying;
joe4465 2:969dfa4f2436 264 }
joe4465 2:969dfa4f2436 265
joe4465 2:969dfa4f2436 266 bool Status::setDeadZone(bool deadZone)
joe4465 2:969dfa4f2436 267 {
joe4465 2:969dfa4f2436 268 if(deadZone != _deadZone)
joe4465 2:969dfa4f2436 269 {
joe4465 2:969dfa4f2436 270 _deadZone = deadZone;
joe4465 2:969dfa4f2436 271 DEBUG("Dead zone set to %d\r\n", _deadZone);
joe4465 2:969dfa4f2436 272 return true;
joe4465 2:969dfa4f2436 273 }
joe4465 2:969dfa4f2436 274 else return false;
joe4465 2:969dfa4f2436 275 }
joe4465 2:969dfa4f2436 276
joe4465 2:969dfa4f2436 277 bool Status::getDeadZone()
joe4465 2:969dfa4f2436 278 {
joe4465 2:969dfa4f2436 279 return _deadZone;
joe4465 0:c6a85bb2a827 280 }