From 3a529b2ceff1b664f76b7d96437727e54a4526f0 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Mon, 17 Nov 2014 14:36:13 -0800
Subject: [PATCH] Copter: utilize get_control_mid for throttle calculations
---
ArduCopter/Attitude.pde | 38 ++++++++++++++++++++------------------
1 file changed, 20 insertions(+), 18 deletions(-)
diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde
index 91fb26b12..4ac969f0a 100644
--- a/ArduCopter/Attitude.pde
+++ b/ArduCopter/Attitude.pde
@@ -124,27 +124,23 @@ set_throttle_takeoff()
// get_pilot_desired_throttle - transform pilot's throttle input to make cruise throttle mid stick
// used only for manual throttle modes
// returns throttle output 0 to 1000
-#define THROTTLE_IN_MIDDLE 500 // the throttle mid point
static int16_t get_pilot_desired_throttle(int16_t throttle_control)
{
int16_t throttle_out;
- // exit immediately in the simple cases
- if( throttle_control == 0 || g.throttle_mid == 500) {
- return throttle_control;
- }
+ int16_t mid_stick = g.rc_3.get_control_mid();
// ensure reasonable throttle values
throttle_control = constrain_int16(throttle_control,0,1000);
g.throttle_mid = constrain_int16(g.throttle_mid,300,700);
// check throttle is above, below or in the deadband
- if (throttle_control < THROTTLE_IN_MIDDLE) {
+ if (throttle_control < mid_stick) {
// below the deadband
- throttle_out = g.throttle_min + ((float)(throttle_control-g.throttle_min))*((float)(g.throttle_mid - g.throttle_min))/((float)(500-g.throttle_min));
- }else if(throttle_control > THROTTLE_IN_MIDDLE) {
+ throttle_out = g.throttle_min + ((float)(throttle_control-g.throttle_min))*((float)(g.throttle_mid - g.throttle_min))/((float)(mid_stick-g.throttle_min));
+ }else if(throttle_control > mid_stick) {
// above the deadband
- throttle_out = g.throttle_mid + ((float)(throttle_control-500))*(float)(1000-g.throttle_mid)/500.0f;
+ throttle_out = g.throttle_mid + ((float)(throttle_control-mid_stick))*(float)(1000-g.throttle_mid)/mid_stick;
}else{
// must be in the deadband
throttle_out = g.throttle_mid;
@@ -156,8 +152,6 @@ static int16_t get_pilot_desired_throttle(int16_t throttle_control)
// get_pilot_desired_climb_rate - transform pilot's throttle input to
// climb rate in cm/s. we use radio_in instead of control_in to get the full range
// without any deadzone at the bottom
-#define THROTTLE_IN_DEADBAND_TOP (THROTTLE_IN_MIDDLE+g.throttle_deadzone) // top of the deadband
-#define THROTTLE_IN_DEADBAND_BOTTOM (THROTTLE_IN_MIDDLE-g.throttle_deadzone) // bottom of the deadband
static int16_t get_pilot_desired_climb_rate(int16_t throttle_control)
{
int16_t desired_rate = 0;
@@ -167,19 +161,24 @@ static int16_t get_pilot_desired_climb_rate(int16_t throttle_control)
return 0;
}
+ int16_t mid_stick = g.rc_3.get_control_mid();
+ int16_t deadband_top = mid_stick + g.throttle_deadzone;
+ int16_t deadband_bottom = mid_stick - g.throttle_deadzone;
+
+
// ensure a reasonable throttle value
- throttle_control = constrain_int16(throttle_control,0,1000);
+ throttle_control = constrain_int16(throttle_control,g.throttle_min,1000);
// ensure a reasonable deadzone
g.throttle_deadzone = constrain_int16(g.throttle_deadzone, 0, 400);
// check throttle is above, below or in the deadband
- if (throttle_control < THROTTLE_IN_DEADBAND_BOTTOM) {
+ if (throttle_control < deadband_bottom) {
// below the deadband
- desired_rate = (int32_t)g.pilot_velocity_z_max * (throttle_control-THROTTLE_IN_DEADBAND_BOTTOM) / (THROTTLE_IN_MIDDLE - g.throttle_deadzone);
- }else if (throttle_control > THROTTLE_IN_DEADBAND_TOP) {
+ desired_rate = (int32_t)g.pilot_velocity_z_max * (throttle_control-deadband_bottom) / (deadband_bottom-g.throttle_min);
+ }else if (throttle_control > deadband_top) {
// above the deadband
- desired_rate = (int32_t)g.pilot_velocity_z_max * (throttle_control-THROTTLE_IN_DEADBAND_TOP) / (THROTTLE_IN_MIDDLE - g.throttle_deadzone);
+ desired_rate = (int32_t)g.pilot_velocity_z_max * (throttle_control-deadband_top) / (1000-deadband_top);
}else{
// must be in the deadband
desired_rate = 0;
@@ -209,6 +208,9 @@ static int16_t get_throttle_pre_takeoff(int16_t throttle_control)
return 0;
}
+ int16_t mid_stick = g.rc_3.get_control_mid();
+ int16_t deadband_top = mid_stick + g.throttle_deadzone;
+
// sanity check throttle input
throttle_control = constrain_int16(throttle_control,0,1000);
@@ -221,8 +223,8 @@ static int16_t get_throttle_pre_takeoff(int16_t throttle_control)
}
// check throttle is below top of deadband
- if (throttle_control < THROTTLE_IN_DEADBAND_TOP) {
- throttle_out = g.throttle_min + ((float)(throttle_control-g.throttle_min))*((float)(get_non_takeoff_throttle() - g.throttle_min))/((float)(THROTTLE_IN_DEADBAND_TOP-g.throttle_min));
+ if (throttle_control < deadband_top) {
+ throttle_out = g.throttle_min + ((float)(throttle_control-g.throttle_min))*((float)(get_non_takeoff_throttle() - g.throttle_min))/((float)(deadband_top-g.throttle_min));
}else{
// must be in the deadband
throttle_out = get_non_takeoff_throttle();
--
GitLab