diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 88defd199e..637af7d1ec 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1671,6 +1671,14 @@ void update_throttle_mode(void) } #endif +#if FRAME_CONFIG == HELI_FRAME + if (roll_pitch_mode == ROLL_PITCH_STABLE){ + motors.stab_throttle = true; + } else { + motors.stab_throttle = false; + } +#endif // HELI_FRAME + switch(throttle_mode) { case THROTTLE_MANUAL: if (g.rc_3.control_in > 0) { diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 203794b221..17c30113a9 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -159,6 +159,24 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @Range: 0 1 // @User: Standard AP_GROUPINFO("FLYBAR_MODE", 18, AP_MotorsHeli, flybar_mode, 0), + + // @Param: STAB_COL_MIN + // @DisplayName: Stabilize Throttle Minimum + // @Description: This is the minimum collective setpoint in Stabilize Mode + // @Range: 0 50 + // @Units: 1% + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("STAB_COL_MIN", 19, AP_MotorsHeli, stab_col_min, 0), + + // @Param: STAB_COL_MAX + // @DisplayName: Stabilize Throttle Maximum + // @Description: This is the maximum collective setpoint in Stabilize Mode + // @Range: 50 100 + // @Units: 1% + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("STAB_COL_MAX", 20, AP_MotorsHeli, stab_col_max, 100), AP_GROUPEND }; @@ -340,6 +358,7 @@ void AP_MotorsHeli::reset_swash() _roll_scaler = 1.0; _pitch_scaler = 1.0; _collective_scalar = ((float)(_rc_throttle->radio_max - _rc_throttle->radio_min))/1000.0; + _stab_throttle_scalar = 1.0; // we must be in set-up mode so mark swash as uninitialised _swash_initialised = false; @@ -369,6 +388,7 @@ void AP_MotorsHeli::init_swash() _roll_scaler = (float)roll_max/4500.0; _pitch_scaler = (float)pitch_max/4500.0; _collective_scalar = ((float)(collective_max-collective_min))/1000.0; + _stab_throttle_scalar = ((float)(stab_col_max - stab_col_min))/100.0; if( swash_type == AP_MOTORS_HELI_SWASH_CCPM ) { //CCPM Swashplate, perform control mixing @@ -456,8 +476,11 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll // scale collective pitch coll_out = constrain(coll_out, 0, 1000); + if (stab_throttle){ + coll_out = coll_out * _stab_throttle_scalar + stab_col_min*10; + } coll_out_scaled = coll_out * _collective_scalar + collective_min - 1000; - + // rudder feed forward based on collective if( !ext_gyro_enabled ) { yaw_offset = collective_yaw_effect * abs(coll_out_scaled - throttle_mid); diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index c5f0430a96..07335373a3 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -74,7 +74,9 @@ public: _roll_scaler = 1; _pitch_scaler = 1; _collective_scalar = 1; + _stab_throttle_scalar = 1; _swash_initialised = false; + stab_throttle = false; }; RC_Channel *_servo_1; @@ -97,10 +99,13 @@ public: AP_Int16 collective_yaw_effect; AP_Int8 servo_manual; // used to trigger swash reset from mission planner AP_Int16 ext_gov_setpoint; // maximum output to the motor governor - AP_Int8 rsc_mode; // sets the mode for rotor speed controller + AP_Int8 rsc_mode; // sets the mode for rotor speed controller AP_Int16 rsc_ramp_up_rate; // sets the time in 100th seconds the RSC takes to ramp up to speed AP_Int8 flybar_mode; // selects FBL Acro Mode, or Flybarred Acro Mode int16_t throttle_mid; // throttle mid point in pwm form (i.e. 0 ~ 1000) + AP_Int8 stab_col_min; // collective pitch minimum in Stabilize Mode + AP_Int8 stab_col_max; // collective pitch maximum in Stabilize Mode + bool stab_throttle; // true if we are in Stabilize Mode for reduced Swash Range // init @@ -153,12 +158,13 @@ protected: // internally used variables - float _roll_scaler; // scaler to convert roll input from radio (i.e. -4500 ~ 4500) to max roll range + float _roll_scaler; // scaler to convert roll input from radio (i.e. -4500 ~ 4500) to max roll range float _pitch_scaler; // scaler to convert pitch input from radio (i.e. -4500 ~ 4500) to max pitch range float _collective_scalar; // throttle scalar to convert pwm form (i.e. 0 ~ 1000) passed in to actual servo range (i.e 1250~1750 would be 500) + float _stab_throttle_scalar; // throttle scalar to reduce the range of the collective movement in stabilize mode bool _swash_initialised; // true if swash has been initialised - int16_t rsc_output; // final output to the external motor governor 1000-2000 - int16_t rsc_ramp; // current state of ramping + int16_t rsc_output; // final output to the external motor governor 1000-2000 + int16_t rsc_ramp; // current state of ramping };