Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 21:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: circle mode fixes

Browse Source
This commit is contained in:
Randy Mackay 2013-06-01 17:53:57 +09:00
parent e3cd494709
commit 99f5462a03
2 changed files with 30 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopter/ArduCopter.pde
View File

@ -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

46
ArduCopter/navigation.pde
View File

@ -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);