From c1a9f75034fc8f0c64fb9bce9cfa5d42fe24357f Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 5 Jul 2022 04:08:56 +0100 Subject: [PATCH] AP_Motors: params always use set method --- libraries/AP_Motors/AP_MotorsHeli.cpp | 6 +++--- libraries/AP_Motors/AP_MotorsHeli_Dual.cpp | 12 ++++++------ libraries/AP_Motors/AP_MotorsHeli_Quad.cpp | 8 ++++---- libraries/AP_Motors/AP_MotorsHeli_RSC.h | 8 ++++---- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 10 +++++----- libraries/AP_Motors/AP_MotorsHeli_Single.h | 2 +- libraries/AP_Motors/AP_MotorsHeli_Swash.cpp | 4 ++-- libraries/AP_Motors/AP_MotorsMulticopter.cpp | 4 ++-- libraries/AP_Motors/AP_MotorsMulticopter.h | 2 +- libraries/AP_Motors/AP_MotorsTri.cpp | 2 +- 10 files changed, 29 insertions(+), 29 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index fdb8df9492..523872cf23 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -167,7 +167,7 @@ void AP_MotorsHeli::init(motor_frame_class frame_class, motor_frame_type frame_t _servo_test_cycle_counter = _servo_test; // ensure inputs are not passed through to servos on start-up - _servo_mode = SERVO_CONTROL_MODE_AUTOMATED; + _servo_mode.set(SERVO_CONTROL_MODE_AUTOMATED); // initialise radio passthrough for collective to middle _throttle_radio_passthrough = 0.5f; @@ -536,7 +536,7 @@ void AP_MotorsHeli::update_throttle_filter() // reset_flight_controls - resets all controls and scalars to flight status void AP_MotorsHeli::reset_flight_controls() { - _servo_mode = SERVO_CONTROL_MODE_AUTOMATED; + _servo_mode.set(SERVO_CONTROL_MODE_AUTOMATED); init_outputs(); calculate_scalars(); } @@ -564,7 +564,7 @@ void AP_MotorsHeli::update_throttle_hover(float dt) } // we have chosen to constrain the hover collective to be within the range reachable by the third order expo polynomial. - _collective_hover = constrain_float(_collective_hover + (dt / (dt + AP_MOTORS_HELI_COLLECTIVE_HOVER_TC)) * (curr_collective - _collective_hover), AP_MOTORS_HELI_COLLECTIVE_HOVER_MIN, AP_MOTORS_HELI_COLLECTIVE_HOVER_MAX); + _collective_hover.set(constrain_float(_collective_hover + (dt / (dt + AP_MOTORS_HELI_COLLECTIVE_HOVER_TC)) * (curr_collective - _collective_hover), AP_MOTORS_HELI_COLLECTIVE_HOVER_MIN, AP_MOTORS_HELI_COLLECTIVE_HOVER_MAX)); } } diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp index 527fa32270..cf279941ee 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp @@ -353,20 +353,20 @@ void AP_MotorsHeli_Dual::calculate_scalars() { // range check collective min, max and mid if( _collective_min >= _collective_max ) { - _collective_min = AP_MOTORS_HELI_COLLECTIVE_MIN; - _collective_max = AP_MOTORS_HELI_COLLECTIVE_MAX; + _collective_min.set(AP_MOTORS_HELI_COLLECTIVE_MIN); + _collective_max.set(AP_MOTORS_HELI_COLLECTIVE_MAX); } // range check collective min, max and mid for rear swashplate if( _collective2_min >= _collective2_max ) { - _collective2_min = AP_MOTORS_HELI_DUAL_COLLECTIVE2_MIN; - _collective2_max = AP_MOTORS_HELI_DUAL_COLLECTIVE2_MAX; + _collective2_min.set(AP_MOTORS_HELI_DUAL_COLLECTIVE2_MIN); + _collective2_max.set(AP_MOTORS_HELI_DUAL_COLLECTIVE2_MAX); } - _collective_zero_thrust_deg = constrain_float(_collective_zero_thrust_deg, _collective_min_deg, _collective_max_deg); + _collective_zero_thrust_deg.set(constrain_float(_collective_zero_thrust_deg, _collective_min_deg, _collective_max_deg)); - _collective_land_min_deg = constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg); + _collective_land_min_deg.set(constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg)); if (!is_equal((float)_collective_max_deg, (float)_collective_min_deg)) { // calculate collective zero thrust point as a number from 0 to 1 diff --git a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp index 690f602eda..7d44022640 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp @@ -132,13 +132,13 @@ void AP_MotorsHeli_Quad::calculate_scalars() { // range check collective min, max and mid if( _collective_min >= _collective_max ) { - _collective_min = AP_MOTORS_HELI_COLLECTIVE_MIN; - _collective_max = AP_MOTORS_HELI_COLLECTIVE_MAX; + _collective_min.set(AP_MOTORS_HELI_COLLECTIVE_MIN); + _collective_max.set(AP_MOTORS_HELI_COLLECTIVE_MAX); } - _collective_zero_thrust_deg = constrain_float(_collective_zero_thrust_deg, _collective_min_deg, _collective_max_deg); + _collective_zero_thrust_deg.set(constrain_float(_collective_zero_thrust_deg, _collective_min_deg, _collective_max_deg)); - _collective_land_min_deg = constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg); + _collective_land_min_deg.set(constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg)); if (!is_equal((float)_collective_max_deg, (float)_collective_min_deg)) { // calculate collective zero thrust point as a number from 0 to 1 diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h index eb4bc82344..4af13b6d21 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h @@ -72,7 +72,7 @@ public: uint8_t get_control_mode() const { return _control_mode; } // set_critical_speed - void set_critical_speed(float critical_speed) { _critical_speed = critical_speed; } + void set_critical_speed(float critical_speed) { _critical_speed.set(critical_speed); } // get_desired_speed float get_desired_speed() const { return _desired_speed; } @@ -88,13 +88,13 @@ public: float get_governor_output() const { return _governor_output; } void governor_reset(); float get_control_output() const { return _control_output; } - void set_idle_output(float idle_output) { _idle_output = idle_output; } + void set_idle_output(float idle_output) { _idle_output.set(idle_output); } void autothrottle_run(); void set_throttle_curve(); // functions for ramp and runup timers, runup_complete flag - void set_ramp_time(int8_t ramp_time) { _ramp_time = ramp_time; } - void set_runup_time(int8_t runup_time) { _runup_time = runup_time; } + void set_ramp_time(int8_t ramp_time) { _ramp_time.set(ramp_time); } + void set_runup_time(int8_t runup_time) { _runup_time.set(runup_time); } bool is_runup_complete() const { return _runup_complete; } // is_spooldown_complete diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index a226aa6f02..3c89f519cd 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -310,13 +310,13 @@ void AP_MotorsHeli_Single::calculate_scalars() { // range check collective min, max and zero if( _collective_min >= _collective_max ) { - _collective_min = AP_MOTORS_HELI_COLLECTIVE_MIN; - _collective_max = AP_MOTORS_HELI_COLLECTIVE_MAX; + _collective_min.set(AP_MOTORS_HELI_COLLECTIVE_MIN); + _collective_max.set(AP_MOTORS_HELI_COLLECTIVE_MAX); } - _collective_zero_thrust_deg = constrain_float(_collective_zero_thrust_deg, _collective_min_deg, _collective_max_deg); + _collective_zero_thrust_deg.set(constrain_float(_collective_zero_thrust_deg, _collective_min_deg, _collective_max_deg)); - _collective_land_min_deg = constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg); + _collective_land_min_deg.set(constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg)); if (!is_equal((float)_collective_max_deg, (float)_collective_min_deg)) { // calculate collective zero thrust point as a number from 0 to 1 @@ -467,7 +467,7 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float // also not required if we are using external gyro if ((_main_rotor.get_control_output() > _main_rotor.get_idle_output()) && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { // sanity check collective_yaw_effect - _collective_yaw_effect = constrain_float(_collective_yaw_effect, -AP_MOTORS_HELI_SINGLE_COLYAW_RANGE, AP_MOTORS_HELI_SINGLE_COLYAW_RANGE); + _collective_yaw_effect.set(constrain_float(_collective_yaw_effect, -AP_MOTORS_HELI_SINGLE_COLYAW_RANGE, AP_MOTORS_HELI_SINGLE_COLYAW_RANGE)); // the 4.5 scaling factor is to bring the values in line with previous releases yaw_offset = _collective_yaw_effect * fabsf(collective_out - _collective_zero_thrust_pct) / 4.5f; } diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index 386033572c..c9c0d76739 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -82,7 +82,7 @@ public: uint32_t get_motor_mask() override; // ext_gyro_gain - set external gyro gain in range 0 ~ 1000 - void ext_gyro_gain(float gain) override { if (gain >= 0 && gain <= 1000) { _ext_gyro_gain_std = gain; }} + void ext_gyro_gain(float gain) override { if (gain >= 0 && gain <= 1000) { _ext_gyro_gain_std.set(gain); }} // has_flybar - returns true if we have a mechical flybar bool has_flybar() const override { return _flybar_mode; } diff --git a/libraries/AP_Motors/AP_MotorsHeli_Swash.cpp b/libraries/AP_Motors/AP_MotorsHeli_Swash.cpp index be325bad3d..6b92fcb27e 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Swash.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Swash.cpp @@ -94,9 +94,9 @@ void AP_MotorsHeli_Swash::configure() _collective_direction = static_cast(_swash_coll_dir.get()); _make_servo_linear = _linear_swash_servo; if (_swash_type == SWASHPLATE_TYPE_H3) { - enable = 1; + enable.set(1); } else { - enable = 0; + enable.set(0); } } diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index e7ee8631b6..914331e263 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -383,7 +383,7 @@ void AP_MotorsMulticopter::update_lift_max_from_batt_voltage() return; } - _batt_voltage_min = MAX(_batt_voltage_min, _batt_voltage_max * 0.6f); + _batt_voltage_min.set(MAX(_batt_voltage_min, _batt_voltage_max * 0.6f)); // contrain resting voltage estimate (resting voltage is actual voltage with sag removed based on current draw and resistance) _batt_voltage_resting_estimate = constrain_float(_batt_voltage_resting_estimate, _batt_voltage_min, _batt_voltage_max); @@ -529,7 +529,7 @@ void AP_MotorsMulticopter::update_throttle_hover(float dt) { if (_throttle_hover_learn != HOVER_LEARN_DISABLED) { // we have chosen to constrain the hover throttle to be within the range reachable by the third order expo polynomial. - _throttle_hover = constrain_float(_throttle_hover + (dt / (dt + AP_MOTORS_THST_HOVER_TC)) * (get_throttle() - _throttle_hover), AP_MOTORS_THST_HOVER_MIN, AP_MOTORS_THST_HOVER_MAX); + _throttle_hover.set(constrain_float(_throttle_hover + (dt / (dt + AP_MOTORS_THST_HOVER_TC)) * (get_throttle() - _throttle_hover), AP_MOTORS_THST_HOVER_MIN, AP_MOTORS_THST_HOVER_MAX)); } } diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index 5c3e583fce..0c05f04b1c 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -42,7 +42,7 @@ public: void output_min() override; // set_yaw_headroom - set yaw headroom (yaw is given at least this amount of pwm) - void set_yaw_headroom(int16_t pwm) { _yaw_headroom = pwm; } + void set_yaw_headroom(int16_t pwm) { _yaw_headroom.set(pwm); } // update_throttle_range - update throttle endpoints void update_throttle_range(); diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index bc94fb0f70..ab734a8b15 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -159,7 +159,7 @@ void AP_MotorsTri::output_armed_stabilizing() SRV_Channels::set_angle(SRV_Channels::get_motor_function(AP_MOTORS_CH_TRI_YAW), _yaw_servo_angle_max_deg*100); // sanity check YAW_SV_ANGLE parameter value to avoid divide by zero - _yaw_servo_angle_max_deg = constrain_float(_yaw_servo_angle_max_deg, AP_MOTORS_TRI_SERVO_RANGE_DEG_MIN, AP_MOTORS_TRI_SERVO_RANGE_DEG_MAX); + _yaw_servo_angle_max_deg.set(constrain_float(_yaw_servo_angle_max_deg, AP_MOTORS_TRI_SERVO_RANGE_DEG_MIN, AP_MOTORS_TRI_SERVO_RANGE_DEG_MAX)); // apply voltage and air pressure compensation const float compensation_gain = get_compensation_gain();