Testing with Inertial navigation
Change configurator display parameters
[unremarkable]
Change default sonar_pid value
Trim the main code