From 14700063d248fb4cdecedb815d4f6388f848faae Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Fri, 11 Mar 2022 19:03:13 -0600 Subject: [PATCH] ArduPlane: convert floating point divides into multiplys --- ArduPlane/navigation.cpp | 2 +- ArduPlane/quadplane.cpp | 6 +++--- ArduPlane/servos.cpp | 2 +- ArduPlane/system.cpp | 2 +- ArduPlane/tiltrotor.cpp | 26 +++++++++++++------------- 5 files changed, 19 insertions(+), 19 deletions(-) diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 1f2c8b744e..fadfcb62a6 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -380,7 +380,7 @@ void Plane::update_fbwb_speed_height(void) target_altitude.last_elev_check_us = now; - float elevator_input = channel_pitch->get_control_in() / 4500.0f; + float elevator_input = channel_pitch->get_control_in() * (1/4500.0); if (g.flybywire_elev_reverse) { elevator_input = -elevator_input; diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 819fe2b6ba..37adb00201 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1202,7 +1202,7 @@ float QuadPlane::get_pilot_input_yaw_rate_cds(void) const // must have a non-zero max yaw rate for scaling to work max_rate = (yaw_rate_max < 1.0f) ? 1 : yaw_rate_max; } - return plane.channel_rudder->get_control_in() * max_rate / 45; + return plane.channel_rudder->get_control_in() * max_rate * (1/45.0); } /* @@ -3206,7 +3206,7 @@ void QuadPlane::Log_Write_QControl_Tuning() float des_alt_m = 0.0f; int16_t target_climb_rate_cms = 0; if (plane.control_mode != &plane.mode_qstabilize) { - des_alt_m = pos_control->get_pos_target_z_cm() / 100.0f; + des_alt_m = pos_control->get_pos_target_z_cm() * 0.01f; target_climb_rate_cms = pos_control->get_vel_target_z_cms(); } @@ -3391,7 +3391,7 @@ float QuadPlane::get_weathervane_yaw_rate_cds(void) wp_nav->get_pitch(), is_takeoff, in_vtol_land_sequence())) { - return constrain_float(wv_output / 45, -100.0, 100.0) * yaw_rate_max * 0.5; + return constrain_float(wv_output * (1/45.0), -100.0, 100.0) * yaw_rate_max * 0.5; } return 0.0; diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 2e52219587..9511c18853 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -710,7 +710,7 @@ void Plane::set_landing_gear(void) void Plane::servos_twin_engine_mix(void) { float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle); - float rud_gain = float(plane.g2.rudd_dt_gain) / 100; + float rud_gain = float(plane.g2.rudd_dt_gain) * 0.01f; rudder_dt = rud_gain * SRV_Channels::get_output_scaled(SRV_Channel::k_rudder) / SERVO_MAX; #if ADVANCED_FAILSAFE == ENABLED diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 205dda4445..1c4570e3c0 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -474,7 +474,7 @@ void Plane::update_dynamic_notch() float rpm; if (rpm_sensor.get_rpm(0, rpm)) { // set the harmonic notch filter frequency from the main rotor rpm - ins.update_harmonic_notch_freq_hz(MAX(ref_freq, rpm * ref / 60.0f)); + ins.update_harmonic_notch_freq_hz(MAX(ref_freq, rpm * ref * (1/60.0))); } else { ins.update_harmonic_notch_freq_hz(ref_freq); } diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index 3f0660d166..fc8561f4c5 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -175,7 +175,7 @@ float Tiltrotor::tilt_max_change(bool up, bool in_flap_range) const rate = MAX(rate, 90); } } - return rate * plane.G_Dt / 90.0f; + return rate * plane.G_Dt * (1/90.0); } /* @@ -196,13 +196,13 @@ void Tiltrotor::slew(float newtilt) // tilt wings can sustain forward flight with some amount of wing tilt float Tiltrotor::get_fully_forward_tilt() const { - return 1.0 - (flap_angle_deg / 90.0); + return 1.0 - (flap_angle_deg * (1/90.0)); } // return the target tilt value for forward flight float Tiltrotor::get_forward_flight_tilt() const { - return 1.0 - ((flap_angle_deg / 90.0) * SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_flap_auto) * 0.01); + return 1.0 - ((flap_angle_deg * (1/90.0)) * SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_flap_auto) * 0.01); } /* @@ -305,8 +305,8 @@ void Tiltrotor::continuous_update(void) // Q_TILT_MAX. Anything above 50% throttle gets // Q_TILT_MAX. Below 50% throttle we decrease linearly. This // relies heavily on Q_VFWD_GAIN being set appropriately. - float settilt = constrain_float((SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)-MAX(plane.aparm.throttle_min.get(),0)) / 50.0f, 0, 1); - slew(MIN(settilt * max_angle_deg / 90.0f, get_forward_flight_tilt())); + float settilt = constrain_float((SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)-MAX(plane.aparm.throttle_min.get(),0)) * 0.02, 0, 1); + slew(MIN(settilt * max_angle_deg * (1/90.0), get_forward_flight_tilt())); } } @@ -527,9 +527,9 @@ void Tiltrotor::vectoring(void) // base the tilt on elevon mixing, which means it // takes account of the MIXING_GAIN. The rear tilt is // based on elevator - const float right = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_right) / 4500.0; - const float left = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_left) / 4500.0; - const float mid = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) / 4500.0; + const float right = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_right) * (1/4500.0); + const float left = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_left) * (1/4500.0); + const float mid = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) * (1/4500.0); // front tilt is effective canards, so need to swap and use negative. Rear motors are treated live elevons. SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft,1000 * constrain_float(base_output - right,0,1)); SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight,1000 * constrain_float(base_output - left,0,1)); @@ -547,9 +547,9 @@ void Tiltrotor::vectoring(void) // we don't want tilt impacted by airspeed const float scaler = plane.control_mode == &plane.mode_manual?1:(quadplane.FW_vector_throttle_scaling() / plane.get_speed_scaler()); const float gain = fixed_gain * fixed_tilt_limit * scaler; - const float right = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_right) / 4500.0; - const float left = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_left) / 4500.0; - const float mid = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) / 4500.0; + const float right = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_right) * (1/4500.0); + const float left = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_left) * (1/4500.0); + const float mid = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) * (1/4500.0); SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft,1000 * constrain_float(base_output - right,0,1)); SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight,1000 * constrain_float(base_output - left,0,1)); SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearLeft,1000 * constrain_float(base_output + left,0,1)); @@ -607,10 +607,10 @@ void Tiltrotor::bicopter_output(void) float tilt_right = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorRight); if (is_negative(tilt_left)) { - tilt_left *= tilt_yaw_angle / 90.0f; + tilt_left *= tilt_yaw_angle * (1/90.0); } if (is_negative(tilt_right)) { - tilt_right *= tilt_yaw_angle / 90.0f; + tilt_right *= tilt_yaw_angle * (1/90.0); } // reduce authority of bicopter as motors are tilted forwards