Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-03 14:38:44 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: add RC_FEEL_RP parameter

Browse Source 
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
This commit is contained in:
Randy Mackay 2014-02-12 16:28:41 +09:00
parent 7e8438cfc9
commit 5a88bc4dd7
5 changed files with 60 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduCopter/ArduCopter.pde
View File

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

41
ArduCopter/Attitude.pde
View File

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

4
ArduCopter/Parameters.h
View File

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

9
ArduCopter/Parameters.pde
View File

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

7
ArduCopter/defines.h
View File

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