From 88a1a0e861a061ed1c7613f65da63d73ea3f7359 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 7 Sep 2021 12:53:02 +1000 Subject: [PATCH] Sub: no need to fabs() get_default_speed_down() as it does fabs already --- ArduSub/GCS_Mavlink.cpp | 2 +- ArduSub/control_guided.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 9e1e8c3f0d..99cefea08d 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -576,7 +576,7 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * sub.wp_nav.get_default_speed_up(); } else { // descend at up to WPNAV_SPEED_DN - climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * fabsf(sub.wp_nav.get_default_speed_down()); + climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * sub.wp_nav.get_default_speed_down(); } sub.guided_set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]), climb_rate_cms); break; diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp index cc933c8a76..f5128f7af8 100644 --- a/ArduSub/control_guided.cpp +++ b/ArduSub/control_guided.cpp @@ -481,7 +481,7 @@ void Sub::guided_angle_control_run() float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd); // constrain climb rate - 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()); + float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -wp_nav.get_default_speed_down(), wp_nav.get_default_speed_up()); // check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds uint32_t tnow = AP_HAL::millis();