Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

Copter: Cope with AC_PosControl renaming

Browse Source
This commit is contained in:
Michael du Breuil 2018-09-19 22:45:02 -07:00 committed by WickedShell
parent 27fad4489e
commit 7e1ed948f5
14 changed files with 57 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Attitude.cpp
View File

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

4
ArduCopter/mode.cpp
View File

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

4
ArduCopter/mode_althold.cpp
View File

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

8
ArduCopter/mode_autotune.cpp
View File

@ -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()) {
@ -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

4
ArduCopter/mode_brake.cpp
View File

@ -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()) {

16
ArduCopter/mode_circle.cpp
View File

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

8
ArduCopter/mode_flowhold.cpp
View File

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

14
ArduCopter/mode_follow.cpp
View File

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

26
ArduCopter/mode_guided.cpp
View File

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

4
ArduCopter/mode_land.cpp
View File

@ -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()) {

4
ArduCopter/mode_loiter.cpp
View File

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

8
ArduCopter/mode_poshold.cpp
View File

@ -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()) {
@ -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

8
ArduCopter/mode_sport.cpp
View File

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

4
ArduCopter/mode_throw.cpp
View File

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