Copter: add panorama to CIRCLE mode

Yaw will slowly rotate if CIRCLE_RADIUS is set to zero
Circle center is projected forward CIRCLE_RADIUS from current position
and heading

ArduCopter/ArduCopter.pde

@@ -1442,6 +1442,15 @@ bool set_yaw_mode(uint8_t new_yaw_mode)
                 yaw_initialised = true;
             }
             break;
+        case YAW_CIRCLE:
+            if( ap.home_is_set ) {
+                // set yaw to point to center of circle
+                yaw_look_at_WP = circle_center;
+                // initialise bearing to current heading
+                yaw_look_at_WP_bearing = ahrs.yaw_sensor;
+                yaw_initialised = true;
+            }
+            break;
         case YAW_LOOK_AT_HEADING:
             yaw_initialised = true;
             break;
@@ -1502,9 +1511,18 @@ void update_yaw_mode(void)
         break;
 
     case YAW_LOOK_AT_LOCATION:
-        // point towards a location held in yaw_look_at_WP (no pilot input accepted)
+        // point towards a location held in yaw_look_at_WP
-        nav_yaw = get_yaw_slew(nav_yaw, yaw_look_at_WP_bearing, AUTO_YAW_SLEW_RATE);
+        get_look_at_yaw();
-        get_stabilize_yaw(nav_yaw);
+
+        // if there is any pilot input, switch to YAW_HOLD mode for the next iteration
+        if( g.rc_4.control_in != 0 ) {
+            set_yaw_mode(YAW_HOLD);
+        }
+        break;
+
+    case YAW_CIRCLE:
+        // points toward the center of the circle or does a panorama
+        get_circle_yaw();
 
         // if there is any pilot input, switch to YAW_HOLD mode for the next iteration
         if( g.rc_4.control_in != 0 ) {

ArduCopter/Attitude.pde

@@ -682,6 +682,47 @@ get_of_pitch(int32_t input_pitch)
  * yaw controllers
  *************************************************************/
 
+ // get_look_at_yaw - updates bearing to look at center of circle or do a panorama
+// should be called at 100hz
+static void get_circle_yaw()
+{
+    static uint8_t look_at_yaw_counter = 0;     // used to reduce update rate to 10hz
+
+    // if circle radius is zero do panorama
+    if( g.circle_radius == 0 ) {
+        // slew yaw towards circle angle
+        nav_yaw = get_yaw_slew(nav_yaw, ToDeg(circle_angle)*100, AUTO_YAW_SLEW_RATE);
+    }else{
+        look_at_yaw_counter++;
+        if( look_at_yaw_counter >= 10 ) {
+            look_at_yaw_counter = 0;
+            yaw_look_at_WP_bearing = pv_get_bearing_cd(inertial_nav.get_position(), yaw_look_at_WP);
+        }
+        // slew yaw
+        nav_yaw = get_yaw_slew(nav_yaw, yaw_look_at_WP_bearing, AUTO_YAW_SLEW_RATE);
+    }
+
+    // call stabilize yaw controller
+    get_stabilize_yaw(nav_yaw);
+}
+
+// get_look_at_yaw - updates bearing to location held in look_at_yaw_WP and calls stabilize yaw controller
+// should be called at 100hz
+static void get_look_at_yaw()
+{
+    static uint8_t look_at_yaw_counter = 0;     // used to reduce update rate to 10hz
+
+    look_at_yaw_counter++;
+    if( look_at_yaw_counter >= 10 ) {
+        look_at_yaw_counter = 0;
+        yaw_look_at_WP_bearing = pv_get_bearing_cd(inertial_nav.get_position(), yaw_look_at_WP);
+    }
+
+    // slew yaw and call stabilize controller
+    nav_yaw = get_yaw_slew(nav_yaw, yaw_look_at_WP_bearing, AUTO_YAW_SLEW_RATE);
+    get_stabilize_yaw(nav_yaw);
+}
+
 static void get_look_ahead_yaw(int16_t pilot_yaw)
 {
     // Commanded Yaw to automatically look ahead.

ArduCopter/commands_logic.pde

@@ -372,7 +372,6 @@ static void do_circle()
     }
 
     // set yaw to point to center of circle
-    yaw_look_at_WP = circle_center;
     set_yaw_mode(CIRCLE_YAW);
 
     // set angle travelled so far to zero

ArduCopter/config.h

@@ -593,7 +593,7 @@
 
 // CIRCLE Mode
 #ifndef CIRCLE_YAW
- # define CIRCLE_YAW             	YAW_LOOK_AT_LOCATION
+ # define CIRCLE_YAW             	YAW_CIRCLE
 #endif
 
 #ifndef CIRCLE_RP

ArduCopter/defines.h

@@ -19,10 +19,11 @@
 #define YAW_ACRO                        1       // pilot controlled yaw using rate controller
 #define YAW_LOOK_AT_NEXT_WP             2       // point towards next waypoint (no pilot input accepted)
 #define YAW_LOOK_AT_LOCATION            3       // point towards a location held in yaw_look_at_WP (no pilot input accepted)
-#define YAW_LOOK_AT_HOME    		    4       // point towards home (no pilot input accepted)
+#define YAW_CIRCLE                      4       // point towards a location held in yaw_look_at_WP (no pilot input accepted)
-#define YAW_LOOK_AT_HEADING    		    5       // point towards a particular angle (not pilot input accepted)
+#define YAW_LOOK_AT_HOME    		    5       // point towards home (no pilot input accepted)
-#define YAW_LOOK_AHEAD					6		// WARNING!  CODE IN DEVELOPMENT NOT PROVEN
+#define YAW_LOOK_AT_HEADING    		    6       // point towards a particular angle (not pilot input accepted)
-#define YAW_TOY                         7       // THOR This is the Yaw mode
+#define YAW_LOOK_AHEAD					7		// WARNING!  CODE IN DEVELOPMENT NOT PROVEN
+#define YAW_TOY                         8       // THOR This is the Yaw mode
 
 
 #define ROLL_PITCH_STABLE           0       // pilot input roll, pitch angles

ArduCopter/navigation.pde

@@ -81,12 +81,6 @@ static void calc_distance_and_bearing()
         home_distance = 0;
         home_bearing = 0;
     }
-
-    // calculate bearing to target (used when yaw_mode = YAW_LOOK_AT_LOCATION)
-    // To-Do: move this to the look-at-waypoint yaw controller
-    if( yaw_mode == YAW_LOOK_AT_LOCATION ) {
-        yaw_look_at_WP_bearing = pv_get_bearing_cd(curr, yaw_look_at_WP);
-    }
 }
 
 // run_autopilot - highest level call to process mission commands
@@ -279,25 +273,31 @@ static bool waypoint_valid(Location &wp)
 
 // circle_set_center -- set circle controller's center position and starting angle
 static void
-circle_set_center(const Vector3f pos_vec, float heading_in_radians)
+circle_set_center(const Vector3f current_position, float heading_in_radians)
 {
-    // set circle center
+    // set circle center to circle_radius ahead of current position
-    circle_center = pos_vec;
+    circle_center.x = current_position.x + (float)g.circle_radius * 100 * sin_yaw_y;
+    circle_center.y = current_position.y + (float)g.circle_radius * 100 * cos_yaw_x;
 
-    // set starting angle to current heading - 180 degrees
+    // if we are doing a panorama set the circle_angle to the current heading
-    circle_angle = heading_in_radians-ToRad(180);
+    if( g.circle_radius == 0 ) {
-    if( circle_angle > 180 ) {
+        circle_angle = heading_in_radians;
-        circle_angle -= 180;
+    }else{
-    }
+        // set starting angle to current heading - 180 degrees
-    if( circle_angle < -180 ) {
+        circle_angle = heading_in_radians-ToRad(180);
-        circle_angle -= 180;
+        if( circle_angle > 180 ) {
+            circle_angle -= 180;
+        }
+        if( circle_angle < -180 ) {
+            circle_angle -= 180;
+        }
     }
 
     // initialise other variables
     circle_angle_total = 0;
 }
 
-// circle_get_pos - circle position controller's main call which in turn calls loiter controller with updated target position
+// update_circle - circle position controller's main call which in turn calls loiter controller with updated target position
 static void
 update_circle(float dt)
 {
@@ -317,12 +317,15 @@ update_circle(float dt)
     // update the total angle travelled
     circle_angle_total += angle_delta;
 
-    // calculate target position
+    // if the circle_radius is zero we are doing panorama so no need to update loiter target
-    circle_target.x = circle_center.x + cir_radius * sinf(1.57f - circle_angle);
+    if( g.circle_radius != 0.0 ) {
-    circle_target.y = circle_center.y + cir_radius * cosf(1.57f - circle_angle);
+        // calculate target position
+        circle_target.x = circle_center.x + cir_radius * sinf(1.57f - circle_angle);
+        circle_target.y = circle_center.y + cir_radius * cosf(1.57f - circle_angle);
 
-    // re-use loiter position controller
+        // re-use loiter position controller
-    wp_nav.set_loiter_target(circle_target);
+        wp_nav.set_loiter_target(circle_target);
+    }
 
     // call loiter controller
     wp_nav.update_loiter();

ArduCopter/system.pde

@@ -426,11 +426,10 @@ static void set_mode(uint8_t mode)
     case CIRCLE:
     	ap.manual_throttle = false;
     	ap.manual_attitude = false;
-        // set yaw to point to center of circle
-        set_yaw_mode(CIRCLE_YAW);
         set_roll_pitch_mode(CIRCLE_RP);
         set_throttle_mode(CIRCLE_THR);
         set_nav_mode(CIRCLE_NAV);
+        set_yaw_mode(CIRCLE_YAW);
         break;
 
     case LOITER: