aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AR_Motors: Change from division to multiplication

Browse Source
This commit is contained in:
murata 2022-03-12 02:43:01 +09:00 committed by Andrew Tridgell
parent bb72f91dda
commit 82afaf70f6
1 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AR_Motors/AP_MotorsUGV.cpp
View File

@ -634,7 +634,7 @@ void AP_MotorsUGV::output_regular(bool armed, float ground_speed, float steering
// normalise desired steering and throttle to ease calculations // normalise desired steering and throttle to ease calculations
float steering_norm = steering / 4500.0f; 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) // 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)); 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 // skid steering mixer
float steering_scaled = steering / 4500.0f; // steering scaled -1 to +1 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 // apply constraints
steering_scaled = constrain_float(steering_scaled, -1.0f, 1.0f); 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); steering = constrain_float(steering, -4500.0f, 4500.0f);
// scale throttle, steering and lateral inputs to -1 to 1 // 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_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_out[AP_MOTORS_NUM_MOTORS_MAX];
float thr_str_ltr_max = 1; float thr_str_ltr_max = 1;
@ -956,9 +956,9 @@ float AP_MotorsUGV::get_scaled_throttle(float throttle) const
// scale using throttle_min // scale using throttle_min
if (_throttle_min > 0) { if (_throttle_min > 0) {
if (is_negative(throttle)) { 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 { } 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 // calculate scaler
const float sign = (throttle < 0.0f) ? -1.0f : 1.0f; 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); 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);
} }