mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Sub: Add throttle channel gain scalar parameter
This commit is contained in:
parent
e61bf46882
commit
5bd2af6b77
@ -517,6 +517,13 @@ const AP_Param::Info Sub::var_info[] = {
|
|||||||
// @Range: 30 400
|
// @Range: 30 400
|
||||||
GSCALAR(lights_step, "JS_LIGHTS_STEP", 100),
|
GSCALAR(lights_step, "JS_LIGHTS_STEP", 100),
|
||||||
|
|
||||||
|
// @Param: JS_THR_GAIN
|
||||||
|
// @DisplayName: Throttle gain scalar
|
||||||
|
// @Description: Scalar for gain on the throttle channel
|
||||||
|
// @User: Standard
|
||||||
|
// @Range: 0.5 4.0
|
||||||
|
GSCALAR(throttle_gain, "JS_THR_GAIN", 1.0f),
|
||||||
|
|
||||||
// @Group: BTN0_
|
// @Group: BTN0_
|
||||||
// @Path: ../libraries/AP_JSButton/AP_JSButton.cpp
|
// @Path: ../libraries/AP_JSButton/AP_JSButton.cpp
|
||||||
GGROUP(jbtn_0, "BTN0_", JSButton),
|
GGROUP(jbtn_0, "BTN0_", JSButton),
|
||||||
|
@ -206,7 +206,7 @@ public:
|
|||||||
k_param_disarm_delay,
|
k_param_disarm_delay,
|
||||||
k_param_terrain_follow,
|
k_param_terrain_follow,
|
||||||
k_param_rc_feel_rp,
|
k_param_rc_feel_rp,
|
||||||
|
k_param_throttle_gain,
|
||||||
|
|
||||||
// Acro Mode parameters
|
// Acro Mode parameters
|
||||||
k_param_acro_yaw_p = 220, // Used in all modes for get_pilot_desired_yaw_rate
|
k_param_acro_yaw_p = 220, // Used in all modes for get_pilot_desired_yaw_rate
|
||||||
@ -345,6 +345,7 @@ public:
|
|||||||
AP_Float maxGain;
|
AP_Float maxGain;
|
||||||
AP_Float minGain;
|
AP_Float minGain;
|
||||||
AP_Int8 numGainSettings;
|
AP_Int8 numGainSettings;
|
||||||
|
AP_Float throttle_gain;
|
||||||
|
|
||||||
AP_Int16 cam_tilt_step;
|
AP_Int16 cam_tilt_step;
|
||||||
AP_Int16 lights_step;
|
AP_Int16 lights_step;
|
||||||
|
@ -46,7 +46,7 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t
|
|||||||
uint32_t tnow_ms = millis();
|
uint32_t tnow_ms = millis();
|
||||||
|
|
||||||
float rpyScale = 0.4*gain; // Scale -1000-1000 to -400-400 with gain
|
float rpyScale = 0.4*gain; // Scale -1000-1000 to -400-400 with gain
|
||||||
float throttleScale = 0.8*gain; // Scale 0-1000 to 0-800 with gain
|
float throttleScale = 0.8*gain*g.throttle_gain; // Scale 0-1000 to 0-800 times gain
|
||||||
int16_t rpyCenter = 1500;
|
int16_t rpyCenter = 1500;
|
||||||
int16_t throttleBase = 1500-500*throttleScale;
|
int16_t throttleBase = 1500-500*throttleScale;
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user