Copter: reduce twitch when entering CIRCLE mode

Set loiter target and prev iterations velocity when circle mode is
started
Start circling from projected stopping point
This commit is contained in:
Randy Mackay 2013-06-15 18:14:36 +09:00
parent 11bd9e37c8
commit 069d93444f
3 changed files with 22 additions and 10 deletions

View File

@ -103,8 +103,8 @@ static void run_autopilot()
// set_nav_mode - update nav mode and initialise any variables as required
static bool set_nav_mode(uint8_t new_nav_mode)
{
// boolean to ensure proper initialisation of nav modes
bool nav_initialised = false;
bool nav_initialised = false; // boolean to ensure proper initialisation of nav modes
Vector3f stopping_point; // stopping point for circle mode
// return immediately if no change
if( new_nav_mode == nav_mode ) {
@ -119,13 +119,14 @@ static bool set_nav_mode(uint8_t new_nav_mode)
case NAV_CIRCLE:
// set center of circle to current position
circle_set_center(inertial_nav.get_position(), ahrs.yaw);
wp_nav.get_stopping_point(inertial_nav.get_position(),inertial_nav.get_velocity(),stopping_point);
circle_set_center(stopping_point,ahrs.yaw);
nav_initialised = true;
break;
case NAV_LOITER:
// set target to current position
wp_nav.set_loiter_target(inertial_nav.get_position(), inertial_nav.get_velocity());
wp_nav.init_loiter_target(inertial_nav.get_position(), inertial_nav.get_velocity());
nav_initialised = true;
break;
@ -217,10 +218,11 @@ 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 + (float)g.circle_radius * 100 * cos_yaw;
circle_center.y = current_position.y + (float)g.circle_radius * 100 * sin_yaw;
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 ) {
@ -248,6 +250,9 @@ circle_set_center(const Vector3f current_position, float heading_in_radians)
// initialise other variables
circle_angle_total = 0;
circle_angular_velocity = 0;
// initialise loiter target. Note: feed forward velocity set to zero
wp_nav.init_loiter_target(current_position, Vector3f(0,0,0));
}
// update_circle - circle position controller's main call which in turn calls loiter controller with updated target position

View File

@ -143,8 +143,8 @@ void AC_WPNav::set_loiter_target(const Vector3f& position)
_target_vel.y = 0;
}
/// set_loiter_target - set initial loiter target based on current position and velocity
void AC_WPNav::set_loiter_target(const Vector3f& position, const Vector3f& velocity)
/// init_loiter_target - set initial loiter target based on current position and velocity
void AC_WPNav::init_loiter_target(const Vector3f& position, const Vector3f& velocity)
{
// set target position and velocity based on current pos and velocity
_target.x = position.x;
@ -155,6 +155,13 @@ void AC_WPNav::set_loiter_target(const Vector3f& position, const Vector3f& veloc
// initialise desired roll and pitch to current roll and pitch. This avoids a random twitch between now and when the loiter controller is first run
_desired_roll = constrain_int32(_ahrs->roll_sensor,-MAX_LEAN_ANGLE,MAX_LEAN_ANGLE);
_desired_pitch = constrain_int32(_ahrs->pitch_sensor,-MAX_LEAN_ANGLE,MAX_LEAN_ANGLE);
// initialise pilot input
_pilot_vel_forward_cms = 0;
_pilot_vel_right_cms = 0;
// set last velocity to current velocity
_vel_last = _inav->get_velocity();
}
/// move_loiter_target - move loiter target by velocity provided in front/right directions in cm/s

View File

@ -50,8 +50,8 @@ public:
/// set_loiter_target in cm from home
void set_loiter_target(const Vector3f& position);
/// set_loiter_target - set initial loiter target based on current position and velocity
void set_loiter_target(const Vector3f& position, const Vector3f& velocity);
/// init_loiter_target - set initial loiter target based on current position and velocity
void init_loiter_target(const Vector3f& position, const Vector3f& velocity);
/// move_loiter_target - move destination using pilot input
void move_loiter_target(float control_roll, float control_pitch, float dt);