Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-11 10:23:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: integrate AC_WPNav::get_speed rename to get_default_speed

Browse Source
This commit is contained in:
Randy Mackay 2019-01-24 14:01:46 +09:00
parent cde2964f05
commit 717fb4d823
7 changed files with 21 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/GCS_Mavlink.cpp
View File

@ -1082,10 +1082,10 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
climb_rate_cms = 0.0f; climb_rate_cms = 0.0f;
} else if (packet.thrust > 0.5f) { } else if (packet.thrust > 0.5f) {
// climb at up to WPNAV_SPEED_UP // 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 { } else {
// descend at up to WPNAV_SPEED_DN // 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 // if the body_yaw_rate field is ignored, use the commanded yaw position

16
ArduCopter/avoidance_adsb.cpp
View File

@ -175,9 +175,9 @@ bool AP_Avoidance_Copter::handle_avoidance_vertical(const AP_Avoidance::Obstacle
// get best vector away from obstacle // get best vector away from obstacle
Vector3f velocity_neu; Vector3f velocity_neu;
if (should_climb) { if (should_climb) {
velocity_neu.z = copter.wp_nav->get_speed_up(); velocity_neu.z = copter.wp_nav->get_default_speed_up();
} else { } 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 // do not descend if below RTL alt
if (copter.current_loc.alt < copter.g.rtl_altitude) { if (copter.current_loc.alt < copter.g.rtl_altitude) {
velocity_neu.z = 0.0f; velocity_neu.z = 0.0f;
@ -208,8 +208,8 @@ bool AP_Avoidance_Copter::handle_avoidance_horizontal(const AP_Avoidance::Obstac
// re-normalise // re-normalise
velocity_neu.normalize(); velocity_neu.normalize();
// convert horizontal components to velocities // convert horizontal components to velocities
velocity_neu.x *= copter.wp_nav->get_speed_xy(); velocity_neu.x *= copter.wp_nav->get_default_speed_xy();
velocity_neu.y *= copter.wp_nav->get_speed_xy(); velocity_neu.y *= copter.wp_nav->get_default_speed_xy();
// send target velocity // send target velocity
copter.mode_avoid_adsb.set_velocity(velocity_neu); copter.mode_avoid_adsb.set_velocity(velocity_neu);
return true; return true;
@ -230,13 +230,13 @@ bool AP_Avoidance_Copter::handle_avoidance_perpendicular(const AP_Avoidance::Obs
Vector3f velocity_neu; Vector3f velocity_neu;
if (get_vector_perpendicular(obstacle, velocity_neu)) { if (get_vector_perpendicular(obstacle, velocity_neu)) {
// convert horizontal components to velocities // convert horizontal components to velocities
velocity_neu.x *= copter.wp_nav->get_speed_xy(); velocity_neu.x *= copter.wp_nav->get_default_speed_xy();
velocity_neu.y *= copter.wp_nav->get_speed_xy(); velocity_neu.y *= copter.wp_nav->get_default_speed_xy();
// use up and down waypoint speeds // use up and down waypoint speeds
if (velocity_neu.z > 0.0f) { 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 { } 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 // do not descend if below RTL alt
if (copter.current_loc.alt < copter.g.rtl_altitude) { if (copter.current_loc.alt < copter.g.rtl_altitude) {
velocity_neu.z = 0.0f; velocity_neu.z = 0.0f;

4
ArduCopter/mode_auto.cpp
View File

@ -1217,8 +1217,8 @@ void Copter::ModeAuto::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
loiter_to_alt.alt_error_cm = 0; loiter_to_alt.alt_error_cm = 0;
pos_control->set_max_accel_z(wp_nav->get_accel_z()); pos_control->set_max_accel_z(wp_nav->get_accel_z());
pos_control->set_max_speed_z(wp_nav->get_speed_down(), pos_control->set_max_speed_z(wp_nav->get_default_speed_down(),
wp_nav->get_speed_up()); wp_nav->get_default_speed_up());
if (pos_control->is_active_z()) { if (pos_control->is_active_z()) {
pos_control->freeze_ff_z(); pos_control->freeze_ff_z();

4
ArduCopter/mode_circle.cpp
View File

@ -13,7 +13,7 @@ bool Copter::ModeCircle::init(bool ignore_checks)
pilot_yaw_override = false; pilot_yaw_override = false;
// initialize speeds and accelerations // 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_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_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_max_accel_z(g.pilot_accel_z); pos_control->set_max_accel_z(g.pilot_accel_z);
@ -35,7 +35,7 @@ void Copter::ModeCircle::run()
float target_climb_rate = 0; float target_climb_rate = 0;
// initialize speeds and accelerations // 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_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_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_max_accel_z(g.pilot_accel_z); pos_control->set_max_accel_z(g.pilot_accel_z);

10
ArduCopter/mode_guided.cpp
View File

@ -103,7 +103,7 @@ void Copter::ModeGuided::vel_control_start()
guided_mode = Guided_Velocity; guided_mode = Guided_Velocity;
// initialise horizontal speed, acceleration // 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()); pos_control->set_max_accel_xy(wp_nav->get_wp_acceleration());
// initialize vertical speeds and acceleration // initialize vertical speeds and acceleration
@ -123,7 +123,7 @@ void Copter::ModeGuided::posvel_control_start()
pos_control->init_xy_controller(); pos_control->init_xy_controller();
// set speed and acceleration from wpnav's speed and acceleration // 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()); pos_control->set_max_accel_xy(wp_nav->get_wp_acceleration());
const Vector3f& curr_pos = inertial_nav.get_position(); 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); pos_control->set_desired_velocity_xy(curr_vel.x, curr_vel.y);
// set vertical speed and acceleration // 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()); pos_control->set_max_accel_z(wp_nav->get_accel_z());
// pilot always controls yaw // pilot always controls yaw
@ -148,7 +148,7 @@ void Copter::ModeGuided::angle_control_start()
guided_mode = Guided_Angle; guided_mode = Guided_Angle;
// set vertical speed and acceleration // 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()); pos_control->set_max_accel_z(wp_nav->get_accel_z());
// initialise position and desired velocity // 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); float yaw_rate_in = wrap_180_cd(guided_angle_state.yaw_rate_cds);
// constrain climb rate // 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 // get avoidance adjusted climb rate
climb_rate_cms = get_avoidance_adjusted_climbrate(climb_rate_cms); climb_rate_cms = get_avoidance_adjusted_climbrate(climb_rate_cms);

2
ArduCopter/mode_land.cpp
View File

@ -19,7 +19,7 @@ bool Copter::ModeLand::init(bool ignore_checks)
} }
// initialize vertical speeds and leash lengths // 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()); pos_control->set_max_accel_z(wp_nav->get_accel_z());
// initialise position and desired velocity // initialise position and desired velocity

2
ArduCopter/takeoff.cpp
View File

@ -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) void Copter::Mode::_TakeOff::start(float alt_cm)
{ {
// calculate climb rate // 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 // sanity check speed and target
if (running() || speed <= 0.0f || alt_cm <= 0.0f) { if (running() || speed <= 0.0f || alt_cm <= 0.0f) {