From 0df3af4e420a8167ca67c4384d377a7ec9109d9d Mon Sep 17 00:00:00 2001 From: Jolyon Saunders Date: Fri, 19 Sep 2014 11:54:26 +1000 Subject: [PATCH] AP_Motors: Circular swash-plate limits for AP_MotorsHeli --- libraries/AP_Motors/AP_MotorsHeli.cpp | 27 +++++++--------- libraries/AP_Motors/AP_MotorsHeli.h | 6 ++-- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 33 +++++++------------- 3 files changed, 24 insertions(+), 42 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 138132a0f1..070ae6ae94 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -27,23 +27,9 @@ extern const AP_HAL::HAL& hal; const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { - // @Param: ROL_MAX - // @DisplayName: Swash Roll Angle Max - // @Description: Maximum roll angle of the swash plate - // @Range: 0 18000 - // @Units: Centi-Degrees - // @Increment: 100 - // @User: Advanced - AP_GROUPINFO("ROL_MAX", 1, AP_MotorsHeli, _roll_max, AP_MOTORS_HELI_SWASH_ROLL_MAX), + // 1 was ROL_MAX which has been replaced by CYC_MAX - // @Param: PIT_MAX - // @DisplayName: Swash Pitch Angle Max - // @Description: Maximum pitch angle of the swash plate - // @Range: 0 18000 - // @Units: Centi-Degrees - // @Increment: 100 - // @User: Advanced - AP_GROUPINFO("PIT_MAX", 2, AP_MotorsHeli, _pitch_max, AP_MOTORS_HELI_SWASH_PITCH_MAX), + // 2 was PIT_MAX which has been replaced by CYC_MAX // @Param: COL_MIN // @DisplayName: Collective Pitch Minimum @@ -152,6 +138,15 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @User: Standard AP_GROUPINFO("RSC_POWER_HIGH", 15, AP_MotorsHeli, _rsc_power_high, AP_MOTORS_HELI_RSC_POWER_HIGH_DEFAULT), + // @Param: CYC_MAX + // @DisplayName: Cyclic Pitch Angle Max + // @Description: Maximum pitch angle of the swash plate + // @Range: 0 18000 + // @Units: Centi-Degrees + // @Increment: 100 + // @User: Advanced + AP_GROUPINFO("CYC_MAX", 16, AP_MotorsHeli, _cyclic_max, AP_MOTORS_HELI_SWASH_CYCLIC_MAX), + AP_GROUPEND }; diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index dfbd09e9e7..523cbbb4da 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -23,8 +23,7 @@ #define AP_MOTORS_HELI_SPEED_ANALOG_SERVOS 125 // update rate for analog servos // default swash min and max angles and positions -#define AP_MOTORS_HELI_SWASH_ROLL_MAX 2500 -#define AP_MOTORS_HELI_SWASH_PITCH_MAX 2500 +#define AP_MOTORS_HELI_SWASH_CYCLIC_MAX 2500 #define AP_MOTORS_HELI_COLLECTIVE_MIN 1250 #define AP_MOTORS_HELI_COLLECTIVE_MAX 1750 #define AP_MOTORS_HELI_COLLECTIVE_MID 1500 @@ -205,8 +204,7 @@ protected: } _heliflags; // parameters - AP_Int16 _roll_max; // Maximum roll angle of the swash plate in centi-degrees - AP_Int16 _pitch_max; // Maximum pitch angle of the swash plate in centi-degrees + AP_Int16 _cyclic_max; // Maximum cyclic angle of the swash plate in centi-degrees AP_Int16 _collective_min; // Lowest possible servo position for the swashplate AP_Int16 _collective_max; // Highest possible servo position for the swashplate AP_Int16 _collective_mid; // Swash servo position corresponding to zero collective pitch (or zero lift for Assymetrical blades) diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 2d97ab980b..94fe52fff3 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -233,8 +233,8 @@ void AP_MotorsHeli_Single::calculate_scalars() _collective_range = 1000 - _collective_mid_pwm; // determine roll, pitch and collective input scaling - _roll_scaler = (float)_roll_max/4500.0f; - _pitch_scaler = (float)_pitch_max/4500.0f; + _roll_scaler = (float)_cyclic_max/4500.0f; + _pitch_scaler = (float)_cyclic_max/4500.0f; _collective_scalar = ((float)(_collective_max-_collective_min))/1000.0f; // calculate factors based on swash type and servo position @@ -352,29 +352,18 @@ void AP_MotorsHeli_Single::move_actuators(int16_t roll_out, int16_t pitch_out, i limit.throttle_lower = false; limit.throttle_upper = false; - // rescale roll_out and pitch-out into the min and max ranges to provide linear motion + // rescale roll_out and pitch_out into the min and max ranges to provide linear motion // across the input range instead of stopping when the input hits the constrain value - // these calculations are based on an assumption of the user specified roll_max and pitch_max + // these calculations are based on an assumption of the user specified cyclic_max // coming into this equation at 4500 or less, and based on the original assumption of the // total _servo_x.servo_out range being -4500 to 4500. - roll_out = roll_out * _roll_scaler; - if (roll_out < -_roll_max) { - roll_out = -_roll_max; - limit.roll_pitch = true; - } - if (roll_out > _roll_max) { - roll_out = _roll_max; - limit.roll_pitch = true; - } - // scale pitch and update limits - pitch_out = pitch_out * _pitch_scaler; - if (pitch_out < -_pitch_max) { - pitch_out = -_pitch_max; - limit.roll_pitch = true; - } - if (pitch_out > _pitch_max) { - pitch_out = _pitch_max; + float total_out = pythagorous2((float)pitch_out, (float)roll_out); + + if (total_out > _cyclic_max) { + float ratio = (float)_cyclic_max / total_out; + roll_out *= ratio; + pitch_out *= ratio; limit.roll_pitch = true; } @@ -470,4 +459,4 @@ void AP_MotorsHeli_Single::write_aux(int16_t servo_out) _servo_aux.servo_out = servo_out; _servo_aux.calc_pwm(); hal.rcout->write(AP_MOTORS_HELI_SINGLE_AUX, _servo_aux.radio_out); -} \ No newline at end of file +}