From 5a88bc4dd7529742a2e7e5cf291b4b295fa4b256 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 12 Feb 2014 16:28:41 +0900
Subject: [PATCH] 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 | 7 ++++++-
ArduCopter/Attitude.pde | 41 ++++++++++++++++++++++++++++++++++-----
ArduCopter/Parameters.h | 4 +++-
ArduCopter/Parameters.pde | 9 ++++++++-
ArduCopter/defines.h | 7 +++++++
5 files changed, 60 insertions(+), 8 deletions(-)
diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index d2e4250d3..6cbf07a29 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -1552,6 +1552,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:
@@ -1560,9 +1561,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;
@@ -1572,6 +1576,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 35b2596d5..9e4efd2a9 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 237ad7431..b15d126b7 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 c2b64f759..bfe7e404a 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 24646fa43..7a0ea56f3 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -175,6 +175,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
--
GitLab