Added sonar data writing