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

Copter: utilize get_control_mid for throttle calculations

Browse Source
This commit is contained in:
Jonathan Challinger 2014-11-17 14:36:13 -08:00 committed by Randy Mackay
parent 9375fc8947
commit 3a529b2cef
1 changed files with 20 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
ArduCopter/Attitude.pde
View File

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