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@2:969dfa4f2436, 2015-04-01 (annotated)
- Committer:
- joe4465
- Date:
- Wed Apr 01 11:19:21 2015 +0000
- Revision:
- 2:969dfa4f2436
- Parent:
- 0:c6a85bb2a827
- Child:
- 3:4823d6750629
Altitude hold with 2 PID's working. Exported to offline compiler
Who changed what in which revision?
User | Revision | Line number | New 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 | 2:969dfa4f2436 | 197 | else if (armed == true && _navigationMode == Status::ALTITUDE_HOLD && getMotorsSpinning() == false && getDeadZone() == true) |
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 | } |