├── .github └── workflows │ └── manual.yml ├── CODEOWNERS ├── Diagrams ├── airspeed_hold.png ├── airspeed_pitch_hold.png ├── altitude_hold.png ├── course_hold.png ├── eval.png ├── fw_challenge.png ├── pitch_loop.png ├── roll_loop.png ├── sideslip_hold.PNG ├── tuning1.PNG ├── tuning2.PNG └── tuning3.PNG ├── README.md ├── fixed_wing_project.py ├── gains.txt ├── plane_control.py └── plane_drone.py /.github/workflows/manual.yml: -------------------------------------------------------------------------------- 1 | # Workflow to ensure whenever a Github PR is submitted, 2 | # a JIRA ticket gets created automatically. 3 | name: Manual Workflow 4 | 5 | # Controls when the action will run. 6 | on: 7 | # Triggers the workflow on pull request events but only for the master branch 8 | pull_request_target: 9 | types: [opened, reopened] 10 | 11 | # Allows you to run this workflow manually from the Actions tab 12 | workflow_dispatch: 13 | 14 | jobs: 15 | test-transition-issue: 16 | name: Convert Github Issue to Jira Issue 17 | runs-on: ubuntu-latest 18 | steps: 19 | - name: Checkout 20 | uses: actions/checkout@master 21 | 22 | - name: Login 23 | uses: atlassian/gajira-login@master 24 | env: 25 | JIRA_BASE_URL: ${{ secrets.JIRA_BASE_URL }} 26 | JIRA_USER_EMAIL: ${{ secrets.JIRA_USER_EMAIL }} 27 | JIRA_API_TOKEN: ${{ secrets.JIRA_API_TOKEN }} 28 | 29 | - name: Create NEW JIRA ticket 30 | id: create 31 | uses: atlassian/gajira-create@master 32 | with: 33 | project: CONUPDATE 34 | issuetype: Task 35 | summary: | 36 | Github PR [Assign the ND component] | Repo: ${{ github.repository }} | PR# ${{github.event.number}} 37 | description: | 38 | Repo link: https://github.com/${{ github.repository }} 39 | PR no. ${{ github.event.pull_request.number }} 40 | PR title: ${{ github.event.pull_request.title }} 41 | PR description: ${{ github.event.pull_request.description }} 42 | In addition, please resolve other issues, if any. 43 | fields: '{"components": [{"name":"Github PR"}], "customfield_16449":"https://classroom.udacity.com/", "customfield_16450":"Resolve the PR", "labels": ["github"], "priority":{"id": "4"}}' 44 | 45 | - name: Log created issue 46 | run: echo "Issue ${{ steps.create.outputs.issue }} was created" 47 | -------------------------------------------------------------------------------- /CODEOWNERS: -------------------------------------------------------------------------------- 1 | * @udacity/active-public-content -------------------------------------------------------------------------------- /Diagrams/airspeed_hold.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-FixedWing/6f7f67865f92cde35c6e72aeb1a4d8be8ff577f5/Diagrams/airspeed_hold.png -------------------------------------------------------------------------------- /Diagrams/airspeed_pitch_hold.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-FixedWing/6f7f67865f92cde35c6e72aeb1a4d8be8ff577f5/Diagrams/airspeed_pitch_hold.png -------------------------------------------------------------------------------- /Diagrams/altitude_hold.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-FixedWing/6f7f67865f92cde35c6e72aeb1a4d8be8ff577f5/Diagrams/altitude_hold.png -------------------------------------------------------------------------------- /Diagrams/course_hold.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-FixedWing/6f7f67865f92cde35c6e72aeb1a4d8be8ff577f5/Diagrams/course_hold.png -------------------------------------------------------------------------------- /Diagrams/eval.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-FixedWing/6f7f67865f92cde35c6e72aeb1a4d8be8ff577f5/Diagrams/eval.png -------------------------------------------------------------------------------- /Diagrams/fw_challenge.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-FixedWing/6f7f67865f92cde35c6e72aeb1a4d8be8ff577f5/Diagrams/fw_challenge.png -------------------------------------------------------------------------------- /Diagrams/pitch_loop.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-FixedWing/6f7f67865f92cde35c6e72aeb1a4d8be8ff577f5/Diagrams/pitch_loop.png -------------------------------------------------------------------------------- /Diagrams/roll_loop.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-FixedWing/6f7f67865f92cde35c6e72aeb1a4d8be8ff577f5/Diagrams/roll_loop.png -------------------------------------------------------------------------------- /Diagrams/sideslip_hold.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-FixedWing/6f7f67865f92cde35c6e72aeb1a4d8be8ff577f5/Diagrams/sideslip_hold.PNG -------------------------------------------------------------------------------- /Diagrams/tuning1.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-FixedWing/6f7f67865f92cde35c6e72aeb1a4d8be8ff577f5/Diagrams/tuning1.PNG -------------------------------------------------------------------------------- /Diagrams/tuning2.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-FixedWing/6f7f67865f92cde35c6e72aeb1a4d8be8ff577f5/Diagrams/tuning2.PNG -------------------------------------------------------------------------------- /Diagrams/tuning3.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/FCND-FixedWing/6f7f67865f92cde35c6e72aeb1a4d8be8ff577f5/Diagrams/tuning3.PNG -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # FCND - Fixed Wing Project # 2 | 3 | This is the readme for the Udacity Flying Car Nanodegree Fixed Wing Project 4 | 5 | For easy navigation throughout this document, here is an outline: 6 | 7 | - [Development environment setup](#development-environment-setup) 8 | - [Simulator walkthrough](#simulator-walkthrough) 9 | - [Control the Simulator with Python](#pyton-control) 10 | - [The Scenarios](#the-scenarios) 11 | - [Evaluation](#evaluation) 12 | 13 | 14 | ## Development Environment Setup ## 15 | 16 | Regardless of your development platform, the first step is to download or clone this repository. 17 | 18 | ### Udacidrone ### 19 | 20 | Next you'll need to get the latest version of [Udacidrone](https://udacity.github.io/udacidrone/docs/getting-started.html). 21 | If you've previously installed Udacidrone, ensure that you are working with version 0.3.4 or later. 22 | 23 | You can update Udacidrone by running the following from the command line: 24 | 25 | `pip install -U git+https://github.com/udacity/udacidrone.git` 26 | 27 | ### Unity Simulation ### 28 | 29 | Finally, download the version of the simulator that's appropriate for your operating system [from this repository](https://github.com/udacity/FCND-FixedWing/releases). 30 | 31 | Note: you may need to pass an exception with anti-virus to run the simulator 32 | 33 | ## Simulator Walkthrough ## 34 | 35 | Now that you have all the code on your computer and the simulator running, let's walk through some of the elements of the code and the simulator itself. 36 | 37 | ### Fixed Wing UI ### 38 | 39 | The simulator interface should look fairly familiar to the Unity quadcopter simulation with a few additions: 40 | 41 | - Airspeed is displayed along with the GPS information 42 | - Scenario menu (explained more below), the simulation will start in the Sandbox mode 43 | - Throttle setting (displays the throttle setting between 0 (no throttle) and 1 (full throttle) 44 | 45 | ### Sandbox Mode ### 46 | 47 | Try flying around in sandbox mode. There are several different flight modes available, you'll start in manual. 48 | 49 | #### Manual Mode #### 50 | 51 | You have direct control over the throttle, ailerons, elevators, and rudder. Use the following keyboard commands to control the aircraft: 52 | 53 | - C/Space: throttle control (incremental) 54 | - Up/Down Arrow: elevator control 55 | - Left/Right Arrow: aileron control 56 | - Q/E: rudder control 57 | - W/S: elevator trim (incremental) 58 | 59 | 60 | ### Scenario Selection ### 61 | 62 | Click the scenario selection menu to see a drop-down list of possible scenarios. When you select a scenario, the aircraft will be reset to a specified starting location and you will be shown a start-up screen. The start-up screen will describe the scenario task and the evaluation criteria. In the start-up screen, you'll have two options to choose from: 63 | 64 | - Tune parameters: select control parameters for the Unity simulation will be displayed for tuning prior to starting the scenario. The scenario will be run with the unity simulation 65 | - Run Python code: a screen will appear waiting for Python controller to connect/arm. The screen will disappear and the scenario will start once the vehicle is armed from Python 66 | 67 | Most of the scenarios have a time limit associated with them. Upon completion (pass or fail) there will be a window showing your result. From there, you can select another scenario or continue retry the current scenario. 68 | 69 | When running a scenario, there is additional information displayed specific to the scenario as shown below: 70 | 71 | ![eval](Diagrams/eval.png) 72 | 73 | The value of the four primary control surfaces are shown for all scenarios. Throttle is displayed as a percentage of maximum throttle. Elevator, rudder, and aileron are displayed as a value between -1 and 1 correspond to minimum and maximum control deflections, respectively. 74 | 75 | The scenario may also display 1 or 2 parameters which are being used to evaluate success for the scenario. The bar will be green if the parameter is within the acceptable threshold and red otherwise. A few scenarios may include parameters that aren't used to determine success (i.e. pitch angle in the altitude hold scenario). The bar for these parameters will always be red. 76 | 77 | ### Tuning Unity Parameters 78 | 79 | The gains of the control system used internally on the Unity simulation can be tuned. The longitudinal and lateral control gains are tuned independently but the control gains within each type build upon each other (i.e. you cannot tune the outer loop without first tuning the inner loop gains). To tune the gains: 80 | 81 | 1. Default gains are loaded from the text file, gains.txt. The file will be located in the same folder as the simulation executable. Set the values prior to running th program. 82 | 2. Choose the appropriate scenario from the scenario selection menu 83 | 3. Click the "Tune Parameter" button 84 | 85 | ![tune_param](Diagrams/tuning1.PNG) 86 | 87 | 4. A list of parameters will appear. Adjust the sliders or the set the fields to the values you'd like for the parameters. 88 | 89 | ![params](Diagrams/tuning2.PNG) 90 | 91 | 5. Run the scenario! 92 | 6. If you want to save the parameters to a text file, click the save button on the top right corner. The parameters are saved to a file named gains_new.txt. To use these default values next time the simulation is executed, rename the file to gains.txt 93 | 94 | ![save](Diagrams/tuning3.PNG) 95 | 96 | 97 | ## Python Control ## 98 | 99 | The simulation can also be controlled using a Python script and the Udacidrone API. There are three relevant python files found in the FixedWing project repository: 100 | 101 | - plane_control.py: this is where you will fill in the control code 102 | - plane_drone.py: contains PlaneDrone, a sub-class of the Udacidrone drone class with additional commands specific to the fixed wing project 103 | - fixed_wing_project.py: contains a subclass of PlaneDrone specifically set-up to run the scenarios 104 | 105 | ### Running a scenario ### 106 | To run a scenario from Python: 107 | 1. Select the scenario within the Unity simulation 108 | 2. Select the "Run Python Code" button, you should see a "Waiting for Python" message 109 | 3. Execute fixed_wing_project.py using the appropriately numbered scenario and the scenario should start autonomatically: 110 | 111 | ~~~ 112 | python fixed_wing_project.py -[scenario#] 113 | ~~~ 114 | 115 | For example, the following will execute the altitude hold scenario: 116 | 117 | ~~~ 118 | python fixed_wing_project.py -2 119 | ~~~ 120 | 121 | OR 122 | 123 | 3. With Udacidrone (v0.3.3 or later), the Unity simulator will automatically notify Python of the scenario number being run. Execute fixed_wing_project.py without a scenario number to automatically execute this scenario: 124 | 125 | ~~~py 126 | 127 | # Automatically detects which scenario the Unity simulation is executing 128 | python fixed_wing_project.py 129 | ~~~ 130 | 131 | If fixed_wing_project.py is run before being prompted onscreen in the Unity simulation, the python code will be unable to connect. 132 | 133 | Note: there is a known bug where the python may not connect and get stalled trying to connect (usually after a scenario has already been run). If this happens: 134 | - No need to stop your python script, leave it running 135 | - Click cancel on the "Waiting for Python" window 136 | - Click "Run Python Code" 137 | - The python script should now successfully connect! 138 | 139 | 140 | ## The Scenarios ## 141 | 142 | You'll be implementing several Python controllers in the plane_control.py in order to complete all the scenarios. The scenarios are divided into 2 categories: 143 | 144 | - Longitudinal Scenarios 145 | - Lateral/Directional Scenarios 146 | 147 | The scenarios within a category will build on one another, so you will need to implement and tune them in order. Each category will end in a challenge which will use the control loops you set up in the preceding scenarios. 148 | 149 | Prior to completing a scenario, it's suggested that you first use the Unity based controller to tune the gains. If implemented correctly on Python, the Unity controller gains should get close to meeting the objectives of the scenario, although minor tuning may be required. 150 | 151 | 152 | ### Longitudinal Scenarios ### 153 | 154 | The longitudinal scenarios are designed to incrementally implement control loops to command the aircrafts airspeed, pitch, and altitude using the elevator and the throttle. When running these scenarios from Python, a Unity based lateral controller will maintain a near-zero bank and sideslip. 155 | 156 | #### Scenario #1: Trim (Unity Only) #### 157 | 158 | The objective of this scenario is to find a fixed throttle trim for level flight with no elevator input. 159 | 160 | To achieve success in this scenario two objectives must be met for at least 5 seconds: 161 | 162 | - The vertical speed must be less than 0.5 m/s 163 | - The airspeed rate must be less than 0.1 m^2/s 164 | 165 | This scenario will run indefinitely. 166 | 167 | Completing this scenario will help find a route estimate for your feed-forward throttle setting. 168 | 169 | Tips: 170 | 171 | - Try small increments in your throttle 172 | - If both the airspeed AND altitude are increasing, the throttle is probably too high. 173 | - If both the airspeed AND altitude are decreasing, the throttle is probably too low. 174 | - If the airspeed and altitude are opposite from one another, you'll have to wait for the the phugoid mode to damp out. 175 | - The phugoid oscillations can be very lightly damped, the damping can be assisted by using the elevator controls. 176 | 177 | 178 | 179 | #### Scenario #2: Altitude Hold #### 180 | 181 | ![altitude](Diagrams/altitude_hold.png) 182 | 183 | The objective of this scenario is to tune/implement a controller to maintain a constant altitude using the elevator. The throttle will be set to a fixed value. The altitude hold should be implemented using successive loop closure as shown above. The inner loop will be a PD controller on the aircraft pitch. The outer loop will be a PI controller on the aircraft altitude. Ensure to implement anti-windup for the integrator. 184 | 185 | 186 | To complete this scenario: 187 | 188 | - The altitude must be within +/-3 meters of the target altitude (450 meters) within 10s 189 | - The altitude must maintain within those bounds for 5 seconds 190 | 191 | This controller should be implemented in plane_control.py, by filling in the following functions: 192 | ~~~py 193 | 194 | """Used to calculate the elevator command required to acheive the target 195 | pitch 196 | 197 | Args: 198 | pitch: in radians 199 | pitch_rate: in radians/sec 200 | pitch_cmd: in radians 201 | 202 | Returns: 203 | elevator_cmd: in percentage elevator [-1,1] 204 | """ 205 | def pitch_loop(self, pitch, pitch_rate, pitch_cmd): 206 | elevator_cmd = 0.0 207 | # STUDENT CODE HERE 208 | return elevator_cmd 209 | 210 | """Used to calculate the pitch command required to maintain the commanded 211 | altitude 212 | 213 | Args: 214 | altitude: in meters (positive up) 215 | altitude_cmd: in meters (positive up) 216 | dt: timestep in seconds 217 | 218 | Returns: 219 | pitch_cmd: in radians 220 | """ 221 | def altitude_loop(self, altitude, altitude_cmd, dt): 222 | pitch_cmd = 0.0 223 | # STUDENT CODE HERE 224 | return pitch_cmd 225 | ~~~ 226 | 227 | 228 | Tips: 229 | 230 | - Implement and tune the inner most loop first and work outward. 231 | - Increase the proportional pitch gain until the aircraft is nearly unstable OR set a maximum pitch angle and set the gain to be full elevator at the maximum pitch angle 232 | - Increase the derivative pitch gain to achieve a nice dynamic response (fast but not too many oscillations) 233 | - Next, increase the proportional altitude gain until your achieve a nice step response 234 | - Finally, increase the integral altitude gain to meet the scenario objective threshold. 235 | 236 | 237 | #### Scenario #3: Airspeed Hold #### 238 | 239 | ![airspeed_hold](Diagrams/airspeed_hold.png) 240 | 241 | The objective of this scenario is to tune/implement a controller to maintain a constant airspeed. The altitude will be maintained using the altitude controller from the previous scenario. The controller should be implemented as a PI controller using the throttle setting. Ensure to implement anti-windup for the integrator. 242 | 243 | To complete this scenario: 244 | 245 | - The airspeed must be within +/- 0.5 meters/sec of the target airspeed (41 meters/sec) within 10s 246 | - The airspeed must maintain the airspeed within those bounds for 5 seconds 247 | 248 | This controller should be implemented in plane_control.py, by filling in the following function: 249 | ~~~py 250 | 251 | """Used to calculate the throttle command required command the target 252 | airspeed 253 | 254 | Args: 255 | airspeed: in meters/sec 256 | airspeed_cmd: in meters/sec 257 | dt: timestep in seconds 258 | 259 | Returns: 260 | throttle_command: in percent throttle [0,1] 261 | """ 262 | def airspeed_loop(self, airspeed, airspeed_cmd, dt): 263 | throttle_cmd = 0.0 264 | # STUDENT CODE HERE 265 | return throttle_cmd 266 | ~~~ 267 | 268 | 269 | 270 | Tips: 271 | 272 | - First, set your throttle feed-forward value determined from the trim analysis 273 | - Next, increase the proportional gain until you get an acceptable step response (fast but not too many oscillations) 274 | - Increase the integral gain to increase the rate at which the steady state error is removed 275 | - If you notice a large contribution from the integral portion of your controller at steady state, adjust your feed-forward throttle setting accordingly. This should allow you to decrease the value of you integral gain (and provide a better dynamic response). 276 | 277 | 278 | #### Scenario #4: Steady Climb #### 279 | 280 | ![climb](Diagrams/airspeed_pitch_hold.png) 281 | 282 | The objective of this scenario is to tune/design a controller to maintain a constant airspeed using the elevator with full throttle and a PI controller as shown above. This will put the aircraft in a steady climb. In the previous scenario, the pitch angle was used to control altitude. In this scenario, the pitch angle will be used to control the airspeed. Ensure to implement anti-windup for the integrator. 283 | 284 | To complete this scenario: 285 | 286 | - The airspeed must be within +/-1 meters/sec of the target airspeed (41 meters/sec) within 10s 287 | - The airspeed must maintain within those bounds for 5 seconds 288 | 289 | This controller should be implemented in plane_control.py, by filling in the following functions: 290 | ~~~py 291 | 292 | """Used to calculate the pitch command required to maintain the commanded 293 | airspeed 294 | 295 | Args: 296 | airspeed: in meters/sec 297 | airspeed_cmd: in meters/sec 298 | dt: timestep in seconds 299 | 300 | Returns: 301 | pitch_cmd: in radians 302 | """ 303 | 304 | def airspeed_pitch_loop(self, airspeed, airspeed_cmd, dt): 305 | pitch_cmd = 0.0 306 | # STUDENT CODE HERE 307 | return pitch_cmd 308 | ~~~ 309 | 310 | Tips: 311 | 312 | - Use the same inner loop pitch controller from the previous scenario. If you change the inner loop gains, return to the previous scenario to ensure they meet the altitude hold objectives. 313 | - If you saturated the commanded pitch inside the inner loop, you may have to increase the limits. A steady climb at full throttle will need to command higher pitch angles than a constant altitude controller. 314 | - Start with increasing the proportional airspeed gain to achieve a nice dynamic response (fast but not too many oscillations) 315 | - Finally, increase the integral airspeed gain to meet the scenario objective threshold. 316 | 317 | 318 | ### Scenario #5: Longitudinal Challenge ### 319 | 320 | The objective of this challenge is to successfully fly through a series of virtual gates in the sky. 321 | To do this, tune/implement a longitudinal state machine: 322 | 323 | - If the vehicle is within a specified threshold of the target altitude, use the maintain altitude controller. 324 | - If the vehicle is below or above the target altitude by the specified threshold, use the steady climb/descend controller with full or min throttle (respectively) 325 | 326 | To complete the challenge, your altitude must be within +/-3 meters when arriving at the gate. 327 | The gate locations (x = horizontal distance from the start location): 328 | 329 | - Gate #1: x = 200m, altitude=200m 330 | - Gate #2: x = 1100m, altitude = 300m 331 | - Gate #3: x = 1400m, altitude = 280m 332 | - Gate #4: x = 2200m, altitude = 200m 333 | 334 | This controller should be implemented in plane_control.py, by filling in the following functions: 335 | 336 | ~~~py 337 | 338 | """Used to calculate the pitch command and throttle command based on the 339 | aicraft altitude error 340 | 341 | Args: 342 | airspeed: in meter/sec 343 | altitude: in meters (positive up) 344 | airspeed_cmd: in meters/sec 345 | altitude_cmd: in meters/sec (positive up) 346 | dt: timestep in seconds 347 | 348 | Returns: 349 | pitch_cmd: in radians 350 | throttle_cmd: in in percent throttle [0,1] 351 | """ 352 | def longitudinal_loop(self, airspeed, altitude, airspeed_cmd, altitude_cmd, dt): 353 | pitch_cmd = 0.0 354 | throttle_cmd = 0.0 355 | # STUDENT CODE HERE 356 | return[pitch_cmd, throttle_cmd] 357 | ~~~ 358 | 359 | Tips: 360 | 361 | - The only parameter to tune in the challenge should be the threshold at which to switch between the different modes of control. The altitude hold scenario starts the aircraft 20m below the target altitude, which would be a good starting point for this threshold. 362 | - Well tuned parameters from the Longitudinal scenarios will help make this challenge less challenging. If you change your control gains for this challenge, return to the scenarios to ensure the new gains meet the objectives of all previous scenarios. 363 | 364 | ### Lateral/Directional Scenarios ### 365 | 366 | The lateral/directional scenarios are designed to incrementally implement control loops to command the aircrafts airspeed, pitch, and altitude using the elevator and the throttle. When running these scenarios from Python, a Unity based longitudinal controller will maintain altitude and airspeed. 367 | 368 | #### Scenario #6: Stabilized Roll Angle #### 369 | 370 | ![roll](Diagrams/roll_loop.png) 371 | 372 | The objective of this scenario is to tune/design a controller to maintain a constant roll angle using PD control as shown above. The aircraft will start at 45 degree roll angle and the controller should return the aircraft to wings level (0 degree roll). In this scenario roll angle and roll rate will be used to calculate a desired aileron command. 373 | 374 | To complete this scenario: 375 | 376 | - The roll angle must be within +/- 5 degrees within 5s 377 | - The roll angle must maintain within those bounds for 5 seconds 378 | 379 | This controller should be implemented in plane_control.py, by filling in the following functions: 380 | ~~~py 381 | 382 | """Used to calculate the commanded aileron based on the roll error 383 | 384 | Args: 385 | phi_cmd: commanded roll in radians 386 | phi: roll angle in radians 387 | roll_rate: in radians/sec 388 | T_s: timestep in sec 389 | 390 | Returns: 391 | aileron: in percent full aileron [-1,1] 392 | """ 393 | def roll_attitude_hold_loop(self, 394 | phi_cmd, # commanded roll 395 | phi, # actual roll 396 | roll_rate, 397 | T_s = 0.0): 398 | aileron = 0 399 | # STUDENT CODE HERE 400 | return aileron 401 | ~~~ 402 | 403 | Tips: 404 | 405 | - Increase the proportional until you start seeing small oscillations near wings level. Add a derivative term to smooth the dynamic response 406 | - Due to the high drag of the wings, the derivative term may not be necessary to complete the objectives. The derivative term is required for a complete solution 407 | - The aircraft should be near symmetric, but if you are seeing a small steady state error near wings level, increase your proportional gain to decrease this error. Do not add an integral term! 408 | 409 | 410 | #### Scenario #7: Coordinated Turn #### 411 | 412 | ![turn](Diagrams/sideslip_hold.PNG) 413 | 414 | The objective of this scenario is to tune/design a controller to regulate the sideslip of the aircraft during a banked turn using PI control as shown above. The aircraft will be commanded to a 45 degree bank angle, which will cause the aircraft to sideslip. The coordinated turn assumptions used in the course hold assume that the turn is coordinated and the sideslip is near zero. The sideslip (approximated from the aircraft velocity and heading) will be used to calculate the rudder command. Ensure to implement anti-windup for the integrator. 415 | 416 | To complete this scenario: 417 | 418 | - The sideslip must be within +/- 0.5 degrees within 25s 419 | - The airspeed must maintain the airspeed within those bounds for 5 seconds 420 | - The integrator uses a discrete integration technique. The timestep used in the Unity simulation is smaller than the python timestep, therefore you may experience integration issues when moving to the Python simulation. You may need to lower your dependence on the integral term when executing python control (smaller gain) or try a higher order integration method (trapezoidal instead of Euler). 421 | 422 | This controller should be implemented in plane_control.py, by filling in the following functions: 423 | 424 | ~~~py 425 | 426 | """Used to calculate the commanded rudder based on the sideslip 427 | 428 | Args: 429 | beta: sideslip angle in radians 430 | T_s: timestep in sec 431 | 432 | Returns: 433 | rudder: in percent full rudder [-1,1] 434 | """ 435 | def sideslip_hold_loop(self, 436 | beta, # sideslip angle 437 | T_s): 438 | rudder = 0 439 | # STUDENT CODE HERE 440 | return rudder 441 | ~~~ 442 | 443 | Tips: 444 | 445 | - Increase the proportional gain to meet the objectives. Increase the integral gain to remove any remaining steady state error. 446 | - You may not be able to drive the steady state error complete to zero, but the smaller it is, the easier the following scenarios will be. 447 | 448 | #### Scenario #8: Constant Course/Yaw Hold #### 449 | 450 | ![course](Diagrams/course_hold.png) 451 | 452 | The objective of this scenario is to tune/design a controller to maintain a constant course/yaw angle using PI control as shown above. The target yaw is zero degrees; the aircraft will start with a 45 degree yaw. The current vehicle heading will be used to calculate a roll command. Ensure to implement anti-windup for the integrator. The commanded roll rate should be saturated between at a 60 degrees maximum. The feed-forward roll should be included in this solution. The feed-forward for this scenario but his term is used in the orbit scenario. 453 | 454 | To complete this scenario: 455 | 456 | - The yaw must be within +/- 5 degrees of the target course (0 degrees) within 10s 457 | - The airspeed must maintain the airspeed within those bounds for 5 seconds 458 | 459 | This controller should be implemented in plane_control.py, by filling in the following functions: 460 | 461 | ~~~py 462 | 463 | """Used to calculate the commanded roll angle from the course/yaw angle 464 | Args: 465 | yaw_cmd: commanded yaw in radians 466 | yaw: roll angle in radians 467 | roll_rate: in radians/sec 468 | T_s: timestep in sec 469 | roll_ff: feed-forward roll command (for orbit scenario) 470 | 471 | Returns: 472 | roll_cmd: commanded roll in radians 473 | """ 474 | def yaw_hold_loop(self, 475 | yaw_cmd, # desired heading 476 | yaw, # actual heading 477 | T_s, 478 | roll_ff=0): 479 | roll_cmd = 0 480 | # STUDENT CODE HERE 481 | return roll_cmd 482 | 483 | ~~~ 484 | 485 | Tips: 486 | 487 | - Ensure the course error is appropriately between -PI and PI (i.e. a heading of 350 degrees should have 10 degrees of error, not -350!) 488 | - Increase the proportional gain to have the dynamic response desired (speedy with little/no overshoot). Add a small integral gain to remove any steady state error. 489 | - Since there are no disturbances, an integral gain should not be required to complete this scenario but should be included in the final solution. The integral gain will also help complete the following scenarios. 490 | - The integrator uses a discrete integration technique. The timestep used in the Unity simulation is smaller than the python timestep, therefore you may experience integration issues when moving to the Python simulation. You may need to lower your dependence on the integral term when executing python control (smaller gain) or try a higher order integration method (trapezoidal instead of Euler). 491 | 492 | #### Scenario #9: Straight Line Following #### 493 | 494 | The objective of this scenario is to tune/design a controller to track the aircraft to the desired line. The line will be defined as an origin point and course. You'll first calculate the crosstrack error from the line and use that to generate a commanded heading (based on an arctan based trajectory towards the line). The infite course approach angle used in the simulation is perpendicular to the line (PI/2), but feel free to play around with different values in your python implementation. The aircraft will start 20 meters to offset from the line. 495 | 496 | To complete this scenario: 497 | 498 | - The crosstrack error from the line must be within +/- 3 meters within 25s 499 | - The crosstrack error must maintain within those bounds for 5s 500 | 501 | This controller should be implemented in plane_control.py, by filling in the following functions: 502 | 503 | ~~~py 504 | 505 | """Used to calculate the desired course angle based on cross-track error 506 | from a desired line 507 | 508 | Args: 509 | line_origin: point on the desired line in meters [N, E, D] 510 | line_course: heading of the line in radians 511 | local_position: vehicle position in meters [N, E, D] 512 | 513 | Returns: 514 | course_cmd: course/yaw cmd for the vehicle in radians 515 | """ 516 | def straight_line_guidance(self, line_origin, line_course, 517 | local_position): 518 | 519 | course_cmd = 0 520 | # STUDENT CODE HERE 521 | return course_cmd 522 | 523 | ~~~ 524 | 525 | Tips: 526 | 527 | - Make sure to include the course of the line in your final course command. You will still be able to complete this scenario while forgetting it because the course command is 0 degrees, but it will hurt you in future scenarios. 528 | - When using the Unity simulation, a blue arrow is provided on the compass heading showing your desired heading angle. If that heading moves faster than the aircraft can change its heading, your line following gain may be too high. If aircraft heading oscillates around that value, you may need to return to the previous scenario and adjust your proportional yaw gain. If the aircraft heading lags behind the blue arrow, you may need to adjust your integral yaw gain in the previous scenario. 529 | 530 | #### Scenario #10: Orbit Following #### 531 | 532 | The objective of this scenario is to tune/design a controller to track the vehicle to a circular orbit. This controller requires two parts. The first generates a course command based on the current radius from the orbit origin and the aircraft heading. The second part calculates the feed-forward roll required for desired turning radius of the orbit (assuming a coordinate turn). Although this scenario is a clockwise orbit, the controller should be able to handle counter clockwise turns also. The aircraft will start with zero roll angle on the orbit. 533 | 534 | To complete this scenario: 535 | 536 | - The radius from the orbit center (North = 0 meters, East = 500 meters) must be within +/- 5 meters of the target radius (500 meters) within 15s 537 | - The radius from the orbit center must maintain within those bounds for 5 seconds 538 | 539 | This controller should be implemented in plane_control.py, by filling in the following functions: 540 | 541 | ~~~py 542 | 543 | """Used to calculate the desired course angle based on radius error from 544 | a specified orbit center 545 | 546 | Args: 547 | orbit_center: in meters [N, E, D] 548 | orbit_radius: desired radius in meters 549 | local_position: vehicle position in meters [N, E, D] 550 | yaw: vehicle heading in radians 551 | clockwise: specifies whether to fly clockwise (increasing yaw) 552 | 553 | Returns: 554 | course_cmd: course/yaw cmd for the vehicle in radians 555 | """ 556 | def orbit_guidance(self, orbit_center, orbit_radius, local_position, yaw, 557 | clockwise = True): 558 | 559 | course_cmd = 0 560 | # STUDENT CODE HERE 561 | return course_cmd 562 | 563 | """Used to calculate the feedforward roll angle for a constant radius 564 | coordinated turn 565 | 566 | Args: 567 | speed: the aircraft speed during the turn in meters/sec 568 | radius: turning radius in meters 569 | cw: true=clockwise turn, false = counter-clockwise turn 570 | 571 | Returns: 572 | roll_ff: feed-forward roll in radians 573 | """ 574 | def coordinated_turn_ff(self, speed, radius, cw): 575 | 576 | roll_ff = 0 577 | # STUDENT CODE HERE 578 | return roll_ff 579 | 580 | ~~~ 581 | 582 | Tips: 583 | 584 | - Increase the gain to ensure the course command guides the vehicle back to the orbit within the threshold 585 | - You will not be able to complete this scenario without including the feed-foward term in your course controller 586 | - When using the Unity simulation, a blue arrow is provided on the compass heading showing your desired heading angle. If that heading moves faster than the aircraft can change its heading, your line following gain may be too high. If aircraft heading oscillates around that value, you may need to return to the previous scenario and adjust your proportional yaw gain. If the aircraft heading lags behind the blue arrow, you may need to adjust your integral yaw gain in the previous scenario. 587 | 588 | ### Scenario #11: Lateral/Directional Challenge ### 589 | 590 | The objective of this challenge is to successfully fly through a series of virtual gates in the sky. All the positions given in this scenario are relative to the vehicle start location. 591 | To do this, tune/implement a lateral state machine using the vehicle position and heading to generate a course command and feed-forward roll. Your state machine should include function calls to the straight_line_guidance, orbit_guidance, and coordinated_turn_ff functions: 592 | 593 | The first leg (before Gate #1) should implement a line following controller between North = 0 meters, East = 20 meters and Gate #1. 594 | The second leg (between Gate#1 and Gate #2) should implement an orbit following controller around the orbit center North = 500 meters, East = -380 meters (radius = 400 meters) 595 | The third leg (between Gate #2 and Gate #3) should implement an orbit following controller around the orbit center North = 600 meters, East = -380 (radius = 300 meters) 596 | The final leg (between Gate #3 and Gate #4) should implement a line following controller between the two gates. 597 | 598 | To complete the challenge, you must be within +/-5 meter lateral distance when arriving at the gate. 599 | 600 | The gate locations: 601 | 602 | - Gate #1: North = 500m, East = 20m 603 | - Gate #2: North = 900m, East = -380m 604 | - Gate #3: North = 600m, East = -680m 605 | - Gate #4: North = 100m, East = -680m 606 | 607 | This controller should be implemented in plane_control.py, by filling in the following functions: 608 | 609 | ~~~py 610 | 611 | """Used to calculate the desired course angle and feed-forward roll 612 | depending on which phase of lateral flight (orbit or line following) the 613 | aicraft is in 614 | 615 | Args: 616 | local_position: vehicle position in meters [N, E, D] 617 | yaw: vehicle heading in radians 618 | airspeed_cmd: in meters/sec 619 | 620 | Returns: 621 | roll_ff: feed-forward roll in radians 622 | yaw_cmd: commanded yaw/course in radians 623 | """ 624 | def path_manager(self, local_position, yaw, airspeed_cmd): 625 | 626 | roll_ff = 0 627 | yaw_cmd = 0 628 | # STUDENT CODE HERE 629 | 630 | return(roll_ff,yaw_cmd) 631 | 632 | ~~~ 633 | 634 | Tips: 635 | 636 | - If you see a slow response when going from line following to orbit follow (or vice versa), reset your integrators to 0 when transitioning to a new phase of flight. 637 | - If you having trouble with the last straight line leg, did you include the course command in your line following controller? 638 | - If you having trouble with the orbit segments, did you implement the changes necessary to do a counter clockwise orbit? 639 | - There are not any gains to tune for this scenario, so if the vehicle seems to be in the correct phase of flight, but you are still not hitting the gates, go back to previous scenarios to exceed the scenario requirements, not just meet minimum requirements. 640 | 641 | ### Scenario #12: Full 3D Challenge ### 642 | 643 | This challenge is meant to test your longitudinal and lateral controllers working together. Your goal is to control the aircraft between a series of waypoints. In between waypoints, you'll controll the aircraft using a line-following controller. To transition between segments, you'll use an orbit following controller with a 500 meter radius. 644 | 645 | To complete this challenge, you'll implement your controller to hit the target gates within the error threshold (5m). Each of the gates are positioned where the aircraft should transition between segments (using a 500m radius orbit) 646 | 647 | The waypoint locations (N/E or relative to the vehicle start location, D is the absolute down value): 648 | - Waypoint #1: N=0m, E=500m, D=-400m 649 | - Waypoint #2: N-2600m, E=500m, D=-500m 650 | - Waypoint #3: N=2600m, E=-2500m, D=-400m 651 | - Waypoint #4: N=100m, E=500m, D=-450m 652 | - Waypoint #5: N=100m, E=-2000m, D=-450m 653 | 654 | This controller should be implemented in plane_control.py, by filling in the following function: 655 | 656 | ~~~py 657 | 658 | """Used to calculate the desired course angle and feed-forward roll 659 | depending on which phase of lateral flight (orbit or line following) the 660 | aicraft is in 661 | 662 | Args: 663 | waypoint_tuple: 3 waypoints, (prev_waypoint, curr_waypoint, next_waypoint), waypoints are in meters [N, E, D] 664 | local_position: vehicle position in meters [N, E, D] 665 | yaw: vehicle heading in radians 666 | airspeed_cmd: in meters/sec 667 | 668 | Returns: 669 | roll_ff: feed-forward roll in radians 670 | yaw_cmd: commanded yaw/course in radians 671 | cycle: True=cycle waypoints (at the end of orbit segment) 672 | """ 673 | def waypoint_follower(self, waypoint_tuple, local_position, yaw, airspeed_cmd): 674 | 675 | roll_ff = 0.0 676 | yaw_cmd = 0.0 677 | cycle = False 678 | 679 | # STUDENT CODE HERE 680 | 681 | return (roll_ff, yaw_cmd, cycle) 682 | 683 | ~~~ 684 | 685 | The waypoint follower can be implemented as a state machine: either the vehicle is line following or orbit following. The transition between the two controllers is set based on where a 500m radius circle is both tangent to the current leg (prev_waypoint->curr_waypoint) and the next leg (curr_waypoint->next_waypoint): 686 | 687 | - When the aircraft crosses a hyper plane defined by a line perpendicular to the current leg going through the first tangent point, transition from line following to orbit following. The first tangent point is location of the gates. The location of the gates is given below. 688 | - When the aircraft crosses a hyper plane defined by a line perpendicular to the next leg going through the second tangent point, transition from orbit following to line following and cycle the waypoints. To help in calculating these points, the orbit centers and the second tangent points are given below. 689 | 690 | The longitudinal controller will use the state machine you implemented in the Longitudinal Challenge scenario. 691 | 692 | The gate positions (N/E relative to aircraft start position, D is absolute down): 693 | - Gate #1: N=2100m, E=500m, D=-450m 694 | - Gate #2: N=2600m, E=-1119m, D=-500m 695 | - Gate #3: N=984m, E=-560m, D=-400m 696 | - Gate #4: N=100m, E=-200m, D=-450m 697 | 698 | The orbit centers and transition points from orbit->line 699 | - Center #1: N=2100m, E=0m 700 | - Transition: N=2600m, E=0m 701 | 702 | - Center #2: N=2100m, E=-1119m 703 | - Transition: N=1715m, E=-1439m 704 | 705 | - Center #3: N=600m, E=-881m 706 | - Transition: N=100m, E=-881m 707 | 708 | A diagram of the whole route is shown below: 709 | 710 | ![route](Diagrams/fw_challenge.png) 711 | 712 | ### Scenario #13: Flying Car Challenge ### 713 | 714 | The goal of this challenge is to take off in VTOL mode from a starting landing pad, transition to fixed wing mode to fly over the terrain, and transition back to VTOL mode to land on the other landing pad. 715 | Other Landing Pad Location (Relative to starting pad location): 716 | 717 | North: 1545m 718 | East: -1816m 719 | Down: -80m 720 | 721 | To accomplish this task, several other modes of control are available in the UdaciPlane class (you'll need Udacidrone v0.3.4 or later): 722 | 723 | ~~~py 724 | 725 | def cmd_hybrid(self, aileron, elevator, rudder, throttle, roll_moment, pitch_moment, yaw_moment, thrust): 726 | """Command the manual aircraft controls, the VTOL moments and total thrust force 727 | 728 | Args: 729 | aileron: in percentage of maximum aileron (-1:1) 730 | rudder: in percentage of maximum rudder (-1:1) 731 | elevator: in percentage of maximum elevator (-1:1) 732 | throttle: in percentage of maximum throttle RPM (0:1) 733 | roll_moment: in percentage of maximum roll moment (-1:1) 734 | pitch_moment: in percentage of maximum pitch moment (-1:1) 735 | yaw_moment: in percentage of maximum yaw_moment (-1:1) 736 | thrust: in percentage of maximum thrust (0:1) 737 | """ 738 | 739 | def cmd_moment(self, roll_moment, pitch_moment, yaw_moment, thrust): 740 | """Command the VTOL moments and total thrust force 741 | 742 | Args: 743 | roll_moment: in percentage of maximum roll moment (-1:1) 744 | pitch_moment: in percentage of maximum pitch moment (-1:1) 745 | yaw_moment: in percentage of maximum yaw_moment (-1:1) 746 | thrust: in percentage of maximum thrust (0:1) 747 | """ 748 | 749 | def cmd_controls(self, aileron, elevator, rudder, throttle): 750 | """Command the manual aircraft controls 751 | 752 | Args: 753 | aileron: in percentage of maximum aileron (-1:1) 754 | rudder: in percentage of maximum rudder (-1:1) 755 | elevator: in percentage of maximum elevator (-1:1) 756 | throttle: in percentage of maximum throttle RPM (0:1) 757 | """ 758 | 759 | def cmd_vtol_position(self, north, east, altitude, heading): 760 | """Command the local position and drone heading. 761 | 762 | Args: 763 | north: local north in meters 764 | east: local east in meters 765 | altitude: altitude above ground in meters 766 | heading: drone yaw in radians 767 | """ 768 | 769 | def cmd_vtol_attitude(self,roll, pitch, yaw_rate, vert_vel): 770 | """Command the drone through attitude command 771 | 772 | Args: 773 | roll: in radians 774 | pitch: in randians 775 | yaw_rate: in radians/second 776 | vert_vel: upward velocity in meters/second 777 | """ 778 | 779 | ~~~ 780 | 781 | You will need to implement your own VTOL controller. You will also need to integrate the control commands into the FixedWingProject under the FLYINGCAR scenario. See the other scenarios in FixedWingProject for examples of how to integrate it into the project. 782 | 783 | 784 | Notes: 785 | - This scenario will run indefinitely 786 | - There are no parameters to tune, selecting "Tune Parameters" will allow instead allow you to control the aircraft manually (after clicking to the Guided button to disengage Guided mode) 787 | - There are currently not success/failure criterion to allow you to control the aircraft back and forth between the two landing pad locations 788 | - The control structure is completely opened ended for this challenge. 789 | - A simplified energy percentage was added to give a metric to compare against. It is only based on time the VTOL and aircraft throttle is being used. The VTOL controls drain the energy 8x's faster than the aircraft throttle as to incentive time spent in the fixed wing mode. 790 | 791 | Additional Manual Flight Controls: 792 | 793 | When in Position Hold VTOL Mode: 794 | Up/Down or W/S: Command forward velocity 795 | Left/Right or A/D: Command sideways velocity 796 | c/space: Command velocity upwards 797 | Q/E: Command yaw rate 798 | 799 | To transition between VTOL and Fixed Wing press 't'. The aircraft will stabilize to a 0 roll/pitch using the VTOL rotors and run full throttle. At an airspeed of 30 m/s the flying car will transition to Fixed Wing Stabilized Mode. 800 | 801 | In Fixed Wing Stabilized Mode: 802 | Up/Down or W/S: Altitude hold command increase/decrease 803 | Left/Right or A/D: Roll command (max roll = 60 degrees) 804 | c/space: Airspeed command increase decrease 805 | Q/E: Sideslip command 806 | 807 | To transition between Fixed Wing and VTOL press 't'. The aircraft will stabilize to a 0 roll/pitch using the aileron/elevator and zero throttle. At an airspeed of 10 m/s the flying car will transition to Position Hold VTOL Mode. 808 | 809 | ## Evaluation ## 810 | 811 | This project does not require a submission and is not evaluated. 812 | 813 | ### Inconsistent Results ### 814 | 815 | The scenarios/challenges are evaluated within Unity using the true aircraft position. Due to asynchronous communication between the Python controller and Unity, successive runs of the same scenario with the same controller may yield different results depending on the speed of your machine and/or other processes running on it. 816 | 817 | If this is the case here are some tips to help get a consistent result: 818 | 819 | - Don't stress! Implementing controllers for aircraft in the real world yield different results every time they are tested! If you can get a successful result more than 50% of the time, save the log file from one of the successful runs. Most likely the implementation will be a success when evaluated because it will be run on sufficiently fast machines. 820 | - Tune the gains to not only meet the objectives of the scenarios but also exceed them. 821 | - Try being less aggressive on your inner loops (smaller gains) 822 | - Use the simple graphics setting available in the simulation. -------------------------------------------------------------------------------- /fixed_wing_project.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | from udacidrone.messaging import MsgID 4 | from enum import Enum 5 | from udacidrone.connection import MavlinkConnection 6 | import numpy as np 7 | from plane_drone import Udaciplane 8 | from plane_control import LongitudinalAutoPilot 9 | from plane_control import LateralAutoPilot 10 | from plane_control import euler2RM 11 | import time 12 | import sys 13 | 14 | class Scenario(Enum): 15 | SANDBOX = 0 16 | TRIM = 1 17 | ALTITUDE = 2 18 | AIRSPEED = 3 19 | CLIMB = 4 20 | LONGITUDINAL = 5 21 | ROLL = 6 22 | TURN = 7 23 | YAW = 8 24 | LINE = 9 25 | ORBIT = 10 26 | LATERAL = 11 27 | FIXEDWING = 12 28 | FLYINGCAR = 13 29 | 30 | class FixedWingProject(Udaciplane): 31 | 32 | def __init__(self, connection, tlog_name="TLog.txt"): 33 | super().__init__(connection, tlog_name) 34 | 35 | self.longitudinal_autopilot= LongitudinalAutoPilot() 36 | self.lateral_autopilot = LateralAutoPilot() 37 | #defined as [along_track_distance (meters), altitude (meters)] 38 | self.longitudinal_gates = [np.array([200.0, 200.0]), 39 | np.array([1100.0, 300.0]), 40 | np.array([1400.0, 280.0]), 41 | np.array([2200.0, 200.0])] 42 | 43 | self.airspeed_cmd = 41.0 44 | self.altitude_cmd = 450.0 45 | self.throttle_cmd = 0.0 46 | self.elevator_cmd = 0.0 47 | self.pitch_cmd = 0.0 48 | 49 | self.aileron_cmd = 0.0 50 | self.rudder_cmd = 0.0 51 | self.roll_cmd = 0.0 52 | self.sideslip_cmd = 0.0 53 | self.yaw_cmd = 0.0 54 | self.roll_ff = 0.0 55 | self.course_cmd = 0.0 56 | self.line_origin = np.array([0.0, 0.0, 0.0]) 57 | self.orbit_center = np.array([0.0, 0.0, 0.0]) 58 | self.orbit_cw = True 59 | 60 | self.waypoints = [np.array([0.0, 500.0, -400.0]), 61 | np.array([2600.0, 500.0, -500.0]), 62 | np.array([2600.0, -2500.0, -400.0]), 63 | np.array([100.0, 500.0, -450.0]), 64 | np.array([100.0, -2000.0, -450.0])] 65 | 66 | self.waypoint_tuple = (np.array([0.0, 0.0, 0.0]), 67 | np.array([0.0, 0.0, 0.0]), 68 | np.array([0.0, 0.0, 0.0])) 69 | 70 | self.scenario = Scenario.SANDBOX 71 | 72 | self.time_cmd = 0.0 73 | self.cmd_freq = 100.0 74 | 75 | self.last_airspeed_time = None 76 | self.last_position_time = None 77 | self.last_attitude_time = None 78 | 79 | # register all your callbacks here 80 | self.register_callback(MsgID.LOCAL_POSITION, 81 | self.local_position_callback) 82 | self.register_callback(MsgID.LOCAL_VELOCITY, self.airspeed_callback) 83 | self.register_callback(MsgID.STATE, self.state_callback) 84 | self.register_callback(MsgID.ATTITUDE, self.attitude_callback) 85 | #self.register_callback(MsgID.RAW_GYROSCOPE, self.gyro_callback) 86 | #self.register_callback(MsgID.AIRSPEED, self.airspeed_callback) 87 | self._scenario_started = False 88 | 89 | def state_callback(self): 90 | if(self.scenario == Scenario.SANDBOX): 91 | if(self.status != 0): 92 | self.scenario = Scenario(self.status) 93 | self.init_scenario() 94 | 95 | if(self.scenario != Scenario.SANDBOX): 96 | if(self._scenario_started == False): 97 | self.take_control() 98 | self.arm() 99 | self._scenario_started = True 100 | print('Start Scenario...') 101 | elif(self.guided != True): 102 | self.stop() 103 | 104 | def airspeed_callback(self): 105 | #Assuming no wind, for now... 106 | self.airspeed = np.linalg.norm(self.local_velocity) 107 | if(self.airspeed != 0.0): 108 | rot_mat = euler2RM(self.attitude[0], self.attitude[1], 109 | self.attitude[2]) 110 | side_velocity = rot_mat.transpose()[1,0]*self.local_velocity[0] +\ 111 | rot_mat.transpose()[1,1]*self.local_velocity[1] +\ 112 | rot_mat.transpose()[1,2]*self.local_velocity[2] 113 | self.sideslip = np.arcsin(side_velocity/self.airspeed) 114 | else: 115 | self.sideslip = 0.0 116 | dt = 0.0 117 | if(self.last_airspeed_time != None): 118 | dt = self.local_velocity_time - self.last_airspeed_time 119 | if(dt <= 0.0): 120 | return 121 | 122 | self.last_airspeed_time = self.local_velocity_time 123 | 124 | if(self._scenario_started == False): 125 | return 126 | 127 | if(self.scenario == Scenario.AIRSPEED): 128 | self.throttle_cmd = self.longitudinal_autopilot.airspeed_loop( 129 | self.airspeed, self.airspeed_cmd, dt) 130 | self.cmd_longitude_mode(self.elevator_cmd, self.throttle_cmd, 131 | 0,0,self.last_airspeed_time) 132 | 133 | if(self.scenario == Scenario.CLIMB): 134 | self.pitch_cmd = self.longitudinal_autopilot.airspeed_pitch_loop( 135 | self.airspeed, self.airspeed_cmd, dt) 136 | 137 | if((self.scenario == Scenario.LONGITUDINAL) | 138 | (self.scenario == Scenario.FIXEDWING)): 139 | altitude = -self.local_position[2] 140 | [self.pitch_cmd, self.throttle_cmd] = \ 141 | self.longitudinal_autopilot.longitudinal_loop( 142 | self.airspeed, altitude, self.airspeed_cmd, 143 | self.altitude_cmd, dt) 144 | 145 | if((self.scenario == Scenario.ROLL) | 146 | (self.scenario == Scenario.TURN) | 147 | (self.scenario == Scenario.YAW) | 148 | (self.scenario == Scenario.LINE) | 149 | (self.scenario == Scenario.ORBIT) | 150 | (self.scenario == Scenario.LATERAL) | 151 | (self.scenario == Scenario.FIXEDWING)): 152 | self.rudder_cmd = self.lateral_autopilot.sideslip_hold_loop( 153 | self.sideslip, dt) 154 | 155 | if(self.scenario == Scenario.FLYINGCAR): 156 | # TODO: Insert Flying Car Scenario code here 157 | pass 158 | 159 | def attitude_callback(self): 160 | dt = 0.0 161 | if(self.last_attitude_time != None): 162 | dt = self.attitude_time-self.last_attitude_time 163 | self.last_attitude_time = self.attitude_time 164 | 165 | if(self._scenario_started == False): 166 | return 167 | 168 | if((self.scenario == Scenario.ALTITUDE) | 169 | (self.scenario == Scenario.AIRSPEED) | 170 | (self.scenario == Scenario.CLIMB) | 171 | (self.scenario == Scenario.LONGITUDINAL)): 172 | self.elevator_cmd = self.longitudinal_autopilot.pitch_loop( 173 | self.attitude[1], self.gyro_raw[1], self.pitch_cmd) 174 | self.cmd_longitude_mode(self.elevator_cmd, self.throttle_cmd) 175 | if(np.abs(self.gyro_raw[1]) >= 1): 176 | print(self.gyro_raw) 177 | 178 | if((self.scenario == Scenario.YAW) | 179 | (self.scenario == Scenario.LINE) | 180 | (self.scenario == Scenario.ORBIT) | 181 | (self.scenario == Scenario.LATERAL) | 182 | (self.scenario == Scenario.FIXEDWING)): 183 | self.roll_cmd = self.lateral_autopilot.yaw_hold_loop( 184 | self.yaw_cmd, self.attitude[2], dt, self.roll_ff) 185 | 186 | 187 | if((self.scenario == Scenario.ROLL) | 188 | (self.scenario == Scenario.TURN) | 189 | (self.scenario == Scenario.YAW) | 190 | (self.scenario == Scenario.LINE) | 191 | (self.scenario == Scenario.ORBIT) | 192 | (self.scenario == Scenario.LATERAL)): 193 | self.aileron_cmd = self.lateral_autopilot.roll_attitude_hold_loop( 194 | self.roll_cmd, self.attitude[0], self.gyro_raw[0]) 195 | self.cmd_lateral_mode(self.aileron_cmd, self.rudder_cmd, 196 | self.altitude_cmd, self.airspeed_cmd) 197 | 198 | if(self.scenario == Scenario.FIXEDWING): 199 | self.aileron_cmd = self.lateral_autopilot.roll_attitude_hold_loop( 200 | self.roll_cmd, self.attitude[0], self.gyro_raw[0]) 201 | self.elevator_cmd = self.longitudinal_autopilot.pitch_loop( 202 | self.attitude[1], self.gyro_raw[1], self.pitch_cmd) 203 | self.cmd_controls(self.aileron_cmd, self.elevator_cmd, self.rudder_cmd, self.throttle_cmd) 204 | 205 | if(self.scenario == Scenario.FLYINGCAR): 206 | # TODO: Insert Flying Car Scenario code here 207 | pass 208 | 209 | def local_position_callback(self): 210 | dt = 0.0 211 | if(self.last_position_time != None): 212 | dt = self.local_position_time - self.last_position_time 213 | 214 | self.last_position_time = self.local_position_time 215 | if(dt <= 0.0): 216 | return 217 | 218 | if(self._scenario_started == False): 219 | return 220 | 221 | if(self.scenario == Scenario.ALTITUDE): 222 | altitude = -self.local_position[2] 223 | self.pitch_cmd = self.longitudinal_autopilot.altitude_loop( 224 | altitude,self.altitude_cmd, dt) 225 | if((self.scenario == Scenario.LONGITUDINAL)): 226 | along_track = np.linalg.norm(self.local_position[0:2]) 227 | if(along_track > self.gate_target[0]): 228 | if(len(self.longitudinal_gates)==0): 229 | self.stop() 230 | else: 231 | self.gate_target = self.longitudinal_gates.pop(0) 232 | print('Gate Target: ', self.gate_target) 233 | self.altitude_cmd = self.gate_target[1] 234 | 235 | if(self.scenario == Scenario.LINE): 236 | self.yaw_cmd = self.lateral_autopilot.straight_line_guidance( 237 | self.line_origin, self.line_course, self.local_position) 238 | self.roll_ff = 0.0 239 | 240 | if(self.scenario == Scenario.ORBIT): 241 | self.yaw_cmd = self.lateral_autopilot.orbit_guidance( 242 | self.orbit_center, self.orbit_radius, self.local_position, 243 | self.attitude[2], self.orbit_cw) 244 | self.roll_ff = self.lateral_autopilot.coordinated_turn_ff( 245 | self.airspeed_cmd, self.orbit_radius, self.orbit_cw) 246 | 247 | if(self.scenario == Scenario.LATERAL): 248 | (self.roll_ff, self.yaw_cmd) = self.lateral_autopilot.path_manager( 249 | self.local_position, self.attitude[2], self.airspeed_cmd) 250 | 251 | 252 | if(self.scenario == Scenario.FIXEDWING): 253 | (self.roll_ff, self.yaw_cmd, switch) = self.lateral_autopilot.waypoint_follower( 254 | self.waypoint_tuple, self.local_position[0:2], self.attitude[2], self.airspeed_cmd) 255 | if(switch): 256 | if(len(self.waypoints)==0): 257 | next_waypoint = self.waypoint_tuple[2] 258 | else: 259 | next_waypoint = self.waypoints.pop(0) 260 | print('Adding waypoint: ', next_waypoint) 261 | self.waypoint_tuple = (self.waypoint_tuple[1], self.waypoint_tuple[2], next_waypoint) 262 | self.altitude_cmd = -self.waypoint_tuple[0][2] 263 | 264 | if(self.scenario == Scenario.FLYINGCAR): 265 | # TODO: Insert Flying Car Scenario code here 266 | pass 267 | 268 | def init_scenario(self): 269 | if(self.scenario == Scenario.SANDBOX): 270 | pass 271 | elif(self.scenario == Scenario.ALTITUDE): 272 | print('Starting Altitude Hold Scenario') 273 | self.throttle_cmd = 0.66 274 | self.altitude_cmd = 450.0 275 | elif(self.scenario == Scenario.AIRSPEED): 276 | print('Starting Airspeed Hold Scenario') 277 | self.elevator_cmd = 0.0 278 | self.airspeed_cmd = 41.0 279 | elif(self.scenario == Scenario.CLIMB): 280 | print('Starting Climb Scenario') 281 | self.airspeed_cmd = 41.0 282 | self.throttle_cmd = 1.0 283 | elif(self.scenario == Scenario.LONGITUDINAL): 284 | print('Starting Longitudinal Challenge') 285 | self.airspeed_cmd = 41.0 286 | self.gate_target = self.longitudinal_gates.pop(0) 287 | self.altitude_cmd = self.gate_target[1] 288 | elif(self.scenario == Scenario.ROLL): 289 | print('Starting Stabilize Roll Scenario') 290 | self.airspeed_cmd = 41.0 291 | self.altitude_cmd = 450.0 292 | self.roll_cmd = 0.0 293 | self.rudder_cmd = 0.0 294 | elif(self.scenario == Scenario.TURN): 295 | print('Starting Coordinated Turn Scenario') 296 | self.airpseed_cmd = 41.0 297 | self.altitude_cmd = 450.0 298 | self.roll_cmd = 45.0*np.pi/180.0 299 | self.sideslip_cmd = 0.0 300 | elif(self.scenario == Scenario.YAW): 301 | print('Starting Yaw Hold Scenario') 302 | self.airspeed_cmd = 41.0 303 | self.altitude_cmd = 450.0 304 | self.yaw_cmd = 0.0; 305 | self.sideslip_cmd = 0.0 306 | self.roll_ff = 0.0 307 | elif(self.scenario == Scenario.LINE): 308 | print('Starting Line Following Scenario') 309 | self.airspeed_cmd = 41.0 310 | self.altitude_cmd = 450.0 311 | self.line_course = 0.0 312 | self.line_origin = np.array([0.0, 20.0, 450.0]) 313 | elif(self.scenario == Scenario.ORBIT): 314 | print('Starting Orbit Following Scenario') 315 | self.airspeed_cmd = 41.0 316 | self.altitude_cmd = 450.0 317 | self.orbit_radius = 500.0 318 | self.orbit_center = np.array([0.0, 500.0, -450.0]) 319 | self.orbit_cw = True 320 | elif(self.scenario == Scenario.LATERAL): 321 | print('Starting Lateral Challenge') 322 | self.airspeed_cmd = 41.0 323 | self.altitude_cmd = 450.0 324 | elif(self.scenario == Scenario.FIXEDWING): 325 | print('Starting Fixed Wing Challenge') 326 | self.airspeed_cmd = 41.0 327 | prev_waypoint = self.waypoints.pop(0) 328 | curr_waypoint = self.waypoints.pop(0) 329 | next_waypoint = self.waypoints.pop(0) 330 | self.waypoint_tuple = (prev_waypoint, curr_waypoint, next_waypoint) 331 | self.altitude_cmd = -self.waypoint_tuple[0][2] 332 | elif(self.scenario == Scenario.FLYINGCAR): 333 | # TODO: Insert Flying Car Scenario code here 334 | pass 335 | else: 336 | print('Invalid Scenario') 337 | return 338 | 339 | def run_scenario(self,scenario): 340 | self.scenario = scenario 341 | 342 | self.init_scenario() 343 | 344 | self.start() 345 | 346 | 347 | if __name__ == "__main__": 348 | conn = MavlinkConnection('tcp:127.0.0.1:5760', threaded=False, PX4=False) 349 | 350 | #conn = WebSocketConnection('ws://127.0.0.1:5760') 351 | drone = FixedWingProject(conn) 352 | time.sleep(2) 353 | 354 | if(len(sys.argv)>1): 355 | try: 356 | scenario = float(sys.argv[1][1:(len(sys.argv[1])+1)]) 357 | #drone.run_scenario(Scenario(scenario)) 358 | except: 359 | print('Scenario argument must be a number') 360 | else: 361 | scenario = 0 362 | 363 | drone.run_scenario(Scenario(scenario)) 364 | -------------------------------------------------------------------------------- /gains.txt: -------------------------------------------------------------------------------- 1 | Kp_speed:0 2 | Ki_speed:0 3 | Kp_pitch:0 4 | Kp_q:0 5 | Kp_alt:0 6 | Ki_alt:0 7 | Kp_speed2:0 8 | Ki_speed2:0 9 | altitudeSwitch:0 10 | Kp_roll:0 11 | Kp_p:0 12 | Kp_yaw:0 13 | Ki_yaw:0 14 | Kp_sideslip:0 15 | Ki_sideslip:0 16 | Kp_xTrack:0 17 | Kp_orbit:0 -------------------------------------------------------------------------------- /plane_control.py: -------------------------------------------------------------------------------- 1 | 2 | # -*- coding: utf-8 -*- 3 | import numpy as np 4 | PI = 3.14159 5 | class LongitudinalAutoPilot(object): 6 | def __init__(self): 7 | self.max_throttle_rpm = 2500 8 | self.max_elevator = 30.0*PI/180.0 9 | 10 | self.min_throttle = 0.0 11 | self.max_throttle = 1.0 12 | self.max_pitch_cmd = 30.0*np.pi/180.0 13 | self.max_pitch_cmd2 = 45.0*np.pi/180.0 14 | 15 | self.speed_int = 0.0 16 | self.alt_int = 0.0 17 | self.climb_speed_int = 0.0 18 | 19 | 20 | 21 | return 22 | 23 | 24 | 25 | """Used to calculate the elevator command required to acheive the target 26 | pitch 27 | 28 | Args: 29 | pitch: in radians 30 | pitch_rate: in radians/sec 31 | pitch_cmd: in radians 32 | 33 | Returns: 34 | elevator_cmd: in percentage elevator [-1,1] 35 | """ 36 | def pitch_loop(self, pitch, pitch_rate, pitch_cmd): 37 | elevator_cmd = 0.0 38 | # STUDENT CODE HERE 39 | 40 | 41 | return elevator_cmd 42 | 43 | """Used to calculate the pitch command required to maintain the commanded 44 | altitude 45 | 46 | Args: 47 | altitude: in meters (positive up) 48 | altitude_cmd: in meters (positive up) 49 | dt: timestep in seconds 50 | 51 | Returns: 52 | pitch_cmd: in radians 53 | """ 54 | def altitude_loop(self, altitude, altitude_cmd, dt): 55 | pitch_cmd = 0.0 56 | # STUDENT CODE HERE 57 | 58 | 59 | 60 | return pitch_cmd 61 | 62 | 63 | """Used to calculate the throttle command required command the target 64 | airspeed 65 | 66 | Args: 67 | airspeed: in meters/sec 68 | airspeed_cmd: in meters/sec 69 | dt: timestep in seconds 70 | 71 | Returns: 72 | throttle_command: in percent throttle [0,1] 73 | """ 74 | def airspeed_loop(self, airspeed, airspeed_cmd, dt): 75 | throttle_cmd = 0.0 76 | # STUDENT CODE HERE 77 | 78 | 79 | return throttle_cmd 80 | """Used to calculate the pitch command required to maintain the commanded 81 | airspeed 82 | 83 | Args: 84 | airspeed: in meters/sec 85 | airspeed_cmd: in meters/sec 86 | dt: timestep in seconds 87 | 88 | Returns: 89 | pitch_cmd: in radians 90 | """ 91 | def airspeed_pitch_loop(self, airspeed, airspeed_cmd, dt): 92 | pitch_cmd = 0.0 93 | # STUDENT CODE HERE 94 | 95 | 96 | return pitch_cmd 97 | 98 | """Used to calculate the pitch command and throttle command based on the 99 | aicraft altitude error 100 | 101 | Args: 102 | airspeed: in meter/sec 103 | altitude: in meters (positive up) 104 | airspeed_cmd: in meters/sec 105 | altitude_cmd: in meters/sec (positive up) 106 | dt: timestep in seconds 107 | 108 | Returns: 109 | pitch_cmd: in radians 110 | throttle_cmd: in in percent throttle [0,1] 111 | """ 112 | def longitudinal_loop(self, airspeed, altitude, airspeed_cmd, altitude_cmd, 113 | dt): 114 | pitch_cmd = 0.0 115 | throttle_cmd = 0.0 116 | # STUDENT CODE HERE 117 | 118 | 119 | return[pitch_cmd, throttle_cmd] 120 | 121 | 122 | 123 | class LateralAutoPilot: 124 | 125 | def __init__(self): 126 | self.g = 9.81 127 | self.integrator_yaw = 0.0 128 | self.integrator_beta = 0.0 129 | self.gate = 1 130 | self.max_roll = 60*np.pi/180.0 131 | self.state = 1 132 | 133 | 134 | 135 | """Used to calculate the commanded aileron based on the roll error 136 | 137 | Args: 138 | phi_cmd: commanded roll in radians 139 | phi: roll angle in radians 140 | roll_rate: in radians/sec 141 | T_s: timestep in sec 142 | 143 | Returns: 144 | aileron: in percent full aileron [-1,1] 145 | """ 146 | def roll_attitude_hold_loop(self, 147 | phi_cmd, # commanded roll 148 | phi, # actual roll 149 | roll_rate, 150 | T_s = 0.0): 151 | aileron = 0 152 | # STUDENT CODE HERE 153 | 154 | 155 | 156 | return aileron 157 | 158 | """Used to calculate the commanded roll angle from the course/yaw angle 159 | 160 | Args: 161 | yaw_cmd: commanded yaw in radians 162 | yaw: roll angle in radians 163 | roll_rate: in radians/sec 164 | T_s: timestep in sec 165 | 166 | Returns: 167 | roll_cmd: commanded roll in radians 168 | """ 169 | def yaw_hold_loop(self, 170 | yaw_cmd, # desired heading 171 | yaw, # actual heading 172 | T_s, 173 | roll_ff=0): 174 | roll_cmd = 0 175 | 176 | # STUDENT CODE HERE 177 | 178 | 179 | return roll_cmd 180 | 181 | 182 | """Used to calculate the commanded rudder based on the sideslip 183 | 184 | Args: 185 | beta: sideslip angle in radians 186 | T_s: timestep in sec 187 | 188 | Returns: 189 | rudder: in percent full rudder [-1,1] 190 | """ 191 | def sideslip_hold_loop(self, 192 | beta, # sideslip angle 193 | T_s): 194 | rudder = 0 195 | # STUDENT CODE HERE 196 | 197 | 198 | return rudder 199 | 200 | """Used to calculate the desired course angle based on cross-track error 201 | from a desired line 202 | 203 | Args: 204 | line_origin: point on the desired line in meters [N, E, D] 205 | line_course: heading of the line in radians 206 | local_position: vehicle position in meters [N, E, D] 207 | 208 | Returns: 209 | course_cmd: course/yaw cmd for the vehicle in radians 210 | """ 211 | def straight_line_guidance(self, line_origin, line_course, 212 | local_position): 213 | course_cmd = 0 214 | # STUDENT CODE HERE 215 | 216 | 217 | return course_cmd 218 | 219 | """Used to calculate the desired course angle based on radius error from 220 | a specified orbit center 221 | 222 | Args: 223 | orbit_center: in meters [N, E, D] 224 | orbit_radius: desired radius in meters 225 | local_position: vehicle position in meters [N, E, D] 226 | yaw: vehicle heading in radians 227 | clockwise: specifies whether to fly clockwise (increasing yaw) 228 | 229 | Returns: 230 | course_cmd: course/yaw cmd for the vehicle in radians 231 | """ 232 | def orbit_guidance(self, orbit_center, orbit_radius, local_position, yaw, 233 | clockwise = True): 234 | course_cmd = 0 235 | # STUDENT CODE HERE 236 | 237 | 238 | return course_cmd 239 | 240 | """Used to calculate the feedforward roll angle for a constant radius 241 | coordinated turn 242 | 243 | Args: 244 | speed: the aircraft speed during the turn in meters/sec 245 | radius: turning radius in meters 246 | cw: true=clockwise turn, false = counter-clockwise turn 247 | 248 | Returns: 249 | roll_ff: feed-forward roll in radians 250 | """ 251 | def coordinated_turn_ff(self, speed, radius, cw): 252 | 253 | roll_ff = 0 254 | # STUDENT CODE HERE 255 | 256 | 257 | return roll_ff 258 | 259 | """Used to calculate the desired course angle and feed-forward roll 260 | depending on which phase of lateral flight (orbit or line following) the 261 | aicraft is in 262 | 263 | Args: 264 | local_position: vehicle position in meters [N, E, D] 265 | yaw: vehicle heading in radians 266 | airspeed_cmd: in meters/sec 267 | 268 | Returns: 269 | roll_ff: feed-forward roll in radians 270 | yaw_cmd: commanded yaw/course in radians 271 | """ 272 | def path_manager(self, local_position, yaw, airspeed_cmd): 273 | 274 | roll_ff = 0 275 | yaw_cmd = 0 276 | # STUDENT CODE HERE 277 | 278 | 279 | return(roll_ff,yaw_cmd) 280 | 281 | 282 | """Used to calculate the desired course angle and feed-forward roll 283 | depending on which phase of lateral flight (orbit or line following) the 284 | aicraft is in 285 | 286 | Args: 287 | waypoint_tuple: 3 waypoints, (prev_waypoint, curr_waypoint, next_waypoint), waypoints are in meters [N, E, D] 288 | local_position: vehicle position in meters [N, E, D] 289 | yaw: vehicle heading in radians 290 | airspeed_cmd: in meters/sec 291 | 292 | Returns: 293 | roll_ff: feed-forward roll in radians 294 | yaw_cmd: commanded yaw/course in radians 295 | cycle: True=cycle waypoints (at the end of orbit segment) 296 | """ 297 | def waypoint_follower(self, waypoint_tuple, local_position, yaw, airspeed_cmd): 298 | roll_ff = 0.0 299 | yaw_cmd = 0.0 300 | cycle = False 301 | 302 | # STUDENT CODE HERE 303 | 304 | 305 | 306 | return(roll_ff, yaw_cmd, cycle) 307 | 308 | 309 | 310 | def euler2RM(roll,pitch,yaw): 311 | R = np.array([[0.0,0.0,0.0],[0.0,0.0,0.0],[0.0,0.0,0.0]]) 312 | cr = np.cos(roll) 313 | sr = np.sin(roll) 314 | 315 | cp = np.cos(pitch) 316 | sp = np.sin(pitch) 317 | 318 | cy = np.cos(yaw) 319 | sy = np.sin(yaw) 320 | 321 | R[0,0] = cp*cy 322 | R[1,0] = -cr*sy+sr*sp*cy 323 | R[2,0] = sr*sy+cr*sp*cy 324 | 325 | R[0,1] = cp*sy 326 | R[1,1] = cr*cy+sr*sp*sy 327 | R[2,1] = -sr*cy+cr*sp*sy 328 | 329 | R[0,2] = -sp 330 | R[1,2] = sr*cp 331 | R[2,2] = cr*cp 332 | 333 | return R.transpose() 334 | -------------------------------------------------------------------------------- /plane_drone.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from enum import Enum 3 | 4 | from udacidrone import Drone 5 | import time 6 | visdom_available= True 7 | try: 8 | import visdom 9 | except: 10 | visdom_available = False 11 | 12 | class PlaneMode(Enum): 13 | """ 14 | Constant which isn't defined in Mavlink but useful when dealing with 15 | the airplane simulation 16 | """ 17 | SUB_MODE_MANUAL = 1 18 | SUB_MODE_LONGITUDE = 2 19 | SUB_MODE_LATERAL = 3 20 | SUB_MODE_STABILIZED = 4 21 | SUB_MODE_VTOL_ATTITUDE = 9 22 | SUB_MODE_VTOL_POSITION = 10 23 | 24 | class Udaciplane(Drone): 25 | """ 26 | Udaciplane class for use with the Unity Fixed Wing/Flying Car simulation 27 | """ 28 | 29 | def __init__(self, connection, tlog_name="TLog.txt"): 30 | 31 | super().__init__(connection, tlog_name) 32 | 33 | 34 | def cmd_stabilized(self, roll, altitude, sideslip, airspeed): 35 | """Command the stabilized mode of the drone 36 | 37 | Args: 38 | roll: in radians 39 | altitude: in meters (positive up) 40 | sideslip: in radians (positive nose left) 41 | airspeed: in meters/sec 42 | 43 | """ 44 | self.connection.set_sub_mode(PlaneMode.SUB_MODE_STABILIZED.value) 45 | self.connection.cmd_moment(roll, altitude, sideslip, airspeed) 46 | 47 | def cmd_longitude_mode(self, elevator, throttle, roll = 0, sideslip = 0, 48 | t=0): 49 | """Command the longitude mode while lateral is stabilized 50 | 51 | Args: 52 | elevator: in percentage of maximum elevator (-1:1) 53 | throttle: in percentage of maximum throttle RPM (0:1) 54 | roll: in radians 55 | sideslip: in radians (positive nose left) 56 | """ 57 | self.connection.set_sub_mode(PlaneMode.SUB_MODE_LONGITUDE.value) 58 | self.connection.cmd_moment(roll, elevator, sideslip, throttle, t) 59 | 60 | 61 | def cmd_lateral_mode(self, aileron, rudder, altitude, airspeed): 62 | """Command the lateral mode while longitudinal mode is stabilized 63 | 64 | Args: 65 | aileron: in percentage of maximum aileron (-1:1) 66 | rudder: in percentage of maximum rudder (-1:1) 67 | altitude: in meters (positive up) 68 | airspeed: in meters/sec 69 | """ 70 | self.connection.set_sub_mode(PlaneMode.SUB_MODE_LATERAL.value) 71 | self.connection.cmd_moment(aileron, altitude, rudder, airspeed) 72 | 73 | 74 | def cmd_controls(self, aileron, elevator, rudder, throttle): 75 | """Command the manual aircraft controls 76 | 77 | Args: 78 | aileron: in percentage of maximum aileron (-1:1) 79 | rudder: in percentage of maximum rudder (-1:1) 80 | elevator: in percentage of maximum elevator (-1:1) 81 | throttle: in percentage of maximum throttle RPM (0:1) 82 | """ 83 | self.connection.set_sub_mode(PlaneMode.SUB_MODE_MANUAL.value) 84 | controls = [aileron, elevator, rudder, throttle] 85 | self.connection.cmd_controls(controls) 86 | 87 | def cmd_hybrid(self, aileron, elevator, rudder, throttle, roll_moment, pitch_moment, yaw_moment, thrust): 88 | """Command the manual aircraft controls, the VTOL moments and total thrust force 89 | 90 | Args: 91 | aileron: in percentage of maximum aileron (-1:1) 92 | rudder: in percentage of maximum rudder (-1:1) 93 | elevator: in percentage of maximum elevator (-1:1) 94 | throttle: in percentage of maximum throttle RPM (0:1) 95 | roll_moment: in percentage of maximum roll moment (-1:1) 96 | pitch_moment: in percentage of maximum pitch moment (-1:1) 97 | yaw_moment: in percentage of maximum yaw_moment (-1:1) 98 | thrust: in percentage of maximum thrust (0:1) 99 | """ 100 | self.connection.set_sub_mode(PlaneMode.SUB_MODE_MANUAL.value) 101 | controls = [aileron, elevator, rudder, throttle, roll_moment, pitch_moment, yaw_moment , thrust] 102 | self.connection.cmd_controls(controls) 103 | 104 | def cmd_moment(self, roll_moment, pitch_moment, yaw_moment, thrust): 105 | """Command the VTOL moments and total thrust force 106 | 107 | Args: 108 | roll_moment: in percentage of maximum roll moment (-1:1) 109 | pitch_moment: in percentage of maximum pitch moment (-1:1) 110 | yaw_moment: in percentage of maximum yaw_moment (-1:1) 111 | thrust: in percentage of maximum thrust (0:1) 112 | """ 113 | self.connection.set_sub_mode(PlaneMode.SUB_MODE_MANUAL.value) 114 | controls = [0.0, 0.0, 0.0, 0.0, roll_moment, pitch_moment, yaw_moment, thrust] 115 | self.connection.cmd_controls(controls) 116 | 117 | def cmd_vtol_position(self, north, east, altitude, heading): 118 | """Command the local position and drone heading. 119 | 120 | Args: 121 | north: local north in meters 122 | east: local east in meters 123 | altitude: altitude above ground in meters 124 | heading: drone yaw in radians 125 | """ 126 | self.connection.set_sub_mode(PlaneMode.SUB_MODE_VTOL_POSITION.value) 127 | self.cmd_position(north, east, altitude, heading) 128 | 129 | def cmd_vtol_attitude(self,roll, pitch, yaw_rate, vert_vel): 130 | """Command the drone through attitude command 131 | 132 | Args: 133 | roll: in radians 134 | pitch: in randians 135 | yaw_rate: in radians/second 136 | vert_vel: upward velocity in meters/second 137 | """ 138 | self.connection.set_sub_mode(PlaneMode.SUB_MODE_VTOL_ATTITUDE.value) 139 | self.cmd_attitude(roll, pitch, yaw_rate, vert_vel) 140 | 141 | --------------------------------------------------------------------------------