From 717fb4d8238e2b09e466cdac0da64f9e3dfb50b2 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 24 Jan 2019 14:01:46 +0900 Subject: [PATCH] Copter: integrate AC_WPNav::get_speed rename to get_default_speed --- ArduCopter/GCS_Mavlink.cpp | 4 ++-- ArduCopter/avoidance_adsb.cpp | 16 ++++++++-------- ArduCopter/mode_auto.cpp | 4 ++-- ArduCopter/mode_circle.cpp | 4 ++-- ArduCopter/mode_guided.cpp | 10 +++++----- ArduCopter/mode_land.cpp | 2 +- ArduCopter/takeoff.cpp | 2 +- 7 files changed, 21 insertions(+), 21 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 9c605e26f8..1be208760b 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1082,10 +1082,10 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) climb_rate_cms = 0.0f; } else if (packet.thrust > 0.5f) { // climb at up to WPNAV_SPEED_UP - climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * copter.wp_nav->get_speed_up(); + climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * copter.wp_nav->get_default_speed_up(); } else { // descend at up to WPNAV_SPEED_DN - climb_rate_cms = (0.5f - packet.thrust) * 2.0f * -fabsf(copter.wp_nav->get_speed_down()); + climb_rate_cms = (0.5f - packet.thrust) * 2.0f * -fabsf(copter.wp_nav->get_default_speed_down()); } // if the body_yaw_rate field is ignored, use the commanded yaw position diff --git a/ArduCopter/avoidance_adsb.cpp b/ArduCopter/avoidance_adsb.cpp index b623e68480..f5f3f176c5 100644 --- a/ArduCopter/avoidance_adsb.cpp +++ b/ArduCopter/avoidance_adsb.cpp @@ -175,9 +175,9 @@ bool AP_Avoidance_Copter::handle_avoidance_vertical(const AP_Avoidance::Obstacle // get best vector away from obstacle Vector3f velocity_neu; if (should_climb) { - velocity_neu.z = copter.wp_nav->get_speed_up(); + velocity_neu.z = copter.wp_nav->get_default_speed_up(); } else { - velocity_neu.z = -copter.wp_nav->get_speed_down(); + velocity_neu.z = -copter.wp_nav->get_default_speed_down(); // do not descend if below RTL alt if (copter.current_loc.alt < copter.g.rtl_altitude) { velocity_neu.z = 0.0f; @@ -208,8 +208,8 @@ bool AP_Avoidance_Copter::handle_avoidance_horizontal(const AP_Avoidance::Obstac // re-normalise velocity_neu.normalize(); // convert horizontal components to velocities - velocity_neu.x *= copter.wp_nav->get_speed_xy(); - velocity_neu.y *= copter.wp_nav->get_speed_xy(); + velocity_neu.x *= copter.wp_nav->get_default_speed_xy(); + velocity_neu.y *= copter.wp_nav->get_default_speed_xy(); // send target velocity copter.mode_avoid_adsb.set_velocity(velocity_neu); return true; @@ -230,13 +230,13 @@ bool AP_Avoidance_Copter::handle_avoidance_perpendicular(const AP_Avoidance::Obs Vector3f velocity_neu; if (get_vector_perpendicular(obstacle, velocity_neu)) { // convert horizontal components to velocities - velocity_neu.x *= copter.wp_nav->get_speed_xy(); - velocity_neu.y *= copter.wp_nav->get_speed_xy(); + velocity_neu.x *= copter.wp_nav->get_default_speed_xy(); + velocity_neu.y *= copter.wp_nav->get_default_speed_xy(); // use up and down waypoint speeds if (velocity_neu.z > 0.0f) { - velocity_neu.z *= copter.wp_nav->get_speed_up(); + velocity_neu.z *= copter.wp_nav->get_default_speed_up(); } else { - velocity_neu.z *= copter.wp_nav->get_speed_down(); + velocity_neu.z *= copter.wp_nav->get_default_speed_down(); // do not descend if below RTL alt if (copter.current_loc.alt < copter.g.rtl_altitude) { velocity_neu.z = 0.0f; diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index dc40ebe412..5d05fc6ca6 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -1217,8 +1217,8 @@ void Copter::ModeAuto::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) loiter_to_alt.alt_error_cm = 0; pos_control->set_max_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_speed_z(wp_nav->get_default_speed_down(), + wp_nav->get_default_speed_up()); if (pos_control->is_active_z()) { pos_control->freeze_ff_z(); diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index a2b0a7d5d3..e046683559 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -13,7 +13,7 @@ bool Copter::ModeCircle::init(bool ignore_checks) pilot_yaw_override = false; // initialize speeds and accelerations - pos_control->set_max_speed_xy(wp_nav->get_speed_xy()); + pos_control->set_max_speed_xy(wp_nav->get_default_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); @@ -35,7 +35,7 @@ void Copter::ModeCircle::run() float target_climb_rate = 0; // initialize speeds and accelerations - pos_control->set_max_speed_xy(wp_nav->get_speed_xy()); + pos_control->set_max_speed_xy(wp_nav->get_default_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); diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 90286ceaca..0a744b75bb 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -103,7 +103,7 @@ void Copter::ModeGuided::vel_control_start() guided_mode = Guided_Velocity; // initialise horizontal speed, acceleration - pos_control->set_max_speed_xy(wp_nav->get_speed_xy()); + pos_control->set_max_speed_xy(wp_nav->get_default_speed_xy()); pos_control->set_max_accel_xy(wp_nav->get_wp_acceleration()); // initialize vertical speeds and acceleration @@ -123,7 +123,7 @@ void Copter::ModeGuided::posvel_control_start() pos_control->init_xy_controller(); // set speed and acceleration from wpnav's speed and acceleration - pos_control->set_max_speed_xy(wp_nav->get_speed_xy()); + pos_control->set_max_speed_xy(wp_nav->get_default_speed_xy()); pos_control->set_max_accel_xy(wp_nav->get_wp_acceleration()); const Vector3f& curr_pos = inertial_nav.get_position(); @@ -134,7 +134,7 @@ 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_max_speed_z(wp_nav->get_speed_down(), wp_nav->get_speed_up()); + pos_control->set_max_speed_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up()); pos_control->set_max_accel_z(wp_nav->get_accel_z()); // pilot always controls yaw @@ -148,7 +148,7 @@ void Copter::ModeGuided::angle_control_start() guided_mode = Guided_Angle; // set vertical speed and acceleration - pos_control->set_max_speed_z(wp_nav->get_speed_down(), wp_nav->get_speed_up()); + pos_control->set_max_speed_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up()); pos_control->set_max_accel_z(wp_nav->get_accel_z()); // initialise position and desired velocity @@ -610,7 +610,7 @@ void Copter::ModeGuided::angle_control_run() float yaw_rate_in = wrap_180_cd(guided_angle_state.yaw_rate_cds); // constrain climb rate - float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav->get_speed_down()), wp_nav->get_speed_up()); + float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav->get_default_speed_down()), wp_nav->get_default_speed_up()); // get avoidance adjusted climb rate climb_rate_cms = get_avoidance_adjusted_climbrate(climb_rate_cms); diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index 38f8e1d01a..8340abf63e 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -19,7 +19,7 @@ bool Copter::ModeLand::init(bool ignore_checks) } // initialize vertical speeds and leash lengths - pos_control->set_max_speed_z(wp_nav->get_speed_down(), wp_nav->get_speed_up()); + pos_control->set_max_speed_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up()); pos_control->set_max_accel_z(wp_nav->get_accel_z()); // initialise position and desired velocity diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index 25fa9eefbb..29747ba004 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -51,7 +51,7 @@ bool Copter::Mode::do_user_takeoff(float takeoff_alt_cm, bool must_navigate) void Copter::Mode::_TakeOff::start(float alt_cm) { // calculate climb rate - const float speed = MIN(copter.wp_nav->get_speed_up(), MAX(copter.g.pilot_speed_up*2.0f/3.0f, copter.g.pilot_speed_up-50.0f)); + const float speed = MIN(copter.wp_nav->get_default_speed_up(), MAX(copter.g.pilot_speed_up*2.0f/3.0f, copter.g.pilot_speed_up-50.0f)); // sanity check speed and target if (running() || speed <= 0.0f || alt_cm <= 0.0f) {