From f2fc39943edd4e4453c9b13c38d3a6156533fc9c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 3 Feb 2016 14:36:48 +0900 Subject: [PATCH] AP_MotorsHeli_Single: remove unused delta_phase_angle --- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 21 ++++++-------------- libraries/AP_Motors/AP_MotorsHeli_Single.h | 4 ---- 2 files changed, 6 insertions(+), 19 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 785d7127c1..2099bdaf10 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -270,14 +270,14 @@ void AP_MotorsHeli_Single::calculate_roll_pitch_collective_factors() if (_swash_type == AP_MOTORS_HELI_SINGLE_SWASH_CCPM) { //CCPM Swashplate, perform control mixing // roll factors - _rollFactor[CH_1] = cosf(radians(_servo1_pos + 90 - (_phase_angle + _delta_phase_angle))); - _rollFactor[CH_2] = cosf(radians(_servo2_pos + 90 - (_phase_angle + _delta_phase_angle))); - _rollFactor[CH_3] = cosf(radians(_servo3_pos + 90 - (_phase_angle + _delta_phase_angle))); + _rollFactor[CH_1] = cosf(radians(_servo1_pos + 90 - _phase_angle)); + _rollFactor[CH_2] = cosf(radians(_servo2_pos + 90 - _phase_angle)); + _rollFactor[CH_3] = cosf(radians(_servo3_pos + 90 - _phase_angle)); // pitch factors - _pitchFactor[CH_1] = cosf(radians(_servo1_pos - (_phase_angle + _delta_phase_angle))); - _pitchFactor[CH_2] = cosf(radians(_servo2_pos - (_phase_angle + _delta_phase_angle))); - _pitchFactor[CH_3] = cosf(radians(_servo3_pos - (_phase_angle + _delta_phase_angle))); + _pitchFactor[CH_1] = cosf(radians(_servo1_pos - _phase_angle)); + _pitchFactor[CH_2] = cosf(radians(_servo2_pos - _phase_angle)); + _pitchFactor[CH_3] = cosf(radians(_servo3_pos - _phase_angle)); // collective factors _collectiveFactor[CH_1] = 1; @@ -330,15 +330,6 @@ void AP_MotorsHeli_Single::update_motor_control(RotorControlState state) _heliflags.rotor_runup_complete = ( _main_rotor.is_runup_complete() && _tail_rotor.is_runup_complete() ); } -// set_delta_phase_angle for setting variable phase angle compensation and force -// recalculation of collective factors -void AP_MotorsHeli_Single::set_delta_phase_angle(int16_t angle) -{ - angle = constrain_int16(angle, -90, 90); - _delta_phase_angle = angle; - calculate_roll_pitch_collective_factors(); -} - // // move_actuators - moves swash plate and tail rotor // - expected ranges: diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index 13b0fa89b2..112af8ca5f 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -114,10 +114,6 @@ public: // supports_yaw_passthrought - returns true if we support yaw passthrough bool supports_yaw_passthrough() const { return _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO; } - // set_delta_phase_angle for setting variable phase angle compensation and force - // recalculation of collective factors - void set_delta_phase_angle(int16_t angle); - void set_acro_tail(bool set) { _acro_tail = set; } // servo_test - move servos through full range of movement