diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index ceceeb28af..0d9e2d4c68 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1057,7 +1057,7 @@ void update_throttle_mode(void) case THROTTLE_MANUAL: if (g.rc_3.control_in > 0){ #if FRAME_CONFIG == HELI_FRAME - g.rc_3.servo_out = heli_get_angle_boost(heli_get_scaled_throttle(g.rc_3.control_in)); + g.rc_3.servo_out = heli_get_angle_boost(g.rc_3.control_in); #else angle_boost = get_angle_boost(g.rc_3.control_in); g.rc_3.servo_out = g.rc_3.control_in + angle_boost; @@ -1077,34 +1077,40 @@ void update_throttle_mode(void) // fall through case THROTTLE_AUTO: - // 10hz, don't run up i term - if(invalid_throttle && motor_auto_armed == true){ - // how far off are we - altitude_error = get_altitude_error(); - - // get the AP throttle - nav_throttle = get_nav_throttle(altitude_error);//, 250); //150 = target speed of 1.5m/s - //Serial.printf("in:%d, cr:%d, NT:%d, I:%1.4f\n", g.rc_3.control_in,altitude_error, nav_throttle, g.pi_throttle.get_integrator()); - - // clear the new data flag - invalid_throttle = false; - } + // calculate angle boost angle_boost = get_angle_boost(g.throttle_cruise); - + + // manual command up or down? if(manual_boost != 0){ + //remove alt_hold_velocity when implemented #if FRAME_CONFIG == HELI_FRAME - g.rc_3.servo_out = heli_get_angle_boost(heli_get_scaled_throttle(g.throttle_cruise + nav_throttle + manual_boost + get_z_damping())); + g.rc_3.servo_out = heli_get_angle_boost(g.throttle_cruise + manual_boost + get_z_damping()); #else g.rc_3.servo_out = g.throttle_cruise + angle_boost + manual_boost + get_z_damping(); #endif + // reset next_WP.alt next_WP.alt = max(current_loc.alt, 100); + }else{ + // 10hz, don't run up i term + if(invalid_throttle && motor_auto_armed == true){ + + // how far off are we + altitude_error = get_altitude_error(); + + // get the AP throttle + nav_throttle = get_nav_throttle(altitude_error);//, 250); //150 = target speed of 1.5m/s + //Serial.printf("in:%d, cr:%d, NT:%d, I:%1.4f\n", g.rc_3.control_in,altitude_error, nav_throttle, g.pi_throttle.get_integrator()); + + // clear the new data flag + invalid_throttle = false; + } + #if FRAME_CONFIG == HELI_FRAME - //g.rc_3.servo_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle + get_z_damping()); - g.rc_3.servo_out = heli_get_angle_boost(heli_get_scaled_throttle(g.throttle_cruise + nav_throttle + get_z_damping())); + g.rc_3.servo_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle + get_z_damping()); #else g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost + get_z_damping(); #endif @@ -1399,7 +1405,7 @@ static void tuning(){ break; case CH6_THROTTLE_KP: - g.rc_6.set_range(0,1000); + g.rc_6.set_range(0,1000); // 0 to 1 g.pi_throttle.kP(tuning_value); break; @@ -1437,6 +1443,11 @@ static void tuning(){ g.heli_ext_gyro_gain = tuning_value * 1000; break; #endif + + case CH6_THR_HOLD_KP: + g.rc_6.set_range(0,1000); // 0 to 1 + g.pi_alt_hold.kP(tuning_value); + break; } } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 21446159dd..d26bf3a658 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -140,7 +140,7 @@ #define CH6_RATE_KP 4 #define CH6_RATE_KI 5 #define CH6_YAW_RATE_KP 6 -// Altitude +// Altitude rate controller #define CH6_THROTTLE_KP 7 // Extras #define CH6_TOP_BOTTOM_RATIO 8 @@ -151,6 +151,9 @@ #define CH6_LOITER_P 12 #define CH6_HELI_EXTERNAL_GYRO 13 +// altitude controller +#define CH6_THR_HOLD_KP 14 + // nav byte mask // ------------- #define NAV_LOCATION 1 diff --git a/ArduCopter/heli.pde b/ArduCopter/heli.pde index 3baf8d5ef1..c0d7c03367 100644 --- a/ArduCopter/heli.pde +++ b/ArduCopter/heli.pde @@ -5,8 +5,8 @@ #define HELI_SERVO_AVERAGING_DIGITAL 0 // 250Hz #define HELI_SERVO_AVERAGING_ANALOG 2 // 125Hz -static float heli_throttle_scaler = 0; static bool heli_swash_initialised = false; +static int heli_throttle_mid = 0; // throttle mid point in pwm form (i.e. 0 ~ 1000) // heli_servo_averaging: // 0 or 1 = no averaging, 250hz @@ -19,8 +19,6 @@ static bool heli_swash_initialised = false; static void heli_init_swash() { int i; - int tilt_max[CH_3+1]; - int total_tilt_max = 0; // swash servo initialisation g.heli_servo_1.set_range(0,1000); @@ -38,23 +36,38 @@ static void heli_init_swash() heli_rollFactor[CH_2] = cos(radians(g.heli_servo2_pos + 90 - g.heli_phase_angle)); heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90 - g.heli_phase_angle)); - // collective min / max - total_tilt_max = 0; - for( i=CH_1; i<=CH_3; i++ ) { - tilt_max[i] = max(abs(heli_rollFactor[i]*g.heli_roll_max), abs(heli_pitchFactor[i]*g.heli_pitch_max))/100; - total_tilt_max = max(total_tilt_max,tilt_max[i]); + // ensure g.heli_coll values are reasonable + if( g.heli_coll_min >= g.heli_coll_max ) { + g.heli_coll_min = 1000; + g.heli_coll_max = 2000; } - - // servo min/max values - or should I use set_range? - g.heli_servo_1.radio_min = g.heli_coll_min - tilt_max[CH_1]; - g.heli_servo_1.radio_max = g.heli_coll_max + tilt_max[CH_1]; - g.heli_servo_2.radio_min = g.heli_coll_min - tilt_max[CH_2]; - g.heli_servo_2.radio_max = g.heli_coll_max + tilt_max[CH_2]; - g.heli_servo_3.radio_min = g.heli_coll_min - tilt_max[CH_3]; - g.heli_servo_3.radio_max = g.heli_coll_max + tilt_max[CH_3]; - - // scaler for changing channel 3 radio input into collective range - heli_throttle_scaler = ((float)(g.heli_coll_max - g.heli_coll_min))/1000; + g.heli_coll_mid = constrain(g.heli_coll_mid, g.heli_coll_min, g.heli_coll_max); + + // servo min/max values + if( g.heli_servo_1.get_reverse() ) { + g.heli_servo_1.radio_min = 3000 - g.heli_coll_max + (g.heli_servo_1.radio_trim-1500); + g.heli_servo_1.radio_max = 3000 - g.heli_coll_min + (g.heli_servo_1.radio_trim-1500); + }else{ + g.heli_servo_1.radio_min = g.heli_coll_min + (g.heli_servo_1.radio_trim-1500); + g.heli_servo_1.radio_max = g.heli_coll_max + (g.heli_servo_1.radio_trim-1500); + } + if( g.heli_servo_2.get_reverse() ) { + g.heli_servo_2.radio_min = 3000 - g.heli_coll_max + (g.heli_servo_2.radio_trim-1500); + g.heli_servo_2.radio_max = 3000 - g.heli_coll_min + (g.heli_servo_2.radio_trim-1500); + }else{ + g.heli_servo_2.radio_min = g.heli_coll_min + (g.heli_servo_2.radio_trim-1500); + g.heli_servo_2.radio_max = g.heli_coll_max + (g.heli_servo_2.radio_trim-1500); + } + if( g.heli_servo_3.get_reverse() ) { + g.heli_servo_3.radio_min = 3000 - g.heli_coll_max + (g.heli_servo_3.radio_trim-1500); + g.heli_servo_3.radio_max = 3000 - g.heli_coll_min + (g.heli_servo_3.radio_trim-1500); + }else{ + g.heli_servo_3.radio_min = g.heli_coll_min + (g.heli_servo_3.radio_trim-1500); + g.heli_servo_3.radio_max = g.heli_coll_max + (g.heli_servo_3.radio_trim-1500); + } + + // calculate throttle mid point + heli_throttle_mid = (g.heli_coll_mid-g.heli_coll_min)*(1000.0/(g.heli_coll_max-g.heli_coll_min)); // reset the servo averaging for( i=0; i<=3; i++ ) @@ -72,7 +85,7 @@ static void heli_init_swash() static void heli_move_servos_to_mid() { - heli_move_swash(0,0,1500,0); + heli_move_swash(0,0,500,0); } // @@ -80,7 +93,7 @@ static void heli_move_servos_to_mid() // - expected ranges: // roll : -4500 ~ 4500 // pitch: -4500 ~ 4500 -// collective: 1000 ~ 2000 +// collective: 0 ~ 1000 // yaw: -4500 ~ 4500 // static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_out) @@ -92,6 +105,29 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o // we must be in set-up mode so mark swash as uninitialised heli_swash_initialised = false; + // free up servo ranges + if( g.heli_servo_1.get_reverse() ) { + g.heli_servo_1.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_1.radio_trim-1500); + g.heli_servo_1.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_1.radio_trim-1500); + }else{ + g.heli_servo_1.radio_min = g.rc_3.radio_min + (g.heli_servo_1.radio_trim-1500); + g.heli_servo_1.radio_max = g.rc_3.radio_max + (g.heli_servo_1.radio_trim-1500); + } + if( g.heli_servo_2.get_reverse() ) { + g.heli_servo_2.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_2.radio_trim-1500); + g.heli_servo_2.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_2.radio_trim-1500); + }else{ + g.heli_servo_2.radio_min = g.rc_3.radio_min + (g.heli_servo_2.radio_trim-1500); + g.heli_servo_2.radio_max = g.rc_3.radio_max + (g.heli_servo_2.radio_trim-1500); + } + if( g.heli_servo_3.get_reverse() ) { + g.heli_servo_3.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_3.radio_trim-1500); + g.heli_servo_3.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_3.radio_trim-1500); + }else{ + g.heli_servo_3.radio_min = g.rc_3.radio_min + (g.heli_servo_3.radio_trim-1500); + g.heli_servo_3.radio_max = g.rc_3.radio_max + (g.heli_servo_3.radio_trim-1500); + } + }else{ // regular flight mode // check if we need to reinitialise the swash @@ -102,7 +138,7 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o // ensure values are acceptable: roll_out = constrain(roll_out, (int)-g.heli_roll_max, (int)g.heli_roll_max); pitch_out = constrain(pitch_out, (int)-g.heli_pitch_max, (int)g.heli_pitch_max); - coll_out = constrain(coll_out, (int)g.heli_coll_min, (int)g.heli_coll_max); + coll_out = constrain(coll_out, 0, 1000); // rudder feed forward based on collective #if HIL_MODE == HIL_MODE_DISABLED // don't do rudder feed forward in simulator @@ -113,9 +149,9 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o } // swashplate servos - g.heli_servo_1.servo_out = (heli_rollFactor[CH_1] * roll_out + heli_pitchFactor[CH_1] * pitch_out)/10 + coll_out - 1000 + (g.heli_servo_1.radio_trim-1500); - g.heli_servo_2.servo_out = (heli_rollFactor[CH_2] * roll_out + heli_pitchFactor[CH_2] * pitch_out)/10 + coll_out - 1000 + (g.heli_servo_2.radio_trim-1500); - g.heli_servo_3.servo_out = (heli_rollFactor[CH_3] * roll_out + heli_pitchFactor[CH_3] * pitch_out)/10 + coll_out - 1000 + (g.heli_servo_3.radio_trim-1500); + g.heli_servo_1.servo_out = (heli_rollFactor[CH_1] * roll_out + heli_pitchFactor[CH_1] * pitch_out)/10 + coll_out + (g.heli_servo_1.radio_trim-1500); + g.heli_servo_2.servo_out = (heli_rollFactor[CH_2] * roll_out + heli_pitchFactor[CH_2] * pitch_out)/10 + coll_out + (g.heli_servo_2.radio_trim-1500); + g.heli_servo_3.servo_out = (heli_rollFactor[CH_3] * roll_out + heli_pitchFactor[CH_3] * pitch_out)/10 + coll_out + (g.heli_servo_3.radio_trim-1500); g.heli_servo_4.servo_out = yaw_out + yaw_offset; // use servo_out to calculate pwm_out and radio_out @@ -130,7 +166,7 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o heli_servo_out[2] += g.heli_servo_3.radio_out; heli_servo_out[3] += g.heli_servo_4.radio_out; heli_servo_out_count++; - + // is it time to move the servos? if( heli_servo_out_count >= g.heli_servo_averaging ) { @@ -171,8 +207,8 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o static void init_motors_out() { #if INSTANT_PWM == 0 - ICR5 = 5000; // 400 hz output CH 1, 2, 9 - ICR1 = 5000; // 400 hz output CH 3, 4, 10 + ICR5 = 8000; // 250 hz output CH 1, 2, 9 + ICR1 = 8000; // 250 hz output CH 3, 4, 10 ICR3 = 40000; // 50 hz output CH 7, 8, 11 #endif } @@ -194,7 +230,7 @@ static void output_motors_armed() g.rc_3.calc_pwm(); g.rc_4.calc_pwm(); - heli_move_swash( g.rc_1.servo_out, g.rc_2.servo_out, g.rc_3.radio_out, g.rc_4.servo_out ); + heli_move_swash( g.rc_1.servo_out, g.rc_2.servo_out, g.rc_3.servo_out, g.rc_4.servo_out ); } // for helis - armed or disarmed we allow servos to move @@ -212,25 +248,16 @@ static void output_motor_test() { } -// heli_get_scaled_throttle - user's throttle scaled to collective range -// input is expected to be in the range of 0~1000 (ie. pwm) -// also does equivalent of angle_boost -static int heli_get_scaled_throttle(int throttle) -{ - float scaled_throttle = (g.heli_coll_min - 1000) + throttle * heli_throttle_scaler; - return scaled_throttle; -} - -// heli_angle_boost - takes servo_out position -// adds a boost depending on roll/pitch values +// heli_angle_boost - adds a boost depending on roll/pitch values // equivalent of quad's angle_boost function -// pwm_out value should be 0 ~ 1000 -static int heli_get_angle_boost(int pwm_out) +// throttle value should be 0 ~ 1000 +static int heli_get_angle_boost(int throttle) { float angle_boost_factor = cos_pitch_x * cos_roll_x; angle_boost_factor = 1.0 - constrain(angle_boost_factor, .5, 1.0); - int throttle_above_center = max(1000 + pwm_out - g.heli_coll_mid,0); - return pwm_out + throttle_above_center*angle_boost_factor; + int throttle_above_mid = max(throttle - heli_throttle_mid,0); + return throttle + throttle_above_mid*angle_boost_factor; + } #endif // HELI_FRAME