Fixes to the wall following demo

[knmcguire]

See for tips #390.

Also this need to be implemented on the crazyflie firmware side as well: https://github.com/bitcraze/crazyflie-firmware/tree/master/examples/demos/app_wall_following_demo