Skip to content

Commit

Permalink
Copter: Cope with AC_PosControl renaming
Browse files Browse the repository at this point in the history
  • Loading branch information
@WickedShell
WickedShell committed Sep 20, 2018
1 parent 27fad44 commit 7e1ed94
Show file tree
Hide file tree
Showing 14 changed files with 57 additions and 57 deletions.
2 changes: 1 addition & 1 deletion ArduCopter/Attitude.cpp
View file
Expand Up @@ -229,7 +229,7 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current
float Copter::get_avoidance_adjusted_climbrate(float target_rate)
{
#if AC_AVOID_ENABLED == ENABLED
avoid.adjust_velocity_z(pos_control->get_pos_z_p().kP(), pos_control->get_accel_z(), target_rate, G_Dt);
avoid.adjust_velocity_z(pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z(), target_rate, G_Dt);
return target_rate;
#else
return target_rate;
Expand Down
4 changes: 2 additions & 2 deletions ArduCopter/mode.cpp
View file
Expand Up @@ -425,14 +425,14 @@ void Copter::Mode::land_run_vertical_control(bool pause_descent)
if (g.land_speed_high > 0) {
max_land_descent_velocity = -g.land_speed_high;
} else {
max_land_descent_velocity = pos_control->get_speed_down();
max_land_descent_velocity = pos_control->get_max_speed_down();
}

// Don't speed up for landing.
max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(g.land_speed));

// Compute a vertical velocity demand such that the vehicle approaches g2.land_alt_low. Without the below constraint, this would cause the vehicle to hover at g2.land_alt_low.
cmb_rate = AC_AttitudeControl::sqrt_controller(MAX(g2.land_alt_low,100)-alt_above_ground, pos_control->get_pos_z_p().kP(), pos_control->get_accel_z(), G_Dt);
cmb_rate = AC_AttitudeControl::sqrt_controller(MAX(g2.land_alt_low,100)-alt_above_ground, pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z(), G_Dt);

// Constrain the demanded vertical velocity so that it is between the configured maximum descent speed and the configured minimum descent speed.
cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(g.land_speed));
Expand Down
4 changes: 2 additions & 2 deletions ArduCopter/mode_althold.cpp
View file
Expand Up @@ -25,8 +25,8 @@ void Copter::ModeAltHold::run()
float takeoff_climb_rate = 0.0f;

// initialize vertical speeds and acceleration
pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_accel_z(g.pilot_accel_z);
pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_max_accel_z(g.pilot_accel_z);

// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
Expand Down
8 changes: 4 additions & 4 deletions ArduCopter/mode_autotune.cpp
View file
Expand Up @@ -181,8 +181,8 @@ bool Copter::ModeAutoTune::start(bool ignore_checks)
}

// initialize vertical speeds and leash lengths
pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_accel_z(g.pilot_accel_z);
pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_max_accel_z(g.pilot_accel_z);

// initialise position and desired velocity
if (!pos_control->is_active_z()) {
Expand Down Expand Up @@ -319,8 +319,8 @@ void Copter::ModeAutoTune::run()
int16_t target_climb_rate;

// initialize vertical speeds and acceleration
pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_accel_z(g.pilot_accel_z);
pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_max_accel_z(g.pilot_accel_z);

// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
// this should not actually be possible because of the init() checks
Expand Down
4 changes: 2 additions & 2 deletions ArduCopter/mode_brake.cpp
View file
Expand Up @@ -13,8 +13,8 @@ bool Copter::ModeBrake::init(bool ignore_checks)
wp_nav->init_brake_target(BRAKE_MODE_DECEL_RATE);

// initialize vertical speed and acceleration
pos_control->set_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z);
pos_control->set_accel_z(BRAKE_MODE_DECEL_RATE);
pos_control->set_max_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z);
pos_control->set_max_accel_z(BRAKE_MODE_DECEL_RATE);

// initialise position and desired velocity
if (!pos_control->is_active_z()) {
Expand Down
16 changes: 8 additions & 8 deletions ArduCopter/mode_circle.cpp
View file
Expand Up @@ -13,10 +13,10 @@ bool Copter::ModeCircle::init(bool ignore_checks)
pilot_yaw_override = false;

// initialize speeds and accelerations
pos_control->set_speed_xy(wp_nav->get_speed_xy());
pos_control->set_accel_xy(wp_nav->get_wp_acceleration());
pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_accel_z(g.pilot_accel_z);
pos_control->set_max_speed_xy(wp_nav->get_speed_xy());
pos_control->set_max_accel_xy(wp_nav->get_wp_acceleration());
pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_max_accel_z(g.pilot_accel_z);

// initialise circle controller including setting the circle center based on vehicle speed
copter.circle_nav->init();
Expand All @@ -35,10 +35,10 @@ void Copter::ModeCircle::run()
float target_climb_rate = 0;

// initialize speeds and accelerations
pos_control->set_speed_xy(wp_nav->get_speed_xy());
pos_control->set_accel_xy(wp_nav->get_wp_acceleration());
pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_accel_z(g.pilot_accel_z);
pos_control->set_max_speed_xy(wp_nav->get_speed_xy());
pos_control->set_max_accel_xy(wp_nav->get_wp_acceleration());
pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_max_accel_z(g.pilot_accel_z);

// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
Expand Down
8 changes: 4 additions & 4 deletions ArduCopter/mode_flowhold.cpp
View file
Expand Up @@ -89,8 +89,8 @@ bool Copter::ModeFlowHold::init(bool ignore_checks)
}

// initialize vertical speeds and leash lengths
copter.pos_control->set_speed_z(-copter.g2.pilot_speed_dn, copter.g.pilot_speed_up);
copter.pos_control->set_accel_z(copter.g.pilot_accel_z);
copter.pos_control->set_max_speed_z(-copter.g2.pilot_speed_dn, copter.g.pilot_speed_up);
copter.pos_control->set_max_accel_z(copter.g.pilot_accel_z);

// initialise position and desired velocity
if (!copter.pos_control->is_active_z()) {
Expand Down Expand Up @@ -224,8 +224,8 @@ void Copter::ModeFlowHold::run()
update_height_estimate();

// initialize vertical speeds and acceleration
copter.pos_control->set_speed_z(-copter.g2.pilot_speed_dn, copter.g.pilot_speed_up);
copter.pos_control->set_accel_z(copter.g.pilot_accel_z);
copter.pos_control->set_max_speed_z(-copter.g2.pilot_speed_dn, copter.g.pilot_speed_up);
copter.pos_control->set_max_accel_z(copter.g.pilot_accel_z);

// apply SIMPLE mode transform to pilot inputs
copter.update_simple_mode();
Expand Down
14 changes: 7 additions & 7 deletions ArduCopter/mode_follow.cpp
View file
Expand Up @@ -75,15 +75,15 @@ void Copter::ModeFollow::run()

// scale desired velocity to stay within horizontal speed limit
float desired_speed_xy = safe_sqrt(sq(desired_velocity_neu_cms.x) + sq(desired_velocity_neu_cms.y));
if (!is_zero(desired_speed_xy) && (desired_speed_xy > pos_control->get_speed_xy())) {
const float scalar_xy = pos_control->get_speed_xy() / desired_speed_xy;
if (!is_zero(desired_speed_xy) && (desired_speed_xy > pos_control->get_max_speed_xy())) {
const float scalar_xy = pos_control->get_max_speed_xy() / desired_speed_xy;
desired_velocity_neu_cms.x *= scalar_xy;
desired_velocity_neu_cms.y *= scalar_xy;
desired_speed_xy = pos_control->get_speed_xy();
desired_speed_xy = pos_control->get_max_speed_xy();
}

// limit desired velocity to be between maximum climb and descent rates
desired_velocity_neu_cms.z = constrain_float(desired_velocity_neu_cms.z, -fabsf(pos_control->get_speed_down()), pos_control->get_speed_up());
desired_velocity_neu_cms.z = constrain_float(desired_velocity_neu_cms.z, -fabsf(pos_control->get_max_speed_down()), pos_control->get_max_speed_up());

// unit vector towards target position (i.e. vector to lead vehicle + offset)
Vector3f dir_to_target_neu = dist_vec_offs_neu;
Expand All @@ -103,17 +103,17 @@ void Copter::ModeFollow::run()

// slow down horizontally as we approach target (use 1/2 of maximum deceleration for gentle slow down)
const float dist_to_target_xy = Vector2f(dist_vec_offs_neu.x, dist_vec_offs_neu.y).length();
copter.avoid.limit_velocity(pos_control->get_pos_xy_p().kP().get(), pos_control->get_accel_xy() * 0.5f, desired_velocity_xy_cms, dir_to_target_xy, dist_to_target_xy, copter.G_Dt);
copter.avoid.limit_velocity(pos_control->get_pos_xy_p().kP().get(), pos_control->get_max_accel_xy() * 0.5f, desired_velocity_xy_cms, dir_to_target_xy, dist_to_target_xy, copter.G_Dt);

// limit the horizontal velocity to prevent fence violations
copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP().get(), pos_control->get_accel_xy(), desired_velocity_xy_cms, G_Dt);
copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP().get(), pos_control->get_max_accel_xy(), desired_velocity_xy_cms, G_Dt);

// copy horizontal velocity limits back to 3d vector
desired_velocity_neu_cms.x = desired_velocity_xy_cms.x;
desired_velocity_neu_cms.y = desired_velocity_xy_cms.y;

// limit vertical desired_velocity_neu_cms to slow as we approach target (we use 1/2 of maximum deceleration for gentle slow down)
const float des_vel_z_max = copter.avoid.get_max_speed(pos_control->get_pos_z_p().kP().get(), pos_control->get_accel_z() * 0.5f, fabsf(dist_vec_offs_neu.z), copter.G_Dt);
const float des_vel_z_max = copter.avoid.get_max_speed(pos_control->get_pos_z_p().kP().get(), pos_control->get_max_accel_z() * 0.5f, fabsf(dist_vec_offs_neu.z), copter.G_Dt);
desired_velocity_neu_cms.z = constrain_float(desired_velocity_neu_cms.z, -des_vel_z_max, des_vel_z_max);

// get avoidance adjusted climb rate
Expand Down
26 changes: 13 additions & 13 deletions ArduCopter/mode_guided.cpp
View file
Expand Up @@ -103,12 +103,12 @@ void Copter::ModeGuided::vel_control_start()
guided_mode = Guided_Velocity;

// initialise horizontal speed, acceleration
pos_control->set_speed_xy(wp_nav->get_speed_xy());
pos_control->set_accel_xy(wp_nav->get_wp_acceleration());
pos_control->set_max_speed_xy(wp_nav->get_speed_xy());
pos_control->set_max_accel_xy(wp_nav->get_wp_acceleration());

// initialize vertical speeds and acceleration
pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_accel_z(g.pilot_accel_z);
pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_max_accel_z(g.pilot_accel_z);

// initialise velocity controller
pos_control->init_vel_controller_xyz();
Expand All @@ -123,8 +123,8 @@ void Copter::ModeGuided::posvel_control_start()
pos_control->init_xy_controller();

// set speed and acceleration from wpnav's speed and acceleration
pos_control->set_speed_xy(wp_nav->get_speed_xy());
pos_control->set_accel_xy(wp_nav->get_wp_acceleration());
pos_control->set_max_speed_xy(wp_nav->get_speed_xy());
pos_control->set_max_accel_xy(wp_nav->get_wp_acceleration());

const Vector3f& curr_pos = inertial_nav.get_position();
const Vector3f& curr_vel = inertial_nav.get_velocity();
Expand All @@ -134,8 +134,8 @@ void Copter::ModeGuided::posvel_control_start()
pos_control->set_desired_velocity_xy(curr_vel.x, curr_vel.y);

// set vertical speed and acceleration
pos_control->set_speed_z(wp_nav->get_speed_down(), wp_nav->get_speed_up());
pos_control->set_accel_z(wp_nav->get_accel_z());
pos_control->set_max_speed_z(wp_nav->get_speed_down(), wp_nav->get_speed_up());
pos_control->set_max_accel_z(wp_nav->get_accel_z());

// pilot always controls yaw
auto_yaw.set_mode(AUTO_YAW_HOLD);
Expand All @@ -148,8 +148,8 @@ void Copter::ModeGuided::angle_control_start()
guided_mode = Guided_Angle;

// set vertical speed and acceleration
pos_control->set_speed_z(wp_nav->get_speed_down(), wp_nav->get_speed_up());
pos_control->set_accel_z(wp_nav->get_accel_z());
pos_control->set_max_speed_z(wp_nav->get_speed_down(), wp_nav->get_speed_up());
pos_control->set_max_accel_z(wp_nav->get_accel_z());

// initialise position and desired velocity
if (!pos_control->is_active_z()) {
Expand Down Expand Up @@ -650,7 +650,7 @@ void Copter::ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const

// limit xy change
float vel_delta_xy = safe_sqrt(sq(vel_delta.x)+sq(vel_delta.y));
float vel_delta_xy_max = G_Dt * pos_control->get_accel_xy();
float vel_delta_xy_max = G_Dt * pos_control->get_max_accel_xy();
float ratio_xy = 1.0f;
if (!is_zero(vel_delta_xy) && (vel_delta_xy > vel_delta_xy_max)) {
ratio_xy = vel_delta_xy_max / vel_delta_xy;
Expand All @@ -659,12 +659,12 @@ void Copter::ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const
curr_vel_des.y += (vel_delta.y * ratio_xy);

// limit z change
float vel_delta_z_max = G_Dt * pos_control->get_accel_z();
float vel_delta_z_max = G_Dt * pos_control->get_max_accel_z();
curr_vel_des.z += constrain_float(vel_delta.z, -vel_delta_z_max, vel_delta_z_max);

#if AC_AVOID_ENABLED
// limit the velocity to prevent fence violations
copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP(), pos_control->get_accel_xy(), curr_vel_des, G_Dt);
copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP(), pos_control->get_max_accel_xy(), curr_vel_des, G_Dt);
// get avoidance adjusted climb rate
curr_vel_des.z = get_avoidance_adjusted_climbrate(curr_vel_des.z);
#endif
Expand Down
4 changes: 2 additions & 2 deletions ArduCopter/mode_land.cpp
View file
Expand Up @@ -19,8 +19,8 @@ bool Copter::ModeLand::init(bool ignore_checks)
}

// initialize vertical speeds and leash lengths
pos_control->set_speed_z(wp_nav->get_speed_down(), wp_nav->get_speed_up());
pos_control->set_accel_z(wp_nav->get_accel_z());
pos_control->set_max_speed_z(wp_nav->get_speed_down(), wp_nav->get_speed_up());
pos_control->set_max_accel_z(wp_nav->get_accel_z());

// initialise position and desired velocity
if (!pos_control->is_active_z()) {
Expand Down
4 changes: 2 additions & 2 deletions ArduCopter/mode_loiter.cpp
View file
Expand Up @@ -84,8 +84,8 @@ void Copter::ModeLoiter::run()
float takeoff_climb_rate = 0.0f;

// initialize vertical speed and acceleration
pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_accel_z(g.pilot_accel_z);
pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_max_accel_z(g.pilot_accel_z);

// process pilot inputs unless we are in radio failsafe
if (!copter.failsafe.radio) {
Expand Down
8 changes: 4 additions & 4 deletions ArduCopter/mode_poshold.cpp
View file
Expand Up @@ -78,8 +78,8 @@ bool Copter::ModePosHold::init(bool ignore_checks)
}

// initialize vertical speeds and acceleration
pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_accel_z(g.pilot_accel_z);
pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_max_accel_z(g.pilot_accel_z);

// initialise position and desired velocity
if (!pos_control->is_active_z()) {
Expand Down Expand Up @@ -132,8 +132,8 @@ void Copter::ModePosHold::run()
const Vector3f& vel = inertial_nav.get_velocity();

// initialize vertical speeds and acceleration
pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_accel_z(g.pilot_accel_z);
pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_max_accel_z(g.pilot_accel_z);
loiter_nav->clear_pilot_desired_acceleration();

// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
Expand Down
8 changes: 4 additions & 4 deletions ArduCopter/mode_sport.cpp
View file
Expand Up @@ -8,8 +8,8 @@
bool Copter::ModeSport::init(bool ignore_checks)
{
// initialize vertical speed and acceleration
pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_accel_z(g.pilot_accel_z);
pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_max_accel_z(g.pilot_accel_z);

// initialise position and desired velocity
if (!pos_control->is_active_z()) {
Expand All @@ -28,8 +28,8 @@ void Copter::ModeSport::run()
float takeoff_climb_rate = 0.0f;

// initialize vertical speed and acceleration
pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_accel_z(g.pilot_accel_z);
pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_max_accel_z(g.pilot_accel_z);

// apply SIMPLE mode transform
update_simple_mode();
Expand Down
4 changes: 2 additions & 2 deletions ArduCopter/mode_throw.cpp
View file
Expand Up @@ -56,8 +56,8 @@ void Copter::ModeThrow::run()

// initialize vertical speed and acceleration limits
// use brake mode values for rapid response
pos_control->set_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z);
pos_control->set_accel_z(BRAKE_MODE_DECEL_RATE);
pos_control->set_max_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z);
pos_control->set_max_accel_z(BRAKE_MODE_DECEL_RATE);

// initialise the demanded height to 3m above the throw height
// we want to rapidly clear surrounding obstacles
Expand Down

0 comments on commit 7e1ed94

Please sign in to comment.