Copter: move auto to control_auto.pde and fix yaw

ArduCopter/control_auto.pde

@@ -0,0 +1,206 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/*
+ * control_auto.pde - init and run calls for auto, guided, land and RTL flight modes
+ */
+
+// auto_init - initialise auto controller
+static bool auto_init(bool ignore_checks)
+{
+    if (GPS_ok() || ignore_checks) {
+        // set target to current position
+        wp_nav.init_loiter_target();
+        // initialise auto_yaw_mode
+        set_auto_yaw_mode(get_default_auto_yaw_mode(false));
+        cliSerial->printf_P(PSTR("\nYM:%d\n"),(int)auto_yaw_mode);
+        // clear the command queues. will be reloaded when "run_autopilot" calls "update_commands" function
+        init_commands();
+        return true;
+    }else{
+        return false;
+    }
+}
+
+// auto_run - runs the auto controller
+//      should be called at 100hz or more
+//      relies on run_autopilot being called at 10hz which handles decision making and non-navigation related commands
+static void auto_run()
+{
+    float target_yaw_rate = 0;
+
+    // if not auto armed set throttle to zero and exit immediately
+    if(!ap.auto_armed || !inertial_nav.position_ok()) {
+        wp_nav.init_loiter_target();
+        attitude_control.init_targets();
+        attitude_control.set_throttle_out(0, false);
+        return;
+    }
+
+    // process pilot's yaw input
+    if (!failsafe.radio) {
+        // get pilot's desired yaw rate
+        target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
+        if (target_yaw_rate != 0) {
+            set_auto_yaw_mode(AUTO_YAW_HOLD);
+        }
+    }
+
+    // run waypoint controller
+    wp_nav.update_wpnav();
+
+    // call z-axis position controller (wpnav should have already updated it's alt target)
+    pos_control.update_z_controller();
+
+    // call attitude controller
+    if (auto_yaw_mode == AUTO_YAW_HOLD) {
+        // roll & pitch from waypoint controller, yaw rate from pilot
+        attitude_control.angleef_rp_rateef_y(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
+    }else{
+        // roll, pitch from waypoint controller, yaw heading from auto_heading()
+        attitude_control.angleef_rpy(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading());
+    }
+
+    // body-frame rate controller is run directly from 100hz loop
+
+    // refetch angle targets for reporting
+    const Vector3f angle_target = attitude_control.angle_ef_targets();
+    control_roll = angle_target.x;
+    control_pitch = angle_target.y;
+    control_yaw = angle_target.z;
+}
+
+// get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter
+// set rtl parameter to true if this is during an RTL
+uint8_t get_default_auto_yaw_mode(bool rtl)
+{
+    switch (g.wp_yaw_behavior) {
+
+        case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL:
+            if (rtl) {
+                return AUTO_YAW_HOLD;
+            }else{
+                return AUTO_YAW_LOOK_AT_NEXT_WP;
+            }
+            break;
+
+        case WP_YAW_BEHAVIOR_LOOK_AHEAD:
+            return AUTO_YAW_LOOK_AHEAD;
+            break;
+
+        case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP:
+        default:
+            return AUTO_YAW_LOOK_AT_NEXT_WP;
+            break;
+    }
+}
+
+// set_auto_yaw_mode - sets the yaw mode for auto
+void set_auto_yaw_mode(uint8_t yaw_mode)
+{
+    // return immediately if no change
+    if (auto_yaw_mode == yaw_mode) {
+        return;
+    }
+    auto_yaw_mode = yaw_mode;
+
+    // perform initialisation
+    switch (auto_yaw_mode) {
+
+    case AUTO_YAW_LOOK_AT_NEXT_WP:
+        // original_wp_bearing will be set by do_nav_wp or other nav command initialisatoin functions so no init required
+        break;
+
+    case AUTO_YAW_LOOK_AT_LOCATION:
+        // point towards a location held in yaw_look_at_WP
+        yaw_look_at_WP_bearing = ahrs.yaw_sensor;
+        break;
+
+    case AUTO_YAW_LOOK_AT_HEADING:
+        // keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed
+        yaw_look_at_heading = ahrs.yaw_sensor;
+        break;
+
+    case AUTO_YAW_LOOK_AHEAD:
+        // Commanded Yaw to automatically look ahead.
+        yaw_look_ahead_bearing = ahrs.yaw_sensor;
+        break;
+
+    case AUTO_YAW_RESETTOARMEDYAW:
+        // initial_armed_bearing will be set during arming so no init required
+        break;
+    }
+}
+
+// get_auto_heading - returns target heading depending upon auto_yaw_mode
+// 100hz update rate
+float get_auto_heading(void)
+{
+    switch(auto_yaw_mode) {
+
+    case AUTO_YAW_LOOK_AT_LOCATION:
+        // point towards a location held in yaw_look_at_WP
+        return get_look_at_yaw();
+        break;
+
+    case AUTO_YAW_LOOK_AT_HEADING:
+        // keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed
+        return yaw_look_at_heading;
+        break;
+
+    case AUTO_YAW_LOOK_AHEAD:
+        // Commanded Yaw to automatically look ahead.
+        return get_look_ahead_yaw();
+        break;
+
+    case AUTO_YAW_RESETTOARMEDYAW:
+        // changes yaw to be same as when quad was armed
+        control_yaw = initial_armed_bearing;
+        break;
+
+    case AUTO_YAW_LOOK_AT_NEXT_WP:
+    default:
+        // point towards next waypoint.
+        // we don't use wp_bearing because we don't want the copter to turn too much during flight
+        return original_wp_bearing;
+        break;
+    }
+}
+
+// guided_init - initialise guided controller
+static bool guided_init(bool ignore_checks)
+{
+    return true;
+}
+
+// guided_run - runs the guided controller
+// should be called at 100hz or more
+static void guided_run()
+{
+}
+
+// land_init - initialise land controller
+static bool land_init(bool ignore_checks)
+{
+    return true;
+}
+
+// land_run - runs the land controller
+// should be called at 100hz or more
+static void land_run()
+{
+    verify_land();
+}
+
+// rtl_init - initialise rtl controller
+static bool rtl_init(bool ignore_checks)
+{
+    return true;
+}
+
+// rtl_run - runs the return-to-launch controller
+// should be called at 100hz or more
+static void rtl_run()
+{
+    verify_RTL();
+}
+

ArduCopter/control_stabilize.pde

@@ -30,6 +30,8 @@ static void acro_run()
     attitude_control.rate_stab_bf_to_rate_bf_roll();
     attitude_control.rate_stab_bf_to_rate_bf_pitch();
     attitude_control.rate_stab_bf_to_rate_bf_yaw();
+    // pilot controlled yaw using rate controller
+    //get_yaw_rate_stabilized_bf(pilot_yaw);
 
     // call get_acro_level_rates() here?
 
@@ -182,46 +184,14 @@ static void althold_run()
     control_yaw = angle_target.z;
 }
 
-// auto_init - initialise auto controller
-static bool auto_init(bool ignore_checks)
-{
-    return true;
-}
-
-// auto_run - runs the auto controller
-// should be called at 100hz or more
-static void auto_run()
-{
-    Vector3f angle_target;
-
-    // run way point controller
-
-    // copy latest output from nav controller to stabilize controller
-    angle_target.x = wp_nav.get_roll();
-    angle_target.y = wp_nav.get_pitch();
-
-    // To-Do: handle pilot input for yaw and different methods to update yaw (ROI, face next wp)
-    angle_target.z = control_yaw;
-
-    // To-Do: shorten below by moving these often used steps into a single function in the AC_AttitudeControl lib
-
-    // set earth-frame angular targets
-    attitude_control.angle_ef_targets(angle_target);
-
-    // convert earth-frame angle targets to earth-frame rate targets
-    attitude_control.angle_to_rate_ef_roll();
-    attitude_control.angle_to_rate_ef_pitch();
-    attitude_control.angle_to_rate_ef_yaw();
-
-    // convert earth-frame rates to body-frame rates
-    attitude_control.rate_ef_targets_to_bf();
-
-    // body-frame rate controller is run directly from 100hz loop
-}
-
 // circle_init - initialise circle controller
 static bool circle_init(bool ignore_checks)
 {
+    // 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;
     return true;
 }
 
@@ -229,6 +199,12 @@ static bool circle_init(bool ignore_checks)
 // should be called at 100hz or more
 static void circle_run()
 {
+    // if we are landed reset yaw target to current heading
+    //if (ap.land_complete) {
+    //    control_yaw = ahrs.yaw_sensor;
+    //}
+    // points toward the center of the circle or does a panorama
+    //get_circle_yaw();
 }
 
 // loiter_init - initialise loiter controller
@@ -236,7 +212,6 @@ static bool loiter_init(bool ignore_checks)
 {
     if (GPS_ok() || ignore_checks) {
         // set target to current position
-        // To-Do: supply zero velocity below?
         wp_nav.init_loiter_target();
         return true;
     }else{
@@ -314,44 +289,6 @@ static void loiter_run()
     control_yaw = angle_target.z;
 }
 
-// guided_init - initialise guided controller
-static bool guided_init(bool ignore_checks)
-{
-    return true;
-}
-
-// guided_run - runs the guided controller
-// should be called at 100hz or more
-static void guided_run()
-{
-}
-
-// land_init - initialise land controller
-static bool land_init(bool ignore_checks)
-{
-    return true;
-}
-
-// land_run - runs the land controller
-// should be called at 100hz or more
-static void land_run()
-{
-    verify_land();
-}
-
-// rtl_init - initialise rtl controller
-static bool rtl_init(bool ignore_checks)
-{
-    return true;
-}
-
-// rtl_run - runs the return-to-launch controller
-// should be called at 100hz or more
-static void rtl_run()
-{
-    verify_RTL();
-}
-
 // ofloiter_init - initialise ofloiter controller
 static bool ofloiter_init(bool ignore_checks)
 {
@@ -374,6 +311,11 @@ static bool drift_init(bool ignore_checks)
 // should be called at 100hz or more
 static void drift_run()
 {
+    // if we have landed reset yaw target to current heading
+    //if (ap.land_complete) {
+    //    control_yaw = ahrs.yaw_sensor;
+    //}
+    //get_yaw_drift();
 }
 
 // sport_init - initialise sport controller