ArduCopter: added back in angle boost and recombined for heli and multicopter

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
+            set_throttle_out(g.rc_3.control_in, true);
-#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
 
             // 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);
-
+        get_throttle_rate_stabilized(pilot_climb_rate);
-        // 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
-        }
         break;
 
     case THROTTLE_AUTO:

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
+// get_angle_boost - returns a throttle including compensation for roll/pitch angle
-// equivalent of quad's angle_boost function
 // 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;
 }
 

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