Implementation of the regulation algorithm
TODO : check for TODOs in code ahahah
TODO : get navdata : pitch