mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 18:08:30 -04:00
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:
parent
11bd9e37c8
commit
069d93444f
@ -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
|
||||
|
@ -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
|
||||
|
@ -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);
|
||||
|
Loading…
Reference in New Issue
Block a user