From b515431008ff49905df1fd532a599c3f060a66ab Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 6 Apr 2019 12:38:40 -0600 Subject: [PATCH] AP_Math: add expo and throttle_curve functions --- ArduPlane/quadplane.cpp | 53 +++++++++++++++++++---------------- libraries/AP_Math/AP_Math.cpp | 31 +++++++++++++++++++- libraries/AP_Math/AP_Math.h | 13 +++++++++ 3 files changed, 72 insertions(+), 25 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index de82c2df14..3bcfb55e41 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -409,6 +409,14 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @Bitmask: 0:Motor 1,1:Motor 2,2:Motor 3,3:Motor 4, 4:Motor 5,5:Motor 6,6:Motor 7,7:Motor 8 AP_GROUPINFO("TAILSIT_MOTMX", 9, QuadPlane, tailsitter.motor_mask, 0), + // @Param: THROTTLE_EXPO + // @DisplayName: Throttle expo strength + // @Description: Amount of curvature in throttle curve: 0 is linear, 1 is cubic + // @Range: 0 1 + // @Increment: .1 + // @User: Advanced + AP_GROUPINFO("THROTTLE_EXPO", 10, QuadPlane, throttle_expo, 0.2), + AP_GROUPEND }; @@ -794,7 +802,7 @@ void QuadPlane::control_stabilize(void) } // normal QSTABILIZE mode - float pilot_throttle_scaled = plane.get_throttle_input() / 100.0f; + float pilot_throttle_scaled = get_pilot_throttle(); hold_stabilize(pilot_throttle_scaled); } @@ -904,6 +912,20 @@ void QuadPlane::hold_hover(float target_climb_rate) run_z_controller(); } +float QuadPlane::get_pilot_throttle() +{ + // get normalized throttle [0,1] + float throttle_in = plane.channel_throttle->get_control_in() / plane.channel_throttle->get_range(); + + // get hover throttle level [0,1] + float thr_mid = motors->get_throttle_hover(); + + float thrust_curve_expo = constrain_float(throttle_expo, 0.0f, 1.0f); + + // this puts mid stick at hover throttle + return throttle_curve(thr_mid, thrust_curve_expo, throttle_in);; +} + /* control QACRO mode */ @@ -933,29 +955,7 @@ void QuadPlane::control_qacro(void) target_yaw = plane.channel_rudder->norm_input() * yaw_rate_max * 100.0; } - // get pilot's desired throttle - int16_t mid_stick = plane.channel_throttle->get_control_mid(); - // protect against unlikely divide by zero - if (mid_stick <= 0) { - mid_stick = 50; - } - float thr_mid = motors->get_throttle_hover(); - int16_t throttle_control = plane.channel_throttle->get_control_in(); - float throttle_in; - if (throttle_control < mid_stick) { - // below the deadband - throttle_in = ((float)throttle_control) * 0.5f / (float)mid_stick; - } else if (throttle_control > mid_stick) { - // above the deadband - throttle_in = 0.5f + ((float)(throttle_control - mid_stick)) * 0.5f / (float)(100 - mid_stick); - } else { - // must be in the deadband - throttle_in = 0.5f; - } - - float expo = constrain_float(-(thr_mid - 0.5) / 0.375, -0.5f, 1.0f); - // calculate the output throttle using the given expo function - float throttle_out = throttle_in*(1.0f-expo) + expo*throttle_in*throttle_in*throttle_in; + float throttle_out = get_pilot_throttle(); // run attitude controller if (plane.g.acro_locking) { @@ -1756,6 +1756,11 @@ void QuadPlane::update_throttle_hover() return; } + // do not update if quadplane forward motor is running (wing may be generating lift) + if (!is_tailsitter() && (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) != 0)) { + return; + } + // get throttle output float throttle = motors->get_throttle(); diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp index 2e58c218bd..887a6747b6 100644 --- a/libraries/AP_Math/AP_Math.cpp +++ b/libraries/AP_Math/AP_Math.cpp @@ -78,7 +78,7 @@ template float safe_sqrt(const float v); template float safe_sqrt(const double v); /* - linear interpolation based on a variable in a range + * linear interpolation based on a variable in a range */ float linear_interpolate(float low_output, float high_output, float var_value, @@ -94,6 +94,35 @@ float linear_interpolate(float low_output, float high_output, return low_output + p * (high_output - low_output); } +/* cubic "expo" curve generator + * alpha range: [0,1] min to max expo + * input range: [-1,1] + */ +float expo_curve(float alpha, float x) +{ + return (1.0f - alpha) * x + alpha * x * x * x; +} + +/* throttle curve generator + * thr_mid: output at mid stick + * alpha: expo coefficient + * thr_in: [0-1] + */ +float throttle_curve(float thr_mid, float alpha, float thr_in) +{ + float alpha2 = alpha + 1.25 * (1.0f - alpha) * (0.5f - thr_mid) / 0.5f; + alpha2 = constrain_float(alpha2, 0.0f, 1.0f); + float thr_out = 0.0f; + if (thr_in < 0.5f) { + float t = linear_interpolate(-1.0f, 0.0f, thr_in, 0.0f, 0.5f); + thr_out = linear_interpolate(0.0f, thr_mid, expo_curve(alpha, t), -1.0f, 0.0f); + } else { + float t = linear_interpolate(0.0f, 1.0f, thr_in, 0.5f, 1.0f); + thr_out = linear_interpolate(thr_mid, 1.0f, expo_curve(alpha2, t), 0.0f, 1.0f); + } + return thr_out; +} + template float wrap_180(const T angle, float unit_mod) { diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index 8fc60d561c..c29a537668 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -247,6 +247,19 @@ float linear_interpolate(float low_output, float high_output, float var_value, float var_low, float var_high); +/* cubic "expo" curve generator + * alpha range: [0,1] min to max expo + * input range: [-1,1] + */ +float expo_curve(float alpha, float input); + +/* throttle curve generator + * thr_mid: output at mid stick + * alpha: expo coefficient + * thr_in: [0-1] + */ +float throttle_curve(float thr_mid, float alpha, float thr_in); + /* simple 16 bit random number generator */ uint16_t get_random16(void);