From 99f5462a03a95fd6acde57236bfdf0d779fd487a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 1 Jun 2013 17:53:57 +0900 Subject: [PATCH] Copter: circle mode fixes --- ArduCopter/ArduCopter.pde | 5 ++++- ArduCopter/navigation.pde | 46 ++++++++++++++++++++++----------------- 2 files changed, 30 insertions(+), 21 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 40dc193efb..5e52ceeb72 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -486,7 +486,7 @@ static float scaleLongDown = 1; //////////////////////////////////////////////////////////////////////////////// // This is the angle from the copter to the next waypoint in centi-degrees static int32_t wp_bearing; -// The original bearing to the next waypoint. used to check if we've passed the waypoint +// The original bearing to the next waypoint. used to point the nose of the copter at the next waypoint static int32_t original_wp_bearing; // The location of home in relation to the copter in centi-degrees static int32_t home_bearing; @@ -580,6 +580,9 @@ static float circle_angle; static float circle_angle_total; // deg : how many times to circle as specified by mission command static uint8_t circle_desired_rotations; +static float circle_angular_acceleration; // circle mode's angular acceleration +static float circle_angular_velocity; // circle mode's angular velocity +static float circle_angular_velocity_max; // circle mode's max angular velocity // How long we should stay in Loiter Mode for mission scripting (time in seconds) static uint16_t loiter_time_max; // How long have we been loitering - The start time in millis diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 2b53971e63..df65cf8bea 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -216,53 +216,59 @@ static int32_t get_yaw_slew(int32_t current_yaw, int32_t desired_yaw, int16_t de static void circle_set_center(const Vector3f current_position, float heading_in_radians) { + float max_velocity; + // set circle center to circle_radius ahead of current position - circle_center.x = current_position.x + (float)g.circle_radius * 100 * sin_yaw; - circle_center.y = current_position.y + (float)g.circle_radius * 100 * cos_yaw; + circle_center.x = current_position.x + (float)g.circle_radius * 100 * cos_yaw; + circle_center.y = current_position.y + (float)g.circle_radius * 100 * 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 = heading_in_radians-ToRad(180); - if( circle_angle > 180 ) { - circle_angle -= 180; - } - if( circle_angle < -180 ) { - circle_angle -= 180; - } + 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*WPNAV_ACCELERATION*g.circle_radius*100.0f)); + + // angular_velocity in radians per second + circle_angular_velocity_max = min(ToRad(g.circle_rate), max_velocity/((float)g.circle_radius * 100.0f)); + circle_angular_acceleration = WPNAV_ACCELERATION/((float)g.circle_radius * 100); } // initialise other variables circle_angle_total = 0; + circle_angular_velocity = 0; } // update_circle - circle position controller's main call which in turn calls loiter controller with updated target position static void update_circle(float dt) { - float angle_delta = ToRad(g.circle_rate) * dt; float cir_radius = g.circle_radius * 100; Vector3f circle_target; - // update the target angle - circle_angle += angle_delta; - if( circle_angle > 180 ) { - circle_angle -= 360; - } - if( circle_angle <= -180 ) { - circle_angle += 360; + // ramp up angular velocity to maximum + 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); } + // update the target angle + circle_angle += circle_angular_velocity * dt; + circle_angle = wrap_PI(circle_angle); + // update the total angle travelled - circle_angle_total += angle_delta; + 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 * sinf(1.57f - circle_angle); - circle_target.y = circle_center.y + cir_radius * cosf(1.57f - circle_angle); + circle_target.x = circle_center.x + cir_radius * cosf(-circle_angle); + circle_target.y = circle_center.y - cir_radius * sinf(-circle_angle); // re-use loiter position controller wp_nav.set_loiter_target(circle_target);