Copter: add RC_FEEL_RP parameter

Values from 0 ~ 100 control amount of filtering on roll and pitch input.
100 = no filter so crisp feel, 0 = a lot of filtering so very sluggish
response

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();
             }

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;
+        roll_out = (int16_t)roll_in_filtered;
-        pitch_out = pitch_in;
+        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;
+    roll_out = (int16_t)(roll_in_filtered * _scaler);
-    pitch_out = pitch_in * _scaler;
+    pitch_out = (int16_t)(pitch_in_filtered * _scaler);
 }
 
 static void

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
     //

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

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