Adding Navi and Safety

unfinished navigation and safey work
Committing to Sync

Please ignore these updates...