From b11600aebe5fedcdab6a65bc0dc4d9a16b564196 Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Tue, 11 Aug 2015 20:37:56 -0400 Subject: [PATCH] AP_MotorsHeli: Move set_delta_phase_angle into _Single class --- libraries/AP_Motors/AP_MotorsHeli.cpp | 9 --------- libraries/AP_Motors/AP_MotorsHeli.h | 6 +++--- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 9 +++++++++ libraries/AP_Motors/AP_MotorsHeli_Single.h | 4 ++++ 4 files changed, 16 insertions(+), 12 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 711e33fb03..2387da7ef0 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -288,15 +288,6 @@ void AP_MotorsHeli::init_swash_servo(RC_Channel& servo) servo.radio_max = 2000; } -// set_delta_phase_angle for setting variable phase angle compensation and force -// recalculation of collective factors -void AP_MotorsHeli::set_delta_phase_angle(int16_t angle) -{ - angle = constrain_int16(angle, -90, 90); - _delta_phase_angle = angle; - calculate_roll_pitch_collective_factors(); -} - // update the throttle input filter void AP_MotorsHeli::update_throttle_filter() { diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index a4500df872..803f44d1a4 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -150,9 +150,9 @@ public: // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; - // set_delta_phase_angle for setting variable phase angle compensation and force - // recalculation of collective factors - void set_delta_phase_angle(int16_t angle); + // set_delta_phase_angle for setting variable phase angle compensation and force recalculation of collective factors + // ignored unless overloaded by child classes + virtual void set_delta_phase_angle(int16_t angle){}; // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used) // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 16c31c9fcc..f2bab5d56a 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -428,6 +428,15 @@ void AP_MotorsHeli_Single::calculate_roll_pitch_collective_factors() } } +// 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(); +} + // // heli_move_swash - moves swash plate to attitude of parameters passed in // - expected ranges: diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index edcc4d265c..75671eae38 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -121,6 +121,10 @@ 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); // var_info static const struct AP_Param::GroupInfo var_info[];