Copter: control_circle.pde added

ArduCopter/control_circle.pde

@@ -0,0 +1,208 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/*
+ * control_circle.pde - init and run calls for circle flight mode
+ */
+
+// circle_init - initialise circle controller
+static bool circle_init(bool ignore_checks)
+{
+    if ((GPS_ok() && inertial_nav.position_ok()) || ignore_checks) {
+        circle_pilot_yaw_override = false;
+        return true;
+    }else{
+        return false;
+    }
+}
+
+// loiter_run - runs the loiter controller
+// should be called at 100hz or more
+static void circle_run()
+{
+    float target_yaw_rate = 0;
+    float target_climb_rate = 0;
+
+    // if not auto armed set throttle to zero and exit immediately
+    if(!ap.auto_armed || ap.land_complete) {
+        wp_nav.init_loiter_target();
+        attitude_control.init_targets();
+        attitude_control.set_throttle_out(0, false);
+        return;
+    }
+
+    // process pilot inputs
+    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) {
+            circle_pilot_yaw_override = true;
+        }
+
+        // get pilot desired climb rate
+        target_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
+
+        // check for pilot requested take-off
+        if (ap.land_complete && target_climb_rate > 0) {
+            // indicate we are taking off
+            set_land_complete(false);
+            // clear i term when we're taking off
+            set_throttle_takeoff();
+        }
+    }
+
+    // run circle controller
+    circle_nav.update();
+
+    // call attitude controller
+    if (circle_pilot_yaw_override) {
+        attitude_control.angleef_rp_rateef_y(circle_nav.get_roll(), circle_nav.get_pitch(), target_yaw_rate);
+    }else{
+        attitude_control.angleef_rpy(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw());
+    }
+
+    // run altitude controller
+    if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {
+        // if sonar is ok, use surface tracking
+        target_climb_rate = get_throttle_surface_tracking(target_climb_rate,G_Dt);
+    }
+    // update altitude target and call position controller
+    pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
+    pos_control.update_z_controller();
+
+    // re-fetch 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;
+}
+
+//////////////////////////////////////////////////////////
+// circle navigation controller
+//////////////////////////////////////////////////////////
+/*
+// circle_set_center -- set circle controller's center position and starting angle
+static void
+circle_set_center(const Vector3f current_position, float heading_in_radians)
+{
+    float max_velocity;
+    float cir_radius = g.circle_radius * 100;
+
+    // set circle center to circle_radius ahead of current position
+    circle_center.x = current_position.x + cir_radius * cos_yaw;
+    circle_center.y = current_position.y + cir_radius * sin_yaw;
+
+    // if we are doing a panorama set the circle_angle to the current heading
+    if( g.circle_radius <= 0 ) {
+        circle_angle = heading_in_radians;
+        circle_angular_velocity_max = ToRad(g.circle_rate);
+        circle_angular_acceleration = circle_angular_velocity_max;  // reach maximum yaw velocity in 1 second
+    }else{
+        // set starting angle to current heading - 180 degrees
+        circle_angle = wrap_PI(heading_in_radians-PI);
+
+        // calculate max velocity based on waypoint speed ensuring we do not use more than half our max acceleration for accelerating towards the center of the circle
+        max_velocity = min(wp_nav.get_horizontal_velocity(), safe_sqrt(0.5f*wp_nav.get_wp_acceleration()*g.circle_radius*100.0f));
+
+        // angular_velocity in radians per second
+        circle_angular_velocity_max = max_velocity/((float)g.circle_radius * 100.0f);
+        circle_angular_velocity_max = constrain_float(ToRad(g.circle_rate),-circle_angular_velocity_max,circle_angular_velocity_max);
+
+        // angular_velocity in radians per second
+        circle_angular_acceleration = wp_nav.get_wp_acceleration()/((float)g.circle_radius * 100);
+        if (g.circle_rate < 0.0f) {
+            circle_angular_acceleration = -circle_angular_acceleration;
+        }
+    }
+
+    // initialise other variables
+    circle_angle_total = 0;
+    circle_angular_velocity = 0;
+
+    // initialise loiter target.  Note: feed forward velocity set to zero
+    // To-Do: modify circle to use position controller and pass in zero velocity.  Vector3f(0,0,0)
+    wp_nav.init_loiter_target();
+}
+
+// update_circle - circle position controller's main call which in turn calls loiter controller with updated target position
+static void
+update_circle()
+{
+    static float last_update;    // time of last circle call
+
+    // calculate dt
+    uint32_t now = hal.scheduler->millis();
+    float dt = (now - last_update) / 1000.0f;
+
+    // ensure enough time has passed since the last iteration
+    if (dt >= 0.095f) {
+        float cir_radius = g.circle_radius * 100;
+        Vector3f circle_target;
+
+        // range check dt
+        if (dt >= 1.0f) {
+            dt = 0;
+        }
+
+        // update time of circle call
+        last_update = now;
+
+        // ramp up angular velocity to maximum
+        if (g.circle_rate >= 0) {
+            if (circle_angular_velocity < circle_angular_velocity_max) {
+                circle_angular_velocity += circle_angular_acceleration * dt;
+                circle_angular_velocity = constrain_float(circle_angular_velocity, 0, circle_angular_velocity_max);
+            }
+        }else{
+            if (circle_angular_velocity > circle_angular_velocity_max) {
+                circle_angular_velocity += circle_angular_acceleration * dt;
+                circle_angular_velocity = constrain_float(circle_angular_velocity, circle_angular_velocity_max, 0);
+            }
+        }
+
+        // update the target angle
+        circle_angle += circle_angular_velocity * dt;
+        circle_angle = wrap_PI(circle_angle);
+
+        // update the total angle travelled
+        circle_angle_total += circle_angular_velocity * dt;
+
+        // if the circle_radius is zero we are doing panorama so no need to update loiter target
+        if( g.circle_radius != 0.0 ) {
+            // calculate target position
+            circle_target.x = circle_center.x + cir_radius * cosf(-circle_angle);
+            circle_target.y = circle_center.y - cir_radius * sinf(-circle_angle);
+            circle_target.z = wp_nav.get_desired_alt();
+
+            // re-use loiter position controller
+            wp_nav.set_loiter_target(circle_target);
+        }
+    }
+
+    // call loiter controller
+    wp_nav.update_loiter();
+}
+
+// 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
+       control_yaw = get_yaw_slew(control_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
+       control_yaw = get_yaw_slew(control_yaw, yaw_look_at_WP_bearing, AUTO_YAW_SLEW_RATE);
+   }
+
+   // call stabilize yaw controller
+   get_stabilize_yaw(control_yaw);
+}
+*/

ArduCopter/control_stabilize.pde

@@ -149,33 +149,6 @@ static void althold_run()
     control_yaw = angle_target.z;
 }
 
-// circle_init - initialise circle controller
-static bool circle_init(bool ignore_checks)
-{
-    if (GPS_ok() || ignore_checks) {
-        return true;
-    }else{
-        return false;
-    }
-    // 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;
-}
-
-// circle_run - runs the circle controller
-// 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();
-}
-
 // drift_init - initialise drift controller
 static bool drift_init(bool ignore_checks)
 {