Added tests for the sonar drivers, they fail, however, due to issues with the simulated HAL.