├── .gitignore ├── LICENSE.md ├── README.md └── src ├── ATCCommand.h ├── AirTrafficControl.cpp ├── AirTrafficControl.h ├── Aircraft.cpp ├── Aircraft.h ├── AircraftFlightController.ino ├── AutoPilot.cpp ├── AutoPilot.h ├── BackProp.cpp ├── BackProp.h ├── Battery.cpp ├── Battery.h ├── Engine.cpp ├── Engine.h ├── FlightControlSurfaces.cpp ├── FlightControlSurfaces.h ├── FlightPlan.cpp ├── FlightPlan.h ├── Global.cpp ├── Global.h ├── Location.cpp ├── Location.h ├── MathUtility.cpp ├── MathUtility.h ├── Navigation.cpp ├── Navigation.h ├── Tracker.cpp ├── Tracker.h ├── Trainer.cpp ├── Trainer.h ├── Waypoint.cpp ├── Waypoint.h ├── WirelessTransceiver.cpp └── WirelessTransceiver.h /.gitignore: -------------------------------------------------------------------------------- 1 | *.obj 2 | *.o -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | ===================== 3 | 4 | Copyright © 2020, Akash Nag 5 | 6 | Permission is hereby granted, free of charge, to any person 7 | obtaining a copy of this software and associated documentation 8 | files (the “Software”), to deal in the Software without 9 | restriction, including without limitation the rights to use, 10 | copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | copies of the Software, and to permit persons to whom the 12 | Software is furnished to do so, subject to the following 13 | conditions: 14 | 15 | The above copyright notice and this permission notice shall be 16 | included in all copies or substantial portions of the Software. 17 | 18 | THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, 19 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES 20 | OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 21 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 22 | HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 23 | WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 24 | FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 25 | OTHER DEALINGS IN THE SOFTWARE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Arduino Flight Controller 2 | ========================== 3 | 4 | This is a prototype for a flight controller system written in Arduino C/C++, capable of carrying out autonomous flight missions with the additional capability of dropping packages. 5 | 6 | ### License 7 | 8 | The application is licensed under the MIT License. Copyright © Akash Nag. -------------------------------------------------------------------------------- /src/ATCCommand.h: -------------------------------------------------------------------------------- 1 | // --------------------------------------------------------------------------------------------- 2 | // Copyright (c) Akash Nag. All rights reserved. 3 | // Licensed under the MIT License. See LICENSE.md in the project root for license information. 4 | // --------------------------------------------------------------------------------------------- 5 | 6 | #ifndef ATCCommand_h 7 | #define ATCCommand_h 8 | 9 | /* 10 | * COMMANDS 11 | * -------- 12 | * HELLO used for maintaining communication / checking whether Aircraft is alive 13 | * MAINTAIN instruction to maintain throttle, attitude, etc.; works same as HELLO 14 | * LEVEL used to cancel all roll and pitch 15 | * CANCEL_ROLL used to cancel any left/right roll 16 | * CANCEL_PITCH used to cancel any up/down pitch 17 | * FLAPS_RETRACT used to retract flaps 18 | * FLAPS_HALF used to deploy half flaps 19 | * FLAPS_FULL used to deploy full flaps 20 | * SPOILERS_ON used to deploy spoilers 21 | * SPOILERS_OFF used to retract spoilers 22 | * ROLL_RIGHT used to inrease roll by 10 degrees 23 | * ROLL_LEFT used to decrease roll by 10 degrees 24 | * CLIMB used to increase pitch by 10 degrees 25 | * DESCEND used to decrease pitch by 10 degrees 26 | * THROTTLE_UP used to increase throttle by 30% 27 | * THROTTLE_DOWN used to decrease throttle by 30% 28 | * YAW_LEFT used to decrease yaw by 10 degrees 29 | * YAW_RIGHT used to increase yaw by 10 degrees 30 | * DROP_PACKAGE used to drop package 31 | * RETURN_TO_BASE used to direct the aircraft to land at the designated airport 32 | * LAND_NOW used to direct the aircraft to start landing procedure immediately: this levels the aircraft and starts landing 33 | * 34 | * Note: Once landing procedure begins, the aircraft is deaf to any more commands 35 | */ 36 | 37 | #define ATCC_NOCOMM -1 38 | #define ATCC_HELLO 0 39 | #define ATCC_MAINTAIN 1 40 | #define ATCC_LEVEL 2 41 | #define ATCC_CANCEL_ROLL 3 42 | #define ATCC_CANCEL_PITCH 4 43 | #define ATCC_FLAPS_RETRACT 5 44 | #define ATCC_FLAPS_HALF 6 45 | #define ATCC_FLAPS_FULL 7 46 | #define ATCC_SPOILERS_ON 8 47 | #define ATCC_SPOILERS_OFF 9 48 | #define ATCC_ROLL_LEFT 10 49 | #define ATCC_ROLL_RIGHT 11 50 | #define ATCC_CLIMB 12 51 | #define ATCC_DESCEND 13 52 | #define ATCC_THROTTLE_UP 14 53 | #define ATCC_THROTTLE_DOWN 15 54 | #define ATCC_YAW_LEFT 16 55 | #define ATCC_YAW_RIGHT 17 56 | #define ATCC_DROP_PACKAGE 18 57 | #define ATCC_RETURN_TO_BASE 19 58 | #define ATCC_LAND_NOW 20 59 | #define ATCC_TEST_CONTROLS 21 60 | #define ATCC_START_MISSION 22 61 | #define ATCC_REPORT_LOCATION_AND_FUEL 23 62 | #define ATCC_MARK_CURRENT_LOCATION_AS_AIRPORT 24 63 | #define ATCC_ADD_CURRENT_LOCATION_AS_WAYPOINT 25 64 | #define ATCC_INSERT_CURRENT_LOCATION_AS_WAYPOINT 26 65 | #define ATCC_ADD_CURRENT_LOCATION_AS_DELIVERY_POINT 27 66 | #define ATCC_REMOVE_WAYPOINT 28 67 | #define ATCC_CLEAR_FLIGHTPLAN 29 68 | #define ATCC_REPORT_FLIGHTPLAN 30 69 | #define ATCC_REQUEST_COMMAND 31 70 | #define ATCC_OPEN_DOOR 32 71 | #define ATCC_CLOSE_DOOR 33 72 | #define ATCC_START_DEMO_MISSION 34 73 | #endif 74 | -------------------------------------------------------------------------------- /src/AirTrafficControl.cpp: -------------------------------------------------------------------------------- 1 | // --------------------------------------------------------------------------------------------- 2 | // Copyright (c) Akash Nag. All rights reserved. 3 | // Licensed under the MIT License. See LICENSE.md in the project root for license information. 4 | // --------------------------------------------------------------------------------------------- 5 | 6 | #include "AirTrafficControl.h" 7 | 8 | AirTrafficControl::AirTrafficControl() 9 | { 10 | 11 | } 12 | 13 | void AirTrafficControl::establishCommunication() 14 | { 15 | comms.begin(); 16 | } 17 | 18 | void AirTrafficControl::inform(const String message) const 19 | { 20 | comms.sendData(message); 21 | } 22 | 23 | void AirTrafficControl::clearedToSend() const 24 | { 25 | comms.sendData("CTS"); 26 | } 27 | 28 | void AirTrafficControl::informCTS(const String message) const 29 | { 30 | comms.sendData(message); 31 | comms.sendData("EOM"); 32 | } 33 | 34 | int AirTrafficControl::receiveCommand(int ¶m) const 35 | { 36 | String comm = comms.receive(); 37 | comm.trim(); 38 | if(comm.equalsIgnoreCase("ERROR")) return -1; 39 | 40 | int pos = comm.indexOf(' '); 41 | 42 | if(pos == -1) { 43 | param = 0; 44 | return comm.toInt(); 45 | } else { 46 | param = comm.substring(pos+1).toInt(); 47 | return comm.substring(0,pos).toInt(); 48 | } 49 | } 50 | 51 | bool AirTrafficControl::withinRange() const 52 | { 53 | int p, tries=0; 54 | do { 55 | informCTS("HELLO"); 56 | tries++; 57 | } while(tries < MAX_ATC_COMM_RETRIES && receiveCommand(p) == ATCC_NOCOMM); 58 | 59 | return(tries 10 | #include "WirelessTransceiver.h" 11 | #include "ATCCommand.h" 12 | 13 | class AirTrafficControl 14 | { 15 | private: 16 | WirelessTransceiver comms; 17 | 18 | public: 19 | AirTrafficControl(); 20 | void establishCommunication(); 21 | void inform(const String) const; 22 | int receiveCommand(int&) const; 23 | void clearedToSend() const; 24 | void informCTS(const String) const; 25 | bool withinRange() const; 26 | }; 27 | #endif 28 | -------------------------------------------------------------------------------- /src/Aircraft.cpp: -------------------------------------------------------------------------------- 1 | // --------------------------------------------------------------------------------------------- 2 | // Copyright (c) Akash Nag. All rights reserved. 3 | // Licensed under the MIT License. See LICENSE.md in the project root for license information. 4 | // --------------------------------------------------------------------------------------------- 5 | 6 | #include "Aircraft.h" 7 | 8 | Aircraft::Aircraft() 9 | { 10 | landingGearDeployed = false; 11 | controlsInitialized = false; 12 | enginesPowered = false; 13 | 14 | for(int i=0; i=0; i--) 75 | { 76 | landingGear.write(i); 77 | delay(15); 78 | } 79 | landingGearDeployed = false; 80 | } 81 | } 82 | 83 | void Aircraft::deployLandingGear() 84 | { 85 | if(!landingGearDeployed) 86 | { 87 | for(int i=0; i<=90; i++) 88 | { 89 | landingGear.write(i); 90 | delay(15); 91 | } 92 | landingGearDeployed = true; 93 | } 94 | } 95 | 96 | void Aircraft::powerUpEngines() 97 | { 98 | for(int i=0; i=MAX_CHAMBERS) return; 121 | chambers[chamber].write(90); 122 | delay(1500); 123 | chambers[chamber].write(0); 124 | } 125 | 126 | void Aircraft::openDoor(const int chamber) 127 | { 128 | if(chamber<0 || chamber>=MAX_CHAMBERS || chamberDoorsOpen[chamber]) return; 129 | for(int i=0; i<=90; i++) 130 | { 131 | chambers[chamber].write(i); 132 | delay(20); 133 | } 134 | chamberDoorsOpen[chamber] = true; 135 | } 136 | 137 | void Aircraft::closeDoor(const int chamber) 138 | { 139 | if(chamber<0 || chamber>=MAX_CHAMBERS || !chamberDoorsOpen[chamber]) return; 140 | for(int i=90; i>=0; i--) 141 | { 142 | chambers[chamber].write(i); 143 | delay(20); 144 | } 145 | chamberDoorsOpen[chamber] = false; 146 | } 147 | 148 | bool Aircraft::isLowOnFuel() const 149 | { 150 | return powerUnit.isLow(); 151 | } 152 | 153 | int Aircraft::getFuel() const 154 | { 155 | return powerUnit.getPercentage(); 156 | } 157 | 158 | float Aircraft::getPitch() const 159 | { 160 | return tracker.getPitch(); 161 | } 162 | 163 | float Aircraft::getRoll() const 164 | { 165 | return tracker.getRoll(); 166 | } 167 | 168 | float Aircraft::getYaw() const 169 | { 170 | return tracker.getYaw(); 171 | } 172 | 173 | void Aircraft::setFlaps(const int f) const 174 | { 175 | flightControls.setFlaps(f); 176 | } 177 | 178 | void Aircraft::setRudder(const int r) const 179 | { 180 | flightControls.setRudder(r); 181 | } 182 | 183 | void Aircraft::setElevators(const int e) const 184 | { 185 | flightControls.setElevators(e); 186 | } 187 | 188 | void Aircraft::setLeftAileron(const int a) const 189 | { 190 | flightControls.setLeftAileron(a); 191 | } 192 | 193 | void Aircraft::setRightAileron(const int a) const 194 | { 195 | flightControls.setRightAileron(a); 196 | } 197 | 198 | float Aircraft::getVelocity() const 199 | { 200 | return tracker.getGPSVelocity(); 201 | } 202 | 203 | void Aircraft::deploySpoilers() const 204 | { 205 | flightControls.deploySpoilers(); 206 | } 207 | 208 | void Aircraft::retractSpoilers() const 209 | { 210 | flightControls.retractSpoilers(); 211 | } 212 | 213 | bool Aircraft::isLocationValid() const 214 | { 215 | bool f1 = tracker.measureGPSLocation(); 216 | bool f2 = (tracker.fixAge <= GPS_STALE_AGE ? true : false); 217 | 218 | return(f1 && (landingGearDeployed || f2)); 219 | } 220 | 221 | bool Aircraft::areControlsInitialized() const 222 | { 223 | return controlsInitialized; 224 | } 225 | 226 | bool Aircraft::areEnginesPowered() const 227 | { 228 | return enginesPowered; 229 | } 230 | 231 | void Aircraft::refresh() 232 | { 233 | long currentRefresh = millis(); 234 | if(HAS_LIGHTS) 235 | { 236 | if(lightsOn) 237 | { 238 | if(currentRefresh - lastRefresh >= LIGHTS_ON_DURATION) 239 | { 240 | digitalWrite(AIRCRAFT_LIGHTS_PIN, LOW); 241 | lightsOn = false; 242 | } 243 | } else { 244 | if(currentRefresh - lastRefresh >= LIGHTS_OFF_DURATION) 245 | { 246 | digitalWrite(AIRCRAFT_LIGHTS_PIN, HIGH); 247 | lightsOn = true; 248 | } 249 | } 250 | } 251 | lastRefresh = currentRefresh; 252 | } 253 | -------------------------------------------------------------------------------- /src/Aircraft.h: -------------------------------------------------------------------------------- 1 | // --------------------------------------------------------------------------------------------- 2 | // Copyright (c) Akash Nag. All rights reserved. 3 | // Licensed under the MIT License. See LICENSE.md in the project root for license information. 4 | // --------------------------------------------------------------------------------------------- 5 | 6 | #ifndef Aircraft_h 7 | #define Aircraft_h 8 | 9 | #include "Location.h" 10 | #include "Engine.h" 11 | #include "FlightControlSurfaces.h" 12 | #include "Battery.h" 13 | #include "Tracker.h" 14 | #include "ATCCommand.h" 15 | #include "Global.h" 16 | 17 | class AutoPilot; 18 | 19 | class Aircraft 20 | { 21 | private: 22 | Engine engines[MAX_ENGINES]; 23 | FlightControlSurfaces flightControls; 24 | Battery powerUnit; 25 | Tracker tracker; 26 | Servo landingGear; 27 | Servo chambers[MAX_CHAMBERS]; 28 | 29 | float aircraftGroundAltitude; 30 | 31 | bool landingGearDeployed; 32 | bool controlsInitialized; 33 | bool enginesPowered; 34 | bool chamberDoorsOpen[MAX_CHAMBERS]; 35 | 36 | bool lightsOn; 37 | long lastRefresh; 38 | 39 | public: 40 | Aircraft(); 41 | Location initControls(); 42 | void powerUpEngines(); 43 | void powerDownEngines(); 44 | 45 | void retractLandingGear(); 46 | void deployLandingGear(); 47 | 48 | bool isLandingGearDeployed() const; 49 | bool areControlsInitialized() const; 50 | bool areEnginesPowered() const; 51 | 52 | void setEngineThrottle(int) const; 53 | void setEngineThrottleFull() const; 54 | 55 | void setFlaps(const int) const; 56 | void setRudder(const int) const; 57 | void setElevators(const int) const; 58 | void setLeftAileron(const int) const; 59 | void setRightAileron(const int) const; 60 | void deploySpoilers() const; 61 | void retractSpoilers() const; 62 | 63 | bool isLocationValid() const; 64 | Location getLocation() const; 65 | float getBearing() const; 66 | float getAltitude() const; 67 | float getPitch() const; 68 | float getRoll() const; 69 | float getYaw() const; 70 | float getVelocity() const; 71 | 72 | void dropPackage(const int) const; 73 | void openDoor(const int); 74 | void closeDoor(const int); 75 | 76 | int getFuel() const; 77 | bool isLowOnFuel() const; 78 | 79 | void refresh(); 80 | }; 81 | #endif 82 | -------------------------------------------------------------------------------- /src/AircraftFlightController.ino: -------------------------------------------------------------------------------- 1 | // --------------------------------------------------------------------------------------------- 2 | // Copyright (c) Akash Nag. All rights reserved. 3 | // Licensed under the MIT License. See LICENSE.md in the project root for license information. 4 | // --------------------------------------------------------------------------------------------- 5 | 6 | /* 7 | * Arduino On-Board Autonomous Flight Controller for Multi-Propeller Fixed-Wing Cargo Aircraft 8 | * -------------------------------------------------------------------------------------------------- 9 | * Normal operation requires: 10 | * 1. Arduino Mega 11 | * 2. BLDC motors 12 | * 3. ESCs 13 | * 4. LiPo battery pack 14 | * 5. Servo motors (for flaps, rudder, ailerons, spoilers, landing-gear, package doors) 15 | * 6. 3-Axis Gyroscope + Accelerometer 16 | * 7. GPS Module 17 | * 8. NRF24L01+ / HC-12 Module 18 | * 9. Barometer 19 | * 10. Magnetometer 20 | * 11. Landing-Gears x 3-sets 21 | * 12. Propellers as required 22 | * 13. Connectors for connecting battery with ESCs 23 | * 14. AA-battery pack/holder for powering Arduino 24 | */ 25 | 26 | #include "FlightPlan.h" 27 | #include "Aircraft.h" 28 | #include "AirTrafficControl.h" 29 | #include "ATCCommand.h" 30 | #include "Global.h" 31 | #include "AutoPilot.h" 32 | 33 | bool airportDesignated = false; 34 | Airport *airport = NULL; 35 | FlightPlan flightPlan; 36 | 37 | Aircraft aircraft; 38 | AirTrafficControl atc; 39 | AutoPilot autoPilot(&aircraft); 40 | 41 | void setup() 42 | { 43 | atc.establishCommunication(); 44 | aircraft.initControls(); 45 | autoPilot.testFlightControls(); 46 | } 47 | 48 | bool startMission() 49 | { 50 | if(!airportDesignated) return false; 51 | 52 | atc.inform("Initiating take-off procedure..."); 53 | autoPilot.takeOff(); 54 | 55 | //atc.inform("Commencing training procedure..."); 56 | //autoPilot.train(); 57 | //atc.inform("Training complete"); 58 | 59 | atc.inform("Attempting to reach cruise altitude..."); 60 | autoPilot.setAltitude(CRUISE_ALTITUDE); 61 | 62 | int maxWaypoints = flightPlan.getWaypointCount(); 63 | 64 | bool altFlag = false, courseFlag = false; 65 | 66 | Waypoint *currentWaypoint; 67 | if(maxWaypoints > 0) 68 | { 69 | bool emergency = false; 70 | for(int i=0; igetLocation()); 76 | while(autoPilot.getETA() > 2) 77 | { 78 | if(RTB_ON_LOW_FUEL && aircraft.isLowOnFuel()) 79 | { 80 | atc.inform("Low fuel warning! Executing emergency RTB procedure..."); 81 | emergency = true; 82 | break; 83 | } else if(RTB_ON_LOSS_OF_TELEMETRY && !atc.withinRange()) { 84 | atc.inform("Outside ATC coverage area! Executing emergency RTB procedure..."); 85 | emergency = true; 86 | break; 87 | } 88 | 89 | do { 90 | courseFlag = autoPilot.keepOnCourse(70); 91 | altFlag = autoPilot.maintainAltitude(70); 92 | } while(!courseFlag || !altFlag); 93 | 94 | delay(500); 95 | } 96 | 97 | if(emergency) break; 98 | if(currentWaypoint->isDeliveryPoint()) 99 | { 100 | atc.inform("Nearing drop location, preparing to drop package..."); 101 | aircraft.dropPackage(currentWaypoint->getPackageID()); 102 | } else { 103 | atc.inform("Waypoint reached."); 104 | } 105 | } 106 | } 107 | 108 | atc.inform("Initiating landing procedure..."); 109 | autoPilot.land(*airport); 110 | 111 | return true; 112 | } 113 | 114 | void startDemoMission() 115 | { 116 | if(!aircraft.isLandingGearDeployed()) return; 117 | 118 | // TURN --> CRUISE --> DROP --> repeat 119 | const int turns[] = { 0, -45, -90, -45, 0, -90, -45 }; 120 | const int delays[] = { 5, 5, 5, 5, 5, 5, 0 }; // delays (in seconds) AFTER the corresponding turn has been completed 121 | const int dropPoints[] = { -1, -1, -1, 0, -1, -1, -1 }; // drop packages (-1:no, >=0:yes) AFTER the cruise has been completed 122 | const int maxWaypoints = 7; 123 | 124 | // TAKEOFF 125 | const float alt = aircraft.getAltitude(); 126 | atc.inform("Initiating take-off procedure..."); 127 | autoPilot.takeOff(); 128 | 129 | // CLIMB 130 | atc.inform("Attempting to reach cruise altitude..."); 131 | autoPilot.setAltitude(CRUISE_ALTITUDE); 132 | 133 | String x; 134 | for(int i=0; i0) 148 | { 149 | x = String("Cruising for "); 150 | x = x + timer + " more seconds..."; 151 | atc.inform(x); 152 | timer--; 153 | aircraft.refresh(); 154 | delay(1000); 155 | } 156 | 157 | // DROP 158 | if(dropPoints[i] > -1) 159 | { 160 | x = String("Dropping package #"); 161 | x = x + dropPoints[i] + "..."; 162 | atc.inform(x); 163 | aircraft.dropPackage(dropPoints[i]); 164 | } 165 | } 166 | 167 | // LAND 168 | atc.inform("Preparing to land..."); 169 | autoPilot.landImmediately(alt); 170 | } 171 | 172 | void loop() 173 | { 174 | aircraft.refresh(); 175 | 176 | String message; 177 | int fuel, n; 178 | float lat, lon; 179 | int param = 0; 180 | int command = atc.receiveCommand(param); 181 | 182 | switch(command) 183 | { 184 | case ATCC_NOCOMM: 185 | break; 186 | 187 | case ATCC_HELLO: 188 | atc.inform("Command received: ATCC_HELLO | Hello"); 189 | break; 190 | 191 | case ATCC_TEST_CONTROLS: 192 | atc.inform("Command received: ATCC_TEST_CONTROLS | Initiating test of flight controls..."); 193 | autoPilot.testFlightControls(); 194 | atc.inform("Flight-controls test complete"); 195 | break; 196 | 197 | case ATCC_START_MISSION: 198 | atc.inform("Command received: ATCC_START_MISSION | Starting mission..."); 199 | bool flag = startMission(); 200 | atc.inform((flag ? "Mission complete" : "Mission failure: no airport designated")); 201 | break; 202 | 203 | case ATCC_REPORT_LOCATION_AND_FUEL: 204 | fuel = aircraft.getFuel(); 205 | Location loc = aircraft.getLocation(); 206 | message = String("Command received: ATCC_REPORT_LOCATION_AND_FUEL | Fuel remaining: "); 207 | message = message + fuel + "%, Coordinates=" + loc.toString(); 208 | atc.inform(message); 209 | break; 210 | 211 | case ATCC_MARK_CURRENT_LOCATION_AS_AIRPORT: 212 | if(airportDesignated) delete airport; 213 | airport = new Airport(&aircraft.getLocation(), aircraft.getBearing(), aircraft.getAltitude()); 214 | airportDesignated = true; 215 | message = String("Command received: ATCC_MARK_CURRENT_LOCATION_AS_AIRPORT | Airport marked at: "); 216 | message = message + airport->location->toString(); 217 | atc.inform(message); 218 | break; 219 | 220 | case ATCC_ADD_CURRENT_LOCATION_AS_WAYPOINT: 221 | Waypoint wp(Location(aircraft.getLocation()), false, -1); 222 | flightPlan.appendWaypoint(wp); 223 | atc.inform("Command received: ATCC_ADD_CURRENT_LOCATION_AS_WAYPOINT | Waypoint marked"); 224 | break; 225 | 226 | case ATCC_INSERT_CURRENT_LOCATION_AS_WAYPOINT: 227 | Waypoint wp1(Location(aircraft.getLocation()), false, -1); 228 | flightPlan.insertWaypoint(param,wp1); 229 | atc.inform("Command received: ATCC_INSERT_CURRENT_LOCATION_AS_WAYPOINT | Waypoint inserted"); 230 | break; 231 | 232 | case ATCC_ADD_CURRENT_LOCATION_AS_DELIVERY_POINT: 233 | Waypoint wp2(Location(aircraft.getLocation()), true, param); 234 | flightPlan.appendWaypoint(wp2); 235 | atc.inform("Command received: ATCC_ADD_CURRENT_LOCATION_AS_DELIVERY_POINT | Delivery point marked"); 236 | break; 237 | 238 | case ATCC_REMOVE_WAYPOINT: 239 | flightPlan.removeWaypoint(param); 240 | atc.inform("Command received: ATCC_REMOVE_WAYPOINT | Waypoint removed"); 241 | break; 242 | 243 | case ATCC_CLEAR_FLIGHTPLAN: 244 | flightPlan.clearAll(); 245 | atc.inform("Command received: ATCC_CLEAR_FLIGHTPLAN | Flightplan cancelled"); 246 | break; 247 | 248 | case ATCC_REPORT_FLIGHTPLAN: 249 | atc.inform("Command received: ATCC_REPORT_FLIGHTPLAN | Retrieving flight-plan..."); 250 | atc.inform(airportDesignated ? "Airport coordinates: " + airport->location->toString() : "No airport designated"); 251 | n = flightPlan.getWaypointCount(); 252 | if(n==0) 253 | atc.inform("Flight-plan empty."); 254 | else { 255 | atc.inform("Waypoint count: " + n); 256 | atc.inform("#\tDel\tPID\tCoordinates\n--\t---\t---\t----------------------------"); 257 | for(int i=0; itoString()); 258 | atc.inform("--------------------------------------------------------------------"); 259 | } 260 | break; 261 | 262 | case ATCC_OPEN_DOOR: 263 | atc.inform("Command received: ATCC_OPEN_DOOR | Opening door..."); 264 | aircraft.openDoor(param); 265 | break; 266 | 267 | case ATCC_CLOSE_DOOR: 268 | atc.inform("Command received: ATCC_CLOSE_DOOR | Closing door..."); 269 | aircraft.closeDoor(param); 270 | break; 271 | 272 | case ATCC_START_DEMO_MISSION: 273 | atc.inform("Command received: ATCC_START_DEMO_MISSION | Starting demo mission..."); 274 | startDemoMission(); 275 | atc.inform("Demo Mission complete"); 276 | break; 277 | 278 | default: 279 | message = String("Command not recognized | Received: "); 280 | message = message + command + " with parameter " + param; 281 | atc.inform(message); 282 | break; 283 | } 284 | atc.clearedToSend(); 285 | } 286 | -------------------------------------------------------------------------------- /src/AutoPilot.cpp: -------------------------------------------------------------------------------- 1 | // --------------------------------------------------------------------------------------------- 2 | // Copyright (c) Akash Nag. All rights reserved. 3 | // Licensed under the MIT License. See LICENSE.md in the project root for license information. 4 | // --------------------------------------------------------------------------------------------- 5 | 6 | #include "AutoPilot.h" 7 | 8 | AutoPilot::AutoPilot(const Aircraft *a) 9 | { 10 | aircraft = a; 11 | 12 | isTargetLocationValid=false; 13 | isTargetAltitudeValid=false; 14 | targetLocation = NULL; 15 | targetAltitude = 0; 16 | } 17 | 18 | void AutoPilot::takeOff() const 19 | { 20 | if(!aircraft->isLandingGearDeployed()) return; 21 | if(!aircraft->areControlsInitialized()) return; 22 | if(!aircraft->areEnginesPowered()) aircraft->powerUpEngines(); 23 | 24 | // initiate take-off sequence 25 | setCruiseMode(); 26 | aircraft->setFlaps(FSR_FLAPS_FULL); 27 | aircraft->setEngineThrottle(30); // don't go full throttle on startup as it may damage engine 28 | delay(1000); 29 | aircraft->setEngineThrottleFull(); 30 | 31 | // wait for take-off velocity and then attempt to get airborne 32 | while(aircraft->getVelocity() < TAKEOFF_VELOCITY) 33 | { 34 | aircraft->refresh(); 35 | delay(1000); 36 | } 37 | aircraft->setElevators(135); 38 | 39 | // wait till actually airborne 40 | while(aircraft->getAltitude() == 0) 41 | { 42 | aircraft->refresh(); 43 | delay(500); 44 | } 45 | 46 | // retract flaps and landing-gear 47 | aircraft->retractLandingGear(); 48 | aircraft->setFlaps(FSR_FLAPS_RETRACT); 49 | 50 | // Handover charge to reach desired altitude; once reached, decrease throttle to cruise 51 | setAltitude(CRUISE_ALTITUDE); 52 | 53 | setCruiseMode(); 54 | aircraft->setEngineThrottle(60); 55 | } 56 | 57 | void AutoPilot::setCourse(const Location &destination) 58 | { 59 | if(isTargetLocationValid) delete targetLocation; 60 | targetLocation = new Location(destination); 61 | isTargetLocationValid = true; 62 | aircraft->setEngineThrottle(70); 63 | keepOnCourse(0); 64 | aircraft->setEngineThrottle(60); 65 | } 66 | 67 | void AutoPilot::setAltitude(const float alt) 68 | { 69 | bool climbing = false; 70 | if(isTargetAltitudeValid && alt > targetAltitude) climbing = true; 71 | if(climbing) aircraft->setEngineThrottle(90); // increase throttle for climbing 72 | targetAltitude = alt; 73 | isTargetAltitudeValid = true; 74 | maintainAltitude(0); 75 | if(climbing) aircraft->setEngineThrottle(60); 76 | } 77 | 78 | float AutoPilot::getETA() const 79 | { 80 | if(!isTargetLocationValid) return -1; 81 | 82 | Location current = aircraft->getLocation(); 83 | float dist = targetLocation->distanceTo(current); 84 | return (dist / aircraft->getVelocity()); 85 | } 86 | 87 | bool AutoPilot::keepOnCourse(const int preemptMillis = 0) const 88 | { 89 | if(aircraft->isLandingGearDeployed()) return; 90 | if(!isTargetLocationValid) return; 91 | 92 | int failCount = 0; 93 | while(!aircraft->isLocationValid()) 94 | { 95 | failCount++; 96 | delay(100); 97 | if(failCount > MAX_GPS_ATTEMPTS) return false; 98 | } 99 | 100 | Location current = aircraft->getLocation(); 101 | float targetBearing = getDesiredBearing(current, *targetLocation); 102 | return setBearing(targetBearing, preemptMillis); 103 | } 104 | 105 | bool AutoPilot::setBearing(const float targetBearing, const int preemptMillis=0) const // target bearing must be unipolar (0-360) 106 | { 107 | float initialBearing = aircraft->getBearing(); 108 | float deltaBearing = unipolarToBipolarBearing(targetBearing - initialBearing); 109 | 110 | float deltaBearingAbs = fabs(deltaBearing); 111 | float currentBearing = initialBearing; 112 | 113 | if(deltaBearingAbs==0) 114 | { 115 | stabilizeRoll(); 116 | return true; 117 | } 118 | 119 | int timer = 0; 120 | if(deltaBearingAbs < RUDDER_KICK_IN_ANGLE) 121 | { 122 | // Use rudder 123 | do { 124 | deltaBearing = unipolarToBipolarBearing(targetBearing - aircraft->getBearing()); 125 | deltaBearingAbs = fabs(deltaBearing); 126 | 127 | float rudderDelta = map(deltaBearingAbs,0,5,0,20); 128 | aircraft->setRudder(deltaBearing < 0 ? 90-rudderDelta : 90 + rudderDelta); 129 | aircraft->refresh(); 130 | 131 | delay(AUTOPILOT_ADJUSTMENT_DELAY); 132 | } while(deltaBearingAbs >= 0.25); 133 | 134 | aircraft->setRudder(90); 135 | return true; 136 | } else { 137 | // Use ailerons 138 | do { 139 | deltaBearing = (targetBearing - aircraft->getBearing()); 140 | if(fabs(deltaBearing) > 180) 141 | { 142 | if(deltaBearing > 0) 143 | deltaBearing -= 360; 144 | else 145 | deltaBearing += 360; 146 | } 147 | 148 | if(!setAilerons(deltaBearing)) break; 149 | aircraft->refresh(); 150 | delay(AUTOPILOT_ADJUSTMENT_DELAY); 151 | 152 | if(preemptMillis > 0) 153 | { 154 | timer+=AUTOPILOT_ADJUSTMENT_DELAY; 155 | if(timer > preemptMillis) return false; 156 | } 157 | } while(1); 158 | } 159 | 160 | stabilizeRoll(); 161 | return true; 162 | } 163 | 164 | bool AutoPilot::setAilerons(const int deg) const // deg must be bipolar 165 | { 166 | int absDeg = abs(deg); 167 | if(absDeg <= BEARING_TOLERANCE) return false; 168 | 169 | // aileron positions: 0 deg = full-up, 90 deg = flat, 180 deg = full down 170 | if(deg < 0) 171 | { 172 | // turn LEFT: set left-aileron UP, set right-aileron DOWN 173 | if(absDeg > EASE_OUT_ANGLE) 174 | { 175 | aircraft->setLeftAileron(0); 176 | aircraft->setRightAileron(180); 177 | } else { 178 | aircraft->setLeftAileron((int)easeOut(EASE_OUT_ANGLE-absDeg, EASE_OUT_ANGLE, 0, 90)); 179 | aircraft->setLeftAileron((int)easeOut(EASE_OUT_ANGLE-absDeg, EASE_OUT_ANGLE, 180, 90)); 180 | } 181 | } else { 182 | // turn RIGHT: set right-aileron UP, set left-aileron DOWN 183 | if(absDeg > EASE_OUT_ANGLE) 184 | { 185 | aircraft->setLeftAileron(180); 186 | aircraft->setRightAileron(0); 187 | } else { 188 | aircraft->setLeftAileron((int)easeOut(EASE_OUT_ANGLE-absDeg, EASE_OUT_ANGLE, 180, 90)); 189 | aircraft->setLeftAileron((int)easeOut(EASE_OUT_ANGLE-absDeg, EASE_OUT_ANGLE, 0, 90)); 190 | } 191 | } 192 | return true; 193 | } 194 | 195 | bool AutoPilot::setElevators(const int delta) const 196 | { 197 | int deltaAbs = abs(delta); 198 | if(deltaAbs <= ALTITUDE_TOLERANCE) return false; 199 | 200 | int degree = (deltaAbs > EASE_OUT_ALTITUDE ? 90 : (deltaAbs * 90)/EASE_OUT_ALTITUDE); 201 | 202 | if(delta < 0) 203 | { 204 | // descend: set elevators UP 205 | aircraft->setElevators(90-degree); 206 | } else { 207 | // climb: set elevators DOWN 208 | aircraft->setElevators(90+degree); 209 | } 210 | 211 | return true; 212 | } 213 | 214 | bool AutoPilot::maintainAltitude(const int preemptMillis = 0) const 215 | { 216 | if(aircraft->isLandingGearDeployed()) return; 217 | if(!isTargetAltitudeValid) return; 218 | 219 | float currentAltitude, delta, deltaAbs; 220 | int degree = 0; 221 | 222 | int timer=0; 223 | do { 224 | currentAltitude = aircraft->getAltitude(); 225 | delta = targetAltitude - currentAltitude; 226 | 227 | if(!setElevators(delta)) break; 228 | aircraft->refresh(); 229 | delay(AUTOPILOT_ADJUSTMENT_DELAY); 230 | 231 | if(preemptMillis > 0) 232 | { 233 | timer+=AUTOPILOT_ADJUSTMENT_DELAY; 234 | if(timer > preemptMillis) return false; // return without flattening elevators 235 | } 236 | } while(1); 237 | 238 | aircraft->setElevators(90); 239 | return true; 240 | } 241 | 242 | void AutoPilot::landImmediately(const float airportAltitude) const 243 | { 244 | if(aircraft->isLandingGearDeployed()) return; // maybe already on ground: rectify: check velocity too 245 | 246 | setCruiseMode(); 247 | aircraft->setEngineThrottle(85); 248 | setAltitude(3 + airportAltitude); // 9 feet 249 | aircraft->setEngineThrottle(70); 250 | setAltitude(2 + airportAltitude); // 6 feet 251 | aircraft->setFlaps(FSR_FLAPS_HALF); 252 | aircraft->setEngineThrottle(50); 253 | setAltitude(1 + airportAltitude); // 3 feet 254 | aircraft->setEngineThrottle(30); 255 | aircraft->deployLandingGear(); 256 | aircraft->setFlaps(FSR_FLAPS_FULL); 257 | aircraft->setElevators(45); 258 | delay(50); 259 | aircraft->setEngineThrottle(20); 260 | 261 | while(aircraft->getAltitude() > 0.15 + ALTITUDE_TOLERANCE + airportAltitude) delay(15); // wait till 0.5 feet above ground 262 | aircraft->deploySpoilers(); 263 | aircraft->setLeftAileron(0); 264 | aircraft->setRightAileron(0); 265 | aircraft->setElevators(140); 266 | 267 | aircraft->setEngineThrottle(10); 268 | while(aircraft->getAltitude() > ALTITUDE_TOLERANCE + airportAltitude) delay(15); 269 | aircraft->powerDownEngines(); 270 | setCruiseMode(); 271 | } 272 | 273 | void AutoPilot::land(const Airport &airport) const 274 | { 275 | Location finalApproachLocation = getLocationBeforeLocation(*airport.location, airport.bearing, FINAL_APPROACH_DISTANCE); 276 | 277 | float sideStepBearing = airport.bearing - 90; 278 | if(sideStepBearing < 0) sideStepBearing += 360; 279 | 280 | Location sideStepLocation = getLocationBeforeLocation(finalApproachLocation, sideStepBearing, TURNING_RADIUS); 281 | 282 | setCourse(sideStepLocation); 283 | do 284 | { 285 | keepOnCourse(100); 286 | } while(getETA() > 2); 287 | setBearing(sideStepBearing); 288 | 289 | setCourse(finalApproachLocation); 290 | do 291 | { 292 | keepOnCourse(100); 293 | } while(getETA() > 2); 294 | setBearing(airport.bearing); 295 | 296 | landImmediately(airport.altitude); 297 | } 298 | 299 | void AutoPilot::stabilizeRoll() const 300 | { 301 | // TO DO: improve this code to use Easing function to adjust ailerons 302 | int roll; 303 | aircraft->setRudder(90); 304 | while((roll=(int)aircraft->getRoll()) != 0) 305 | { 306 | // convert to bipolar 307 | int rbp = (roll <= 180 ? roll : roll-360); 308 | setAilerons(-1 * rbp); // reverse polarity to adjust 309 | aircraft->refresh(); 310 | delay(AUTOPILOT_ADJUSTMENT_DELAY); 311 | } 312 | aircraft->setLeftAileron(90); 313 | aircraft->setRightAileron(90); 314 | } 315 | 316 | void AutoPilot::stabilizePitch() const 317 | { 318 | // TO DO: improve this code to use Easing function to adjust elevators 319 | int pitch; 320 | while((pitch=(int)aircraft->getPitch()) != 0) 321 | { 322 | // convert to bipolar 323 | int pbp = (pitch <= 180 ? pitch : pitch-360); 324 | setElevators(-1 * pbp); // reverse polarity to adjust 325 | aircraft->refresh(); 326 | delay(AUTOPILOT_ADJUSTMENT_DELAY); 327 | } 328 | } 329 | 330 | void AutoPilot::setCruiseMode() const 331 | { 332 | aircraft->retractSpoilers(); 333 | aircraft->setFlaps(FSR_FLAPS_RETRACT); 334 | aircraft->setRudder(90); 335 | 336 | if(!aircraft->isLandingGearDeployed()) 337 | { 338 | stabilizeRoll(); 339 | stabilizePitch(); 340 | } 341 | 342 | aircraft->setElevators(90); 343 | aircraft->setLeftAileron(90); 344 | aircraft->setRightAileron(90); 345 | } 346 | 347 | void AutoPilot::testFlightControls() const 348 | { 349 | if(!aircraft->isLandingGearDeployed()) return; // cannot perform test mid-air! 350 | 351 | setCruiseMode(); 352 | delay(AUTOPILOT_ADJUSTMENT_DELAY); 353 | 354 | aircraft->setFlaps(FSR_FLAPS_HALF); 355 | delay(AUTOPILOT_ADJUSTMENT_DELAY); 356 | aircraft->setFlaps(FSR_FLAPS_FULL); 357 | delay(AUTOPILOT_ADJUSTMENT_DELAY); 358 | aircraft->setFlaps(FSR_FLAPS_RETRACT); 359 | delay(AUTOPILOT_ADJUSTMENT_DELAY); 360 | 361 | aircraft->deploySpoilers(); 362 | delay(AUTOPILOT_ADJUSTMENT_DELAY); 363 | aircraft->retractSpoilers(); 364 | 365 | aircraft->setElevators(45); 366 | delay(AUTOPILOT_ADJUSTMENT_DELAY); 367 | aircraft->setElevators(135); 368 | delay(AUTOPILOT_ADJUSTMENT_DELAY); 369 | aircraft->setElevators(90); 370 | 371 | aircraft->setRudder(45); 372 | delay(AUTOPILOT_ADJUSTMENT_DELAY); 373 | aircraft->setRudder(135); 374 | delay(AUTOPILOT_ADJUSTMENT_DELAY); 375 | aircraft->setRudder(90); 376 | 377 | aircraft->setLeftAileron(45); 378 | delay(AUTOPILOT_ADJUSTMENT_DELAY); 379 | aircraft->setLeftAileron(135); 380 | delay(AUTOPILOT_ADJUSTMENT_DELAY); 381 | aircraft->setLeftAileron(90); 382 | 383 | aircraft->setRightAileron(45); 384 | delay(AUTOPILOT_ADJUSTMENT_DELAY); 385 | aircraft->setRightAileron(135); 386 | delay(AUTOPILOT_ADJUSTMENT_DELAY); 387 | aircraft->setRightAileron(90); 388 | 389 | delay(AUTOPILOT_ADJUSTMENT_DELAY); 390 | setCruiseMode(); 391 | } 392 | 393 | void AutoPilot::turn(const float degree) const // degree must be bipolar (-180 to +180) (negative: LEFT, positive: RIGHT) 394 | { 395 | float currentBearing = aircraft->getBearing(); 396 | setBearing(unipolarToBipolarBearing(currentBearing + degree), 0); 397 | } 398 | -------------------------------------------------------------------------------- /src/AutoPilot.h: -------------------------------------------------------------------------------- 1 | // --------------------------------------------------------------------------------------------- 2 | // Copyright (c) Akash Nag. All rights reserved. 3 | // Licensed under the MIT License. See LICENSE.md in the project root for license information. 4 | // --------------------------------------------------------------------------------------------- 5 | 6 | #ifndef AutoPilot_h 7 | #define AutoPilot_h 8 | 9 | #include "Navigation.h" 10 | #include "Aircraft.h" 11 | #include "Global.h" 12 | #include "MathUtility.h" 13 | 14 | class AutoPilot 15 | { 16 | private: 17 | Aircraft *aircraft; 18 | 19 | Location *targetLocation; 20 | float targetAltitude; 21 | bool isTargetLocationValid; 22 | bool isTargetAltitudeValid; 23 | 24 | bool setAilerons(const int) const; // degree must be bipolar: -180 to +180 25 | bool setElevators(const int) const; // degree must be bipolar: -180 to +180 26 | bool setBearing(const float, const int) const; 27 | 28 | bool isStalled(); 29 | void recoverFromStall(); 30 | bool isBeingSweptBack(); 31 | void preventBeingSweptBack(); 32 | 33 | public: 34 | AutoPilot(const Aircraft*); 35 | 36 | void testFlightControls() const; 37 | void takeOff() const; 38 | void land(const Airport&) const; 39 | void landImmediately(const float) const; 40 | 41 | void setCruiseMode() const; 42 | 43 | void setCourse(const Location&); 44 | bool keepOnCourse(const int) const; 45 | void setAltitude(const float); 46 | bool maintainAltitude(const int) const; 47 | 48 | void stabilizeRoll() const; 49 | void stabilizePitch() const; 50 | 51 | void turn(const float) const; // degree must be bipolar 52 | 53 | float getETA() const; // return ETA in seconds 54 | }; 55 | #endif 56 | -------------------------------------------------------------------------------- /src/BackProp.cpp: -------------------------------------------------------------------------------- 1 | // --------------------------------------------------------------------------------------------- 2 | // Copyright (c) Akash Nag. All rights reserved. 3 | // Licensed under the MIT License. See LICENSE.md in the project root for license information. 4 | // --------------------------------------------------------------------------------------------- 5 | 6 | #include "BackProp.h" 7 | 8 | // initializes and allocates memory on heap 9 | BackProp::BackProp(int nl,int *sz,double b,double a):beta(b),alpha(a) 10 | { 11 | // set no of layers and their sizes 12 | numl=nl; 13 | int i=0; 14 | lsize=new int[numl]; 15 | 16 | for(i=0;i