If you want a "toy" example of this type of code, look at flight control software for drones such as Betaflight. You can modify this code and test it in real life. I did this, as I contributed the GPS Rescue feature. I have a blooper reel of failures during testing.

https://github.com/betaflight/betaflight/