From 8b917b82ee84331521e57a001afe13ef1d6d5438 Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Thu, 18 Jun 2015 15:23:51 -0400 Subject: [PATCH] AP_MotorsHeli: Simplify set_desired_rotor_speed function definition --- libraries/AP_Motors/AP_MotorsHeli.cpp | 14 ++++---------- libraries/AP_Motors/AP_MotorsHeli.h | 6 +++--- 2 files changed, 7 insertions(+), 13 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 08d7312b3c..f128aac314 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -348,12 +348,6 @@ bool AP_MotorsHeli::allow_arming() const return true; } -// set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1000 -void AP_MotorsHeli::set_desired_rotor_speed(int16_t desired_speed) -{ - _rotor_desired = desired_speed; -} - // return true if the main rotor is up to speed bool AP_MotorsHeli::rotor_runup_complete() const { @@ -616,7 +610,7 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll // rudder feed forward based on collective // the feed-forward is not required when the motor is shut down and not creating torque // also not required if we are using external gyro - if ((_rotor_desired > 0) && _tail_type != AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) { + if ((_desired_rotor_speed > 0) && _tail_type != AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) { // sanity check collective_yaw_effect _collective_yaw_effect = constrain_float(_collective_yaw_effect, -AP_MOTOR_HELI_COLYAW_RANGE, AP_MOTOR_HELI_COLYAW_RANGE); yaw_offset = _collective_yaw_effect * abs(_collective_out - _collective_mid_pwm); @@ -682,12 +676,12 @@ void AP_MotorsHeli::rsc_control() } // ramp up or down main rotor and tail - if (_rotor_desired > 0) { + if (_desired_rotor_speed > 0) { // ramp up tail rotor (this does nothing if not using direct drive variable pitch tail) tail_ramp(_direct_drive_tailspeed); // note: this always returns true if not using direct drive variable pitch tail if (tail_rotor_runup_complete()) { - rotor_ramp(_rotor_desired); + rotor_ramp(_desired_rotor_speed); } }else{ // shutting down main rotor @@ -699,7 +693,7 @@ void AP_MotorsHeli::rsc_control() // direct drive fixed pitch tail servo gets copy of yaw servo out (ch4) while main rotor is running if (_tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_FIXEDPITCH) { // output fixed-pitch speed control if Ch8 is high - if (_rotor_desired > 0 || _rotor_speed_estimate > 0) { + if (_desired_rotor_speed > 0 || _rotor_speed_estimate > 0) { // copy yaw output to tail esc write_aux(_servo_4.servo_out); }else{ diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index fbaf5930a5..ddeb33618b 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -107,7 +107,7 @@ public: _collective_scalar_manual(1), _collective_out(0), _collective_mid_pwm(0), - _rotor_desired(0), + _desired_rotor_speed(0), _rotor_out(0), _rsc_ramp_increment(0.0f), _rsc_runup_increment(0.0f), @@ -174,7 +174,7 @@ public: int16_t get_rsc_setpoint() const { return _rsc_setpoint; } // set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1000 - void set_desired_rotor_speed(int16_t desired_speed); + void set_desired_rotor_speed(int16_t desired_speed) { _desired_rotor_speed = desired_speed; } // return true if the main rotor is up to speed bool rotor_runup_complete() const; @@ -297,7 +297,7 @@ private: float _collective_scalar_manual; // collective scalar to reduce the range of the collective movement while collective is being controlled manually (i.e. directly by the pilot) int16_t _collective_out; // actual collective pitch value. Required by the main code for calculating cruise throttle int16_t _collective_mid_pwm; // collective mid parameter value converted to pwm form (i.e. 0 ~ 1000) - int16_t _rotor_desired; // latest desired rotor speed from pilot + int16_t _desired_rotor_speed; // latest desired rotor speed from pilot float _rotor_out; // latest output sent to the main rotor or an estimate of the rotors actual speed (whichever is higher) (0 ~ 1000) float _rsc_ramp_increment; // the amount we can increase the rotor output during each 100hz iteration float _rsc_runup_increment; // the amount we can increase the rotor's estimated speed during each 100hz iteration