Modified commands to be ignored if already accomplished, modified ros failsafe, added altitude waypoints, added autonomous safety switch control