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). |