aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Sub: Add gain parameters

Browse Source
This commit is contained in:
Jacob Walser 2016-11-23 16:22:30 -05:00 committed by Andrew Tridgell
parent 5eb50d5d21
commit 5cc1d3293d
5 changed files with 81 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
ArduSub/Parameters.cpp
View File

@ -622,6 +622,38 @@ const AP_Param::Info Sub::var_info[] = {
// @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_14, "RC14_", RC_Channel_aux),
// @Param: JS_GAIN_DEFAULT
// @DisplayName: Default gain at boot
// @Discription: Default gain at boot, must be in range [JS_GAIN_MIN , JS_GAIN_MAX]
// @User: Standard
// @Range: 0.1 1.0
// @Default: 0.5
GSCALAR(gain_default, "JS_GAIN_DEFAULT", 0.5),
// @Param: JS_GAIN_MAX
// @DisplayName: Maximum joystick gain
// @Description: Maximum joystick gain
// @User: Standard
// @Range: 0.2 1.0
// @Default: 1.0
GSCALAR(maxGain, "JS_GAIN_MAX", 1.0),
// @Param: JS_GAIN_MIN
// @DisplayName: Minimum joystick gain
// @Description: Minimum joystick gain
// @User: Standard
// @Range: 0.1 0.8
// @Default: 0.25
GSCALAR(minGain, "JS_GAIN_MIN", 0.25),
// @Param: JS_GAIN_STEPS
// @DisplayName: Gain steps
// @Discription: Controls the number of steps between minimum and maximum joystick gain when the gain is adjusted using buttons. Set to 1 to always use JS_GAIN_DEFAULT.
// @User: Standard
// @Range: 1 10
// @Default: 4
GSCALAR(numGainSettings, "JS_GAIN_STEPS", 4),
// @Group: BTN0_
// @Path: ../libraries/AP_JSButton/AP_JSButton.cpp
GGROUP(jbtn_0, "BTN0_", JSButton),

11
ArduSub/Parameters.h
View File

@ -369,6 +369,12 @@ public:
//Sub-specific parameters
k_param_surface_depth = 256,
// Joystic gain parameters
k_param_gain_default,
k_param_maxGain,
k_param_minGain,
k_param_numGainSettings,
// Joystick button mapping parameters
k_param_jbtn_0 = 261,
k_param_jbtn_1,
@ -519,6 +525,11 @@ public:
AP_Int16 rc_speed; // speed of fast RC Channels in Hz
AP_Float gain_default;
AP_Float maxGain;
AP_Float minGain;
AP_Int8 numGainSettings;
// Joystick button parameters
JSButton jbtn_0;
JSButton jbtn_1;

1
ArduSub/Sub.h
View File

@ -921,6 +921,7 @@ private:
void init_rc_out();
void enable_motor_output();
void read_radio();
void init_joystick();
void transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons);
void handle_jsbutton_press(uint8_t button,bool shift=false,bool held=false);
void camera_tilt_smooth();

42
ArduSub/joystick.cpp
View File

@ -21,10 +21,20 @@ namespace {
int16_t video_switch = 1100;
int16_t x_last, y_last, z_last;
uint16_t buttons_prev;
float gain = 0.5;
float maxGain = 1.0;
float minGain = 0.25;
int8_t numGainSettings = 4;
float gain;
}
void Sub::init_joystick() {
if(g.numGainSettings < 1) g.numGainSettings.set_and_save(1);
if(g.numGainSettings == 1 || (g.gain_default < g.maxGain + 0.01 && g.gain_default > g.minGain - 0.01)) {
gain = constrain_float(g.gain_default, g.minGain, g.maxGain); // Use default gain parameter
} else {
// Use setting closest to average of minGain and maxGain
gain = g.minGain + (g.numGainSettings/2 - 1) * (g.maxGain - g.minGain) / (g.numGainSettings - 1);
}
gain = constrain_float(gain, 0.1, 1.0);
}
void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons) {
@ -208,13 +218,33 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held) {
break;
case JSButton::button_function_t::k_gain_inc:
if ( !held ) {
gain = constrain_float(gain + (maxGain-minGain)/(numGainSettings-1), minGain, maxGain);
// check that our gain parameters are in correct range, update in eeprom and notify gcs if needed
g.minGain.set_and_save(constrain_float(g.minGain, 0.10, 0.80));
g.maxGain.set_and_save(constrain_float(g.maxGain, g.minGain, 1.0));
g.numGainSettings.set_and_save(constrain_int16(g.numGainSettings, 1, 10));
if(g.numGainSettings == 1) {
gain = constrain_float(g.gain_default, g.minGain, g.maxGain);
} else {
gain = constrain_float(gain + (g.maxGain-g.minGain)/(g.numGainSettings-1), g.minGain, g.maxGain);
}
gcs_send_text_fmt(MAV_SEVERITY_INFO,"#Gain is %2.0f%%",gain*100);
}
break;
case JSButton::button_function_t::k_gain_dec:
if ( !held ) {
gain = constrain_float(gain - (maxGain-minGain)/(numGainSettings-1), minGain, maxGain);
// check that our gain parameters are in correct range, update in eeprom and notify gcs if needed
g.minGain.set_and_save(constrain_float(g.minGain, 0.10, 0.80));
g.maxGain.set_and_save(constrain_float(g.maxGain, g.minGain, 1.0));
g.numGainSettings.set_and_save(constrain_int16(g.numGainSettings, 1, 10));
if(g.numGainSettings == 1) {
gain = constrain_float(g.gain_default, g.minGain, g.maxGain);
} else {
gain = constrain_float(gain - (g.maxGain-g.minGain)/(g.numGainSettings-1), g.minGain, g.maxGain);
}
gcs_send_text_fmt(MAV_SEVERITY_INFO,"#Gain is %2.0f%%",gain*100);
}
break;

1
ArduSub/system.cpp
View File

@ -157,6 +157,7 @@ void Sub::init_ardupilot()
init_rc_in(); // sets up rc channels from radio
init_rc_out(); // sets up motors and output to escs
init_joystick(); // joystick initialization
// initialise which outputs Servo and Relay events can use
ServoRelayEvents.set_channel_mask(~motors.get_motor_mask());