From 82afaf70f66fd94c494a1fe74a9cee22f140580c Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 12 Mar 2022 02:43:01 +0900 Subject: [PATCH] AR_Motors: Change from division to multiplication --- libraries/AR_Motors/AP_MotorsUGV.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/AR_Motors/AP_MotorsUGV.cpp b/libraries/AR_Motors/AP_MotorsUGV.cpp index 4ae276adc7..98bf28c40a 100644 --- a/libraries/AR_Motors/AP_MotorsUGV.cpp +++ b/libraries/AR_Motors/AP_MotorsUGV.cpp @@ -634,7 +634,7 @@ void AP_MotorsUGV::output_regular(bool armed, float ground_speed, float steering // normalise desired steering and throttle to ease calculations float steering_norm = steering / 4500.0f; - const float throttle_norm = throttle / 100.0f; + const float throttle_norm = throttle * 0.01f; // steering can never be more than throttle * tan(_vector_angle_max) const float vector_angle_max_rad = radians(constrain_float(_vector_angle_max, 0.0f, 90.0f)); @@ -739,7 +739,7 @@ void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float thrott // skid steering mixer float steering_scaled = steering / 4500.0f; // steering scaled -1 to +1 - float throttle_scaled = throttle / 100.0f; // throttle scaled -1 to +1 + float throttle_scaled = throttle * 0.01f; // throttle scaled -1 to +1 // apply constraints steering_scaled = constrain_float(steering_scaled, -1.0f, 1.0f); @@ -800,9 +800,9 @@ void AP_MotorsUGV::output_omni(bool armed, float steering, float throttle, float steering = constrain_float(steering, -4500.0f, 4500.0f); // scale throttle, steering and lateral inputs to -1 to 1 - const float scaled_throttle = throttle / 100.0f; + const float scaled_throttle = throttle * 0.01f; const float scaled_steering = steering / 4500.0f; - const float scaled_lateral = lateral / 100.0f; + const float scaled_lateral = lateral * 0.01f; float thr_str_ltr_out[AP_MOTORS_NUM_MOTORS_MAX]; float thr_str_ltr_max = 1; @@ -956,9 +956,9 @@ float AP_MotorsUGV::get_scaled_throttle(float throttle) const // scale using throttle_min if (_throttle_min > 0) { if (is_negative(throttle)) { - throttle = -_throttle_min + (throttle * ((100.0f - _throttle_min) / 100.0f)); + throttle = -_throttle_min + (throttle * ((100.0f - _throttle_min) * 0.01f)); } else { - throttle = _throttle_min + (throttle * ((100.0f - _throttle_min) / 100.0f)); + throttle = _throttle_min + (throttle * ((100.0f - _throttle_min) * 0.01f)); } } @@ -969,7 +969,7 @@ float AP_MotorsUGV::get_scaled_throttle(float throttle) const // calculate scaler const float sign = (throttle < 0.0f) ? -1.0f : 1.0f; - const float throttle_pct = constrain_float(throttle, -100.0f, 100.0f) / 100.0f; + const float throttle_pct = constrain_float(throttle, -100.0f, 100.0f) * 0.01f; return 100.0f * sign * ((_thrust_curve_expo - 1.0f) + safe_sqrt((1.0f - _thrust_curve_expo) * (1.0f - _thrust_curve_expo) + 4.0f * _thrust_curve_expo * fabsf(throttle_pct))) / (2.0f * _thrust_curve_expo); }