From b6db64f9be70ebf2dbc86bae026e5ef9cb68fc44 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Sat, 16 Jul 2011 23:08:07 +0000 Subject: [PATCH] 2.0.36 Added Yaw control when descending in Alt hold lowered kP & kD for Alt hold a tad Adjusted RTL behavior to do speed control up to 4m to home, then go into Loiter Fixed issue with AUTO not getting proper input. Added Limit to high side of motors git-svn-id: https://arducopter.googlecode.com/svn/trunk@2874 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/motors_hexa.pde | 19 +++++++++++++------ ArduCopterMega/motors_octa.pde | 11 +++++++++++ ArduCopterMega/motors_octa_quad.pde | 11 +++++++++++ ArduCopterMega/motors_quad.pde | 6 ++++++ ArduCopterMega/motors_tri.pde | 5 +++++ ArduCopterMega/motors_y6.pde | 16 ++++++++++++++++ 6 files changed, 62 insertions(+), 6 deletions(-) diff --git a/ArduCopterMega/motors_hexa.pde b/ArduCopterMega/motors_hexa.pde index 813a94c79d..722da90dc0 100644 --- a/ArduCopterMega/motors_hexa.pde +++ b/ArduCopterMega/motors_hexa.pde @@ -6,6 +6,7 @@ void output_motors_armed() { int roll_out, pitch_out; int out_min = g.rc_3.radio_min; + int out_max = g.rc_3.radio_max; // Throttle is 0 to 1000 only g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000); @@ -57,13 +58,19 @@ void output_motors_armed() motor_out[CH_8] -= g.rc_4.pwm_out; // CW // limit output so motors don't stop - motor_out[CH_1] = max(motor_out[CH_1], out_min); - motor_out[CH_2] = max(motor_out[CH_2], out_min); - motor_out[CH_3] = max(motor_out[CH_3], out_min); - motor_out[CH_4] = max(motor_out[CH_4], out_min); - motor_out[CH_7] = max(motor_out[CH_7], out_min); - motor_out[CH_8] = max(motor_out[CH_8], out_min); + motor_out[CH_1] = max(motor_out[CH_1], out_min); + motor_out[CH_2] = max(motor_out[CH_2], out_min); + motor_out[CH_3] = max(motor_out[CH_3], out_min); + motor_out[CH_4] = max(motor_out[CH_4], out_min); + motor_out[CH_7] = max(motor_out[CH_7], out_min); + motor_out[CH_8] = max(motor_out[CH_8], out_min); + motor_out[CH_1] = min(motor_out[CH_1], out_max); + motor_out[CH_2] = min(motor_out[CH_2], out_max); + motor_out[CH_3] = min(motor_out[CH_3], out_max); + motor_out[CH_4] = min(motor_out[CH_4], out_max); + motor_out[CH_7] = min(motor_out[CH_7], out_max); + motor_out[CH_8] = min(motor_out[CH_8], out_max); #if CUT_MOTORS == ENABLED diff --git a/ArduCopterMega/motors_octa.pde b/ArduCopterMega/motors_octa.pde index 4aa867df24..a20e090449 100644 --- a/ArduCopterMega/motors_octa.pde +++ b/ArduCopterMega/motors_octa.pde @@ -6,6 +6,7 @@ void output_motors_armed() { int roll_out, pitch_out; int out_min = g.rc_3.radio_min; + int out_max = g.rc_3.radio_max; // Throttle is 0 to 1000 only g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000); @@ -112,6 +113,16 @@ void output_motors_armed() motor_out[CH_10] = max(motor_out[CH_10], out_min); motor_out[CH_11] = max(motor_out[CH_11], out_min); + motor_out[CH_1] = min(motor_out[CH_1], out_max); + motor_out[CH_2] = min(motor_out[CH_2], out_max); + motor_out[CH_3] = min(motor_out[CH_3], out_max); + motor_out[CH_4] = min(motor_out[CH_4], out_max); + motor_out[CH_7] = min(motor_out[CH_7], out_max); + motor_out[CH_8] = min(motor_out[CH_8], out_max); + motor_out[CH_10] = min(motor_out[CH_10], out_max); + motor_out[CH_11] = min(motor_out[CH_11], out_max); + + #if CUT_MOTORS == ENABLED // Send commands to motors if(g.rc_3.servo_out > 0){ diff --git a/ArduCopterMega/motors_octa_quad.pde b/ArduCopterMega/motors_octa_quad.pde index 03c82a9db8..e0e70d6470 100644 --- a/ArduCopterMega/motors_octa_quad.pde +++ b/ArduCopterMega/motors_octa_quad.pde @@ -6,6 +6,7 @@ void output_motors_armed() { int roll_out, pitch_out; int out_min = g.rc_3.radio_min; + int out_max = g.rc_3.radio_max; // Throttle is 0 to 1000 only g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000); @@ -83,6 +84,16 @@ void output_motors_armed() motor_out[CH_10] = max(motor_out[CH_10], out_min); motor_out[CH_11] = max(motor_out[CH_11], out_min); + + motor_out[CH_1] = min(motor_out[CH_1], out_max); + motor_out[CH_2] = min(motor_out[CH_2], out_max); + motor_out[CH_3] = min(motor_out[CH_3], out_max); + motor_out[CH_4] = min(motor_out[CH_4], out_max); + motor_out[CH_7] = min(motor_out[CH_7], out_max); + motor_out[CH_8] = min(motor_out[CH_8], out_max); + motor_out[CH_10] = min(motor_out[CH_10], out_max); + motor_out[CH_11] = min(motor_out[CH_11], out_max); + #if CUT_MOTORS == ENABLED // Send commands to motors if(g.rc_3.servo_out > 0){ diff --git a/ArduCopterMega/motors_quad.pde b/ArduCopterMega/motors_quad.pde index 6bce447f09..35c6005bf7 100644 --- a/ArduCopterMega/motors_quad.pde +++ b/ArduCopterMega/motors_quad.pde @@ -6,6 +6,7 @@ void output_motors_armed() { int roll_out, pitch_out; int out_min = g.rc_3.radio_min; + int out_max = g.rc_3.radio_max; // Throttle is 0 to 1000 only g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000); @@ -57,6 +58,11 @@ void output_motors_armed() motor_out[CH_3] = max(motor_out[CH_3], out_min); motor_out[CH_4] = max(motor_out[CH_4], out_min); + motor_out[CH_1] = min(motor_out[CH_1], out_max); + motor_out[CH_2] = min(motor_out[CH_2], out_max); + motor_out[CH_3] = min(motor_out[CH_3], out_max); + motor_out[CH_4] = min(motor_out[CH_4], out_max); + #if CUT_MOTORS == ENABLED // Send commands to motors if(g.rc_3.servo_out > 0){ diff --git a/ArduCopterMega/motors_tri.pde b/ArduCopterMega/motors_tri.pde index 1885c9a5cb..e1e0902391 100644 --- a/ArduCopterMega/motors_tri.pde +++ b/ArduCopterMega/motors_tri.pde @@ -4,6 +4,7 @@ void output_motors_armed() { int out_min = g.rc_3.radio_min; + int out_max = g.rc_3.radio_max; // Throttle is 0 to 1000 only g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000); @@ -33,6 +34,10 @@ void output_motors_armed() motor_out[CH_2] = max(motor_out[CH_2], out_min); motor_out[CH_4] = max(motor_out[CH_4], out_min); + motor_out[CH_1] = min(motor_out[CH_1], out_max); + motor_out[CH_2] = min(motor_out[CH_2], out_max); + motor_out[CH_4] = min(motor_out[CH_4], out_max); + #if CUT_MOTORS == ENABLED // Send commands to motors if(g.rc_3.servo_out > 0){ diff --git a/ArduCopterMega/motors_y6.pde b/ArduCopterMega/motors_y6.pde index 194862e2a6..07df2e9071 100644 --- a/ArduCopterMega/motors_y6.pde +++ b/ArduCopterMega/motors_y6.pde @@ -5,6 +5,7 @@ void output_motors_armed() { int out_min = g.rc_3.radio_min; + int out_max = g.rc_3.radio_max; // Throttle is 0 to 1000 only g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000); @@ -41,6 +42,21 @@ void output_motors_armed() motor_out[CH_1] -= g.rc_4.pwm_out; // CW motor_out[CH_4] -= g.rc_4.pwm_out; // CW + // limit output so motors don't stop + motor_out[CH_1] = max(motor_out[CH_1], out_min); + motor_out[CH_2] = max(motor_out[CH_2], out_min); + motor_out[CH_3] = max(motor_out[CH_3], out_min); + motor_out[CH_4] = max(motor_out[CH_4], out_min); + motor_out[CH_7] = max(motor_out[CH_7], out_min); + motor_out[CH_8] = max(motor_out[CH_8], out_min); + + motor_out[CH_1] = min(motor_out[CH_1], out_max); + motor_out[CH_2] = min(motor_out[CH_2], out_max); + motor_out[CH_3] = min(motor_out[CH_3], out_max); + motor_out[CH_4] = min(motor_out[CH_4], out_max); + motor_out[CH_7] = min(motor_out[CH_7], out_max); + motor_out[CH_8] = min(motor_out[CH_8], out_max); + #if CUT_MOTORS == ENABLED // Send commands to motors if(g.rc_3.servo_out > 0){