Browse Source

Copter: no need to fabs() get_default_speed_down() as it does fabs already

zr-v5.1
Andrew Tridgell 3 years ago committed by Randy Mackay
parent
commit
291479c277
  1. 2
      ArduCopter/GCS_Mavlink.cpp
  2. 2
      ArduCopter/mode_guided.cpp

2
ArduCopter/GCS_Mavlink.cpp

@ -1082,7 +1082,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) @@ -1082,7 +1082,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
climb_rate_or_thrust = (packet.thrust - 0.5f) * 2.0f * copter.wp_nav->get_default_speed_up();
} else {
// descend at up to WPNAV_SPEED_DN
climb_rate_or_thrust = (0.5f - packet.thrust) * 2.0f * -fabsf(copter.wp_nav->get_default_speed_down());
climb_rate_or_thrust = (0.5f - packet.thrust) * 2.0f * -copter.wp_nav->get_default_speed_down();
}
}

2
ArduCopter/mode_guided.cpp

@ -820,7 +820,7 @@ void ModeGuided::angle_control_run() @@ -820,7 +820,7 @@ void ModeGuided::angle_control_run()
float climb_rate_cms = 0.0f;
if (!guided_angle_state.use_thrust) {
// constrain climb rate
climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav->get_default_speed_down()), wp_nav->get_default_speed_up());
climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -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);

Loading…
Cancel
Save