Fix RTH on FW

Previously it tried to fly directly to the home / hold position which resulted in a mess:

![Screen Shot 2013-03-10 at 3 34 00 PM](https://f.cloud.github.com/assets/618346/241718/3ed8c3be-89cb-11e2-938e-c96cc0472205.png)

Now it works more sensibly:

![Screen Shot 2013-03-10 at 4 42 04 PM](https://f.cloud.github.com/assets/618346/241722/5e29db36-89cb-11e2-90d1-e5442def072e.png)

This also changes the parameterization of circle position mode to use the ModeParameter as the radius.