Add support for geofence return point. Also fixed a bunch of bugs in geofence and rally point support.