| 1. A. Include needed libraries; |
| 2. B. Define PID coefficients values for roll, yaw & Pitch; |
| 3. C. Declaration of variables: |
| 4. + channels; |
| 5. + counters; |
| 6. + control Ui; |
| 7. + battery level; |
| 8. + calibration of gyro reading; |
| 9. + PID intermediate variables, ... |
| 10. D. Setup of the program: |
| 11. + define microcontroller as I2C master for communication; |
| 12. + define inputs & outputs with readiness led signs; |
| 13. + communicate w/GYRO and calibrate readings with a function. (during calibration, silent the ESC by zero Ui); |
| 14. + enable “PCICR” & define masks register for pins; |
| 15. + keep zero Ui command while waiting; |
| 16. + define state of launch. |
| 17. E. the loop: |
| 18. + call FCT of gyro reading; |
| 19. + filter the noise of reading and transfer to deg/sec; |
| 20. + define SATRT/STOP sequence by joystick, (using moor machine); |
| 21. + reset all PID for new cycle for roll yaw pitch; |
| 22. + recalculate the setpoint from channels; |
| 23. + calculate the PID FCT; |
| 24. + define led warning and state-dependent; |
| 25. + limit max throttle (Ui); |
| 26. + calculate signals of ESCs; |
| 27. + compensate the control by battery discharge ranging of ESCs signal [min-max]; |
| 28. + keep motors always with zero Ui in case of waiting for the state; |
| 29. + wait to complete 4 ms, (250 hz). |
