diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 4403819302..e2e3657884 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1680,7 +1680,7 @@ void update_throttle_mode(void) // do not run throttle controllers if motors disarmed if( !motors.armed() ) { - set_throttle_out(0); + set_throttle_out(0, false); return; } @@ -1697,10 +1697,10 @@ void update_throttle_mode(void) case THROTTLE_MANUAL: // completely manual throttle if(g.rc_3.control_in <= 0){ - set_throttle_out(0); + set_throttle_out(0, false); }else{ // send pilot's output directly to motors - set_throttle_out(g.rc_3.control_in); + set_throttle_out(g.rc_3.control_in, false); // update estimate of throttle cruise update_throttle_cruise(g.rc_3.control_in); @@ -1718,15 +1718,9 @@ void update_throttle_mode(void) case THROTTLE_MANUAL_TILT_COMPENSATED: // manual throttle but with angle boost if (g.rc_3.control_in <= 0) { - set_throttle_out(0); + set_throttle_out(0, false); // no need for angle boost with zero throttle }else{ - // TO-DO: combine multicopter and tradheli angle boost functions -#if FRAME_CONFIG == HELI_FRAME - set_throttle_out(heli_get_angle_boost(g.rc_3.control_in)); -#else - angle_boost = get_angle_boost(g.rc_3.control_in); - set_throttle_out(g.rc_3.control_in + angle_boost); -#endif + set_throttle_out(g.rc_3.control_in, true); // update estimate of throttle cruise update_throttle_cruise(g.rc_3.control_in); @@ -1743,16 +1737,17 @@ void update_throttle_mode(void) case THROTTLE_ACCELERATION: // pilot inputs the desired acceleration if(g.rc_3.control_in <= 0){ - set_throttle_out(0); + set_throttle_out(0, false); }else{ int16_t desired_acceleration = get_pilot_desired_acceleration(g.rc_3.control_in); set_throttle_accel_target(desired_acceleration); } + break; case THROTTLE_RATE: // pilot inputs the desired climb rate. Note this is the unstabilized rate controller if(g.rc_3.control_in <= 0){ - set_throttle_out(0); + set_throttle_out(0, false); }else{ pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in); get_throttle_rate(pilot_climb_rate); @@ -1762,7 +1757,7 @@ void update_throttle_mode(void) case THROTTLE_STABILIZED_RATE: // pilot inputs the desired climb rate. Note this is the unstabilized rate controller if(g.rc_3.control_in <= 0){ - set_throttle_out(0); + set_throttle_out(0, false); }else{ pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in); get_throttle_rate_stabilized(pilot_climb_rate); @@ -1772,7 +1767,7 @@ void update_throttle_mode(void) case THROTTLE_DIRECT_ALT: // pilot inputs a desired altitude from 0 ~ 10 meters if(g.rc_3.control_in <= 0){ - set_throttle_out(0); + set_throttle_out(0, false); }else{ // To-Do: this should update the global desired altitude variable next_WP.alt int32_t desired_alt = get_pilot_desired_direct_alt(g.rc_3.control_in); @@ -1783,14 +1778,7 @@ void update_throttle_mode(void) case THROTTLE_HOLD: // alt hold plus pilot input of climb rate pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in); - - // check for pilot override - if( pilot_climb_rate != 0 ) { - get_throttle_rate_stabilized(pilot_climb_rate); - }else{ - get_throttle_rate_stabilized(0); - force_new_altitude(current_loc.alt); //TO-DO: this should be set to stabilized target - } + get_throttle_rate_stabilized(pilot_climb_rate); break; case THROTTLE_AUTO: diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 398f3ee278..0557292508 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -294,7 +294,7 @@ run_rate_controllers() // run throttle controller if an accel based target has been provided if( throttle_accel_controller_active ) { - g.rc_3.servo_out = get_throttle_accel(throttle_accel_target_ef); + set_throttle_out(get_throttle_accel(throttle_accel_target_ef), true); } } @@ -718,32 +718,52 @@ static void update_throttle_cruise(int16_t throttle) } } -static int16_t get_angle_boost(int16_t value) -{ - float temp = cos_pitch_x * cos_roll_x; - temp = constrain(temp, .75, 1.0); - return ((float)(value + 80) / temp) - (value + 80); -} - #if FRAME_CONFIG == HELI_FRAME -// heli_angle_boost - adds a boost depending on roll/pitch values -// equivalent of quad's angle_boost function +// get_angle_boost - returns a throttle including compensation for roll/pitch angle // throttle value should be 0 ~ 1000 -static int16_t heli_get_angle_boost(int16_t throttle) +// for traditional helicopters +static int16_t get_angle_boost(int16_t throttle) { float angle_boost_factor = cos_pitch_x * cos_roll_x; angle_boost_factor = 1.0 - constrain(angle_boost_factor, .5, 1.0); int16_t throttle_above_mid = max(throttle - motors.throttle_mid,0); - return throttle + throttle_above_mid*angle_boost_factor; + // to allow logging of angle boost + angle_boost = throttle_above_mid*angle_boost_factor; + + return throttle + angle_boost; } -#endif // HELI_FRAME +#else // all multicopters +// get_angle_boost - returns a throttle including compensation for roll/pitch angle +// throttle value should be 0 ~ 1000 +static int16_t get_angle_boost(int16_t throttle) +{ + float temp = cos_pitch_x * cos_roll_x; + int16_t throttle_out; + + temp = constrain(temp, .5, 1.0); + temp = constrain(9000-max(labs(roll_axis),labs(pitch_axis)), 0, 3000) / (3000 * temp); + throttle_out = constrain((float)(throttle-g.throttle_min) * temp + g.throttle_min, g.throttle_min, 1000); + //Serial.printf("Thin:%4.2f sincos:%4.2f temp:%4.2f roll_axis:%4.2f Out:%4.2f \n", 1.0*throttle, 1.0*cos_pitch_x * cos_roll_x, 1.0*temp, 1.0*roll_axis, 1.0*constrain((float)value * temp, 0, 1000)); + + // to allow logging of angle boost + angle_boost = throttle_out - throttle; + + return throttle_out; +} +#endif // FRAME_CONFIG == HELI_FRAME // set_throttle_out - to be called by upper throttle controllers when they wish to provide throttle output directly to motors (instead of using accel based throttle controller) // provide 0 to cut motors -void set_throttle_out( int16_t throttle_out ) +void set_throttle_out( int16_t throttle_out, bool apply_angle_boost ) { - g.rc_3.servo_out = throttle_out; + if( apply_angle_boost ) { + g.rc_3.servo_out = get_angle_boost(throttle_out); + }else{ + g.rc_3.servo_out = throttle_out; + // clear angle_boost for logging purposes + angle_boost = 0; + } throttle_accel_controller_active = false; } diff --git a/ArduCopter/flip.pde b/ArduCopter/flip.pde index 1ed9a9d86b..c049eb38e6 100644 --- a/ArduCopter/flip.pde +++ b/ArduCopter/flip.pde @@ -39,7 +39,7 @@ void roll_flip() if (roll < 4500) { // Roll control g.rc_1.servo_out = AAP_ROLL_OUT * flip_dir; - set_throttle_out(g.rc_3.control_in + AAP_THR_INC); + set_throttle_out(g.rc_3.control_in + AAP_THR_INC, false); }else{ flip_state++; } @@ -52,7 +52,7 @@ void roll_flip() #else g.rc_1.servo_out = get_rate_roll(40000 * flip_dir); #endif // HELI_FRAME - set_throttle_out(g.rc_3.control_in - AAP_THR_DEC); + set_throttle_out(g.rc_3.control_in - AAP_THR_DEC, false); }else{ flip_state++; flip_timer = 0; @@ -63,7 +63,7 @@ void roll_flip() if (flip_timer < 100) { //g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in); get_stabilize_roll(g.rc_1.control_in); - set_throttle_out(g.rc_3.control_in + AAP_THR_INC); + set_throttle_out(g.rc_3.control_in + AAP_THR_INC, false); flip_timer++; }else{ Log_Write_Event(DATA_END_FLIP);