diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index c147424329..a560ad2e07 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1537,6 +1537,7 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode) switch( new_roll_pitch_mode ) { case ROLL_PITCH_STABLE: + reset_roll_pitch_in_filters(g.rc_1.control_in, g.rc_2.control_in); roll_pitch_initialised = true; break; case ROLL_PITCH_ACRO: @@ -1545,9 +1546,12 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode) acro_pitch_rate = 0; roll_pitch_initialised = true; break; - case ROLL_PITCH_AUTO: case ROLL_PITCH_STABLE_OF: case ROLL_PITCH_DRIFT: + reset_roll_pitch_in_filters(g.rc_1.control_in, g.rc_2.control_in); + roll_pitch_initialised = true; + break; + case ROLL_PITCH_AUTO: case ROLL_PITCH_LOITER: case ROLL_PITCH_SPORT: roll_pitch_initialised = true; @@ -1557,6 +1561,7 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode) case ROLL_PITCH_AUTOTUNE: // only enter autotune mode from stabilized roll-pitch mode when armed and flying if (roll_pitch_mode == ROLL_PITCH_STABLE && motors.armed() && !ap.land_complete) { + reset_roll_pitch_in_filters(g.rc_1.control_in, g.rc_2.control_in); // auto_tune_start returns true if it wants the roll-pitch mode changed to autotune roll_pitch_initialised = auto_tune_start(); } diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 45e77e5dce..fe944b0be0 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -1,5 +1,15 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +// local variables +float roll_in_filtered; // roll-in in filtered with RC_FEEL_RP parameter +float pitch_in_filtered; // pitch-in filtered with RC_FEEL_RP parameter + +static void reset_roll_pitch_in_filters(int16_t roll_in, int16_t pitch_in) +{ + roll_in_filtered = constrain_int16(roll_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX); + pitch_in_filtered = constrain_int16(pitch_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX); +} + // get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle // returns desired angle in centi-degrees static void get_pilot_desired_lean_angles(int16_t roll_in, int16_t pitch_in, int16_t &roll_out, int16_t &pitch_out) @@ -11,10 +21,31 @@ static void get_pilot_desired_lean_angles(int16_t roll_in, int16_t pitch_in, int roll_in = constrain_int16(roll_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX); pitch_in = constrain_int16(pitch_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX); - // return immediately if no scaling required + // filter input for feel + if (g.rc_feel_rp >= RC_FEEL_RP_VERY_CRISP) { + // no filtering required + roll_in_filtered = roll_in; + pitch_in_filtered = pitch_in; + }else{ + float filter_gain; + if (g.rc_feel_rp >= RC_FEEL_RP_CRISP) { + filter_gain = 0.5; + } else if(g.rc_feel_rp >= RC_FEEL_RP_MEDIUM) { + filter_gain = 0.3; + } else if(g.rc_feel_rp >= RC_FEEL_RP_SOFT) { + filter_gain = 0.05; + } else { + // must be RC_FEEL_RP_VERY_SOFT + filter_gain = 0.02; + } + roll_in_filtered = roll_in_filtered * (1.0 - filter_gain) + (float)roll_in * filter_gain; + pitch_in_filtered = pitch_in_filtered * (1.0 - filter_gain) + (float)pitch_in * filter_gain; + } + + // return filtered roll if no scaling required if (g.angle_max == ROLL_PITCH_INPUT_MAX) { - roll_out = roll_in; - pitch_out = pitch_in; + roll_out = (int16_t)roll_in_filtered; + pitch_out = (int16_t)pitch_in_filtered; return; } @@ -25,8 +56,8 @@ static void get_pilot_desired_lean_angles(int16_t roll_in, int16_t pitch_in, int } // convert pilot input to lean angle - roll_out = roll_in * _scaler; - pitch_out = pitch_in * _scaler; + roll_out = (int16_t)(roll_in_filtered * _scaler); + pitch_out = (int16_t)(pitch_in_filtered * _scaler); } static void diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 8c80dcaa88..49a2e9d5b9 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -96,7 +96,8 @@ public: k_param_battery, k_param_fs_batt_mah, k_param_angle_rate_max, - k_param_rssi_range, // 39 + k_param_rssi_range, + k_param_rc_feel_rp, // 40 // 65: AP_Limits Library k_param_limits = 65, // deprecated - remove @@ -321,6 +322,7 @@ public: AP_Int8 wp_yaw_behavior; // controls how the autopilot controls yaw during missions AP_Int16 angle_max; // maximum lean angle of the copter in centi-degrees AP_Int32 angle_rate_max; // maximum rotation rate in roll/pitch axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes + AP_Int8 rc_feel_rp; // controls vehicle response to user input with 0 being extremely soft and 100 begin extremely crisp // Waypoints // diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 34a7d55a1a..f5700ea45d 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -435,7 +435,14 @@ const AP_Param::Info var_info[] PROGMEM = { // @Range 90000 250000 // @User: Advanced GSCALAR(angle_rate_max, "ANGLE_RATE_MAX", ANGLE_RATE_MAX), - + + // @Param: RC_FEEL_RP + // @DisplayName: RC Feel Roll/Pitch + // @Description: RC feel for roll/pitch which controls vehicle response to user input with 0 being extremely soft and 100 begin crisp + // @User: Advanced + // @Values: 0:Very Soft, 25:Soft, 50:Medium, 75:Crisp, 100:Very Crisp + GSCALAR(rc_feel_rp, "RC_FEEL_RP", RC_FEEL_RP_VERY_CRISP), + #if FRAME_CONFIG == HELI_FRAME // @Group: HS1_ // @Path: ../libraries/RC_Channel/RC_Channel.cpp diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 59514e8a6c..bb3a80f4c5 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -172,6 +172,13 @@ #define ACRO_TRAINER_LEVELING 1 #define ACRO_TRAINER_LIMITED 2 +// RC Feel roll/pitch definitions +#define RC_FEEL_RP_VERY_SOFT 0 +#define RC_FEEL_RP_SOFT 25 +#define RC_FEEL_RP_MEDIUM 50 +#define RC_FEEL_RP_CRISP 75 +#define RC_FEEL_RP_VERY_CRISP 100 + // Commands - Note that APM now uses a subset of the MAVLink protocol // commands. See enum MAV_CMD in the GCS_Mavlink library #define CMD_BLANK 0 // there is no command stored in the mem location