AC_Circle: Restore radius every time

libraries/AC_WPNav/AC_Circle.cpp

@@ -13,7 +13,7 @@ const AP_Param::GroupInfo AC_Circle::var_info[] = {
     // @Range: 0 200000
     // @Increment: 100
     // @User: Standard
-    AP_GROUPINFO("RADIUS",  0,  AC_Circle, _radius, AC_CIRCLE_RADIUS_DEFAULT),
+    AP_GROUPINFO("RADIUS",  0,  AC_Circle, _radius_parm, AC_CIRCLE_RADIUS_DEFAULT),
 
     // @Param: RATE
     // @DisplayName: Circle rate
@@ -62,7 +62,6 @@ void AC_Circle::init(const Vector3f& center, bool terrain_alt)
 {
     _center = center;
     _terrain_alt = terrain_alt;
-
     // initialise position controller (sets target roll angle, pitch angle and I terms based on vehicle current lean angles)
     _pos_control.set_desired_accel_xy(0.0f,0.0f);
     _pos_control.set_desired_velocity_xy(0.0f,0.0f);
@@ -82,7 +81,9 @@ void AC_Circle::init(const Vector3f& center, bool terrain_alt)
 /// init - initialise circle controller setting center using stopping point and projecting out based on the copter's heading
 ///     caller should set the position controller's x,y and z speeds and accelerations before calling this
 void AC_Circle::init()
-{
+{   
+    //initialize radius from params
+    _radius =_radius_parm;
     // initialise position controller (sets target roll angle, pitch angle and I terms based on vehicle current lean angles)
     _pos_control.set_desired_accel_xy(0.0f,0.0f);
     _pos_control.set_desired_velocity_xy(0.0f,0.0f);

libraries/AC_WPNav/AC_Circle.h

@@ -42,7 +42,7 @@ public:
     bool center_is_terrain_alt() const { return _terrain_alt; }
 
     /// get_radius - returns radius of circle in cm
-    float get_radius() const { return _radius; }
+    float get_radius() const { return is_positive(_radius)?_radius:_radius_parm; }
 
     /// set_radius - sets circle radius in cm
     void set_radius(float radius_cm);
@@ -135,12 +135,13 @@ private:
     };
 
     // parameters
-    AP_Float    _radius;        // maximum horizontal speed in cm/s during missions
+    AP_Float    _radius_parm;   // radius of circle in cm loaded from params
     AP_Float    _rate;          // rotation speed in deg/sec
     AP_Int16    _options;       // stick control enable/disable
 
     // internal variables
     Vector3f    _center;        // center of circle in cm from home
+    float       _radius;        // radius of circle in cm
     float       _yaw;           // yaw heading (normally towards circle center)
     float       _angle;         // current angular position around circle in radians (0=directly north of the center of the circle)
     float       _angle_total;   // total angle traveled in radians