Simple Safety Geofence

This implements a simple geofence module that provides a safety radius. When you exceed the warning
radius an alarm is set. When you exceed the error radius your motors will shut down. This module prioritizes
not going outside the boundary of the geofence over the model safety.

In the future when navigation is 100% robust we can enable RTH or something when hitting the warning
boundary but I would like to keep the scope of this to the simple goal of not flying away ever.