ArduCopter: add set_throttle_mode to better control initialisation of variables

ArduCopter/ArduCopter.pde

@@ -431,8 +431,6 @@ static int16_t lon_speed;       // expressed in cm/s.  positive numbers mean mov
 static int16_t lat_speed;       // expressed in cm/s.  positive numbers when moving north
 static int16_t ground_bearing;	// expressed in centidegrees
 
-static int16_t desired_climb_rate;
-
 // The difference between the desired rate of travel and the actual rate of travel
 // updated after GPS read - 5-10hz
 static int16_t x_rate_error;
@@ -616,6 +614,7 @@ static int16_t throttle_accel_target_ef = 0;    // earth frame throttle accelera
 static bool throttle_accel_controller_active = false;   // true when accel based throttle controller is active, false when higher level throttle controllers are providing throttle output directly
 static float z_accel_meas = 0;              // filtered throttle acceleration
 static float throttle_avg;                  // g.throttle_cruise as a float
+static int16_t desired_climb_rate;          // pilot desired climb rate - for logging purposes only
 
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -698,8 +697,6 @@ static int16_t sonar_rate;
 static int32_t baro_alt;
 // The climb_rate as reported by Baro in cm/s
 static int16_t baro_rate;
-// used to switch out of Manual Boost
-static uint8_t reset_throttle_counter;
 
 static int16_t saved_toy_throttle;
 
@@ -1669,6 +1666,64 @@ void update_simple_mode(void)
     g.rc_2.control_in = _pitch;
 }
 
+// set_throttle_mode - sets the throttle mode and initialises any variables as required
+bool set_throttle_mode( uint8_t new_throttle_mode )
+{
+    // boolean to ensure proper initialisation of throttle modes
+    bool throttle_initialised = false;
+
+    // initialise any variables required for the new throttle mode
+    switch(new_throttle_mode) {
+        case THROTTLE_MANUAL:
+        case THROTTLE_MANUAL_TILT_COMPENSATED:
+            throttle_accel_controller_active = false;   // this controller does not use accel based throttle controller
+            throttle_initialised = true;
+            break;
+
+        case THROTTLE_ACCELERATION:
+        case THROTTLE_RATE:
+        case THROTTLE_STABILIZED_RATE:
+        case THROTTLE_DIRECT_ALT:
+            throttle_accel_controller_active = true;    // this controller uses accel based throttle controller
+            throttle_initialised = true;
+            break;
+
+        case THROTTLE_HOLD:
+        case THROTTLE_AUTO:
+            set_new_altitude(current_loc.alt);          // by default hold the current altitude
+            if ( throttle_mode <= THROTTLE_HOLD ) {     // reset the alt hold I terms if previous throttle mode was manual
+                reset_throttle_I();
+            }
+            throttle_accel_controller_active = true;    // this controller uses accel based throttle controller
+            throttle_initialised = true;
+            break;
+
+        case THROTTLE_LAND:
+            set_land_complete(false);   // mark landing as incomplete
+            ground_detector = 0;        // A counter that goes up if our climb rate stalls out.
+            set_new_altitude(0);        // Set a new target altitude
+            throttle_accel_controller_active = true;    // this controller uses accel based throttle controller
+            throttle_initialised = true;
+            break;
+
+        default:
+            cliSerial->printf_P(PSTR("Unsupported throttle mode: %d!!"),new_throttle_mode);
+            break;
+    }
+
+    // update the throttle mode
+    if( throttle_initialised ) {
+        throttle_mode = new_throttle_mode;
+
+        // reset some variables used for logging
+        desired_climb_rate = 0;
+        nav_throttle = 0;
+    }
+
+    // return success or failure
+    return throttle_initialised;
+}
+
 // 50 hz update rate
 // controls all throttle behavior
 void update_throttle_mode(void)

ArduCopter/Attitude.pde

@@ -864,6 +864,9 @@ static int16_t get_pilot_desired_climb_rate(int16_t throttle_control)
         desired_rate = 0;
     }
 
+    // desired climb rate for logging
+    desired_climb_rate = desired_rate;
+
     return desired_rate;
 }
 

ArduCopter/commands_logic.pde

@@ -272,19 +272,10 @@ static void do_nav_wp()
 static void do_land()
 {
     wp_control = LOITER_MODE;
-    throttle_mode = THROTTLE_LAND;
-
-    // just to make sure
-    set_land_complete(false);
-
-    // A counter that goes up if our climb rate stalls out.
-    ground_detector         = 0;
+    set_throttle_mode(THROTTLE_LAND);
 
     // hold at our current location
     set_next_WP(&current_loc);
-
-    // Set a new target altitude
-    set_new_altitude(0);
 }
 
 static void do_loiter_unlimited()

ArduCopter/limits.pde

@@ -269,7 +269,7 @@ static void limits_loop() {
 
                 //set_recovery_home_alt();
                 set_mode(POSITION);
-                throttle_mode = THROTTLE_AUTO;
+                set_throttle_mode(THROTTLE_AUTO);
                 limits.last_action = millis();
                 gcs_send_message(MSG_LIMITS_STATUS);
                 break;

ArduCopter/system.pde

@@ -410,9 +410,6 @@ static void set_mode(byte mode)
     motors.auto_armed(g.rc_3.control_in > 0);
     set_auto_armed(g.rc_3.control_in > 0);
 
-    // clearing value used in interactive alt hold
-    reset_throttle_counter = 0;
-
     // do not auto_land if we are leaving RTL
     loiter_timer = 0;
 
@@ -436,7 +433,7 @@ static void set_mode(byte mode)
     	ap.manual_attitude = true;
         yaw_mode        = YAW_ACRO;
         roll_pitch_mode = ROLL_PITCH_ACRO;
-        throttle_mode   = THROTTLE_MANUAL;
+        set_throttle_mode(THROTTLE_MANUAL);
         // reset acro axis targets to current attitude
 		if(g.axis_enabled){
             roll_axis 	= ahrs.roll_sensor;
@@ -450,7 +447,7 @@ static void set_mode(byte mode)
     	ap.manual_attitude = true;
         yaw_mode        = YAW_HOLD;
         roll_pitch_mode = ROLL_PITCH_STABLE;
-        throttle_mode   = THROTTLE_MANUAL_TILT_COMPENSATED;
+        set_throttle_mode(THROTTLE_MANUAL_TILT_COMPENSATED);
         break;
 
     case ALT_HOLD:
@@ -458,7 +455,7 @@ static void set_mode(byte mode)
     	ap.manual_attitude = true;
         yaw_mode        = ALT_HOLD_YAW;
         roll_pitch_mode = ALT_HOLD_RP;
-        throttle_mode   = ALT_HOLD_THR;
+        set_throttle_mode(ALT_HOLD_THR);
         force_new_altitude(max(current_loc.alt, 100));
         break;
 
@@ -467,7 +464,7 @@ static void set_mode(byte mode)
     	ap.manual_attitude = false;
         yaw_mode        = AUTO_YAW;
         roll_pitch_mode = AUTO_RP;
-        throttle_mode   = AUTO_THR;
+        set_throttle_mode(AUTO_THR);
 
         // loads the commands from where we left off
         init_commands();
@@ -478,7 +475,7 @@ static void set_mode(byte mode)
     	ap.manual_attitude = false;
         yaw_mode        = CIRCLE_YAW;
         roll_pitch_mode = CIRCLE_RP;
-        throttle_mode   = CIRCLE_THR;
+        set_throttle_mode(CIRCLE_THR);
         set_next_WP(&current_loc);
         circle_WP       = next_WP;
         circle_angle    = 0;
@@ -489,7 +486,7 @@ static void set_mode(byte mode)
     	ap.manual_attitude = false;
         yaw_mode        = LOITER_YAW;
         roll_pitch_mode = LOITER_RP;
-        throttle_mode   = LOITER_THR;
+        set_throttle_mode(LOITER_THR);
         set_next_WP(&current_loc);
         break;
 
@@ -498,7 +495,7 @@ static void set_mode(byte mode)
     	ap.manual_attitude = false;
         yaw_mode        = YAW_HOLD;
         roll_pitch_mode = ROLL_PITCH_AUTO;
-        throttle_mode   = THROTTLE_MANUAL;
+        set_throttle_mode(THROTTLE_MANUAL);
         set_next_WP(&current_loc);
         break;
 
@@ -507,7 +504,7 @@ static void set_mode(byte mode)
     	ap.manual_attitude = false;
         yaw_mode        = YAW_AUTO;
         roll_pitch_mode = ROLL_PITCH_AUTO;
-        throttle_mode   = THROTTLE_AUTO;
+        set_throttle_mode(THROTTLE_AUTO);
         next_WP         = current_loc;
         set_next_WP(&guided_WP);
         break;
@@ -525,7 +522,6 @@ static void set_mode(byte mode)
             roll_pitch_mode = ROLL_PITCH_STABLE;
         }
     	ap.manual_throttle = false;
-        throttle_mode   = THROTTLE_LAND;
         do_land();
         break;
 
@@ -534,7 +530,7 @@ static void set_mode(byte mode)
     	ap.manual_attitude = false;
         yaw_mode        = RTL_YAW;
         roll_pitch_mode = RTL_RP;
-        throttle_mode   = RTL_THR;
+        set_throttle_mode(RTL_THR);
         set_rtl_reached_alt(false);
         set_next_WP(&current_loc);
         set_new_altitude(get_RTL_alt());
@@ -545,7 +541,7 @@ static void set_mode(byte mode)
     	ap.manual_attitude = false;
         yaw_mode        = OF_LOITER_YAW;
         roll_pitch_mode = OF_LOITER_RP;
-        throttle_mode   = OF_LOITER_THR;
+        set_throttle_mode(OF_LOITER_THR);
         set_next_WP(&current_loc);
         break;
 
@@ -557,14 +553,12 @@ static void set_mode(byte mode)
     	ap.manual_attitude = true;
         yaw_mode        = YAW_TOY;
         roll_pitch_mode = ROLL_PITCH_TOY;
-        throttle_mode   = THROTTLE_AUTO;
+        set_throttle_mode(THROTTLE_AUTO);
         wp_control      = NO_NAV_MODE;
 
         // save throttle for fast exit of Alt hold
         saved_toy_throttle = g.rc_3.control_in;
 
-        // hold the current altitude
-        set_new_altitude(current_loc.alt);
         break;
 
     case TOY_M:
@@ -573,7 +567,7 @@ static void set_mode(byte mode)
         yaw_mode            = YAW_TOY;
         roll_pitch_mode 	= ROLL_PITCH_TOY;
         wp_control          = NO_NAV_MODE;
-        throttle_mode       = THROTTLE_HOLD;
+        set_throttle_mode(THROTTLE_HOLD);
         break;
 
     default:
@@ -582,7 +576,7 @@ static void set_mode(byte mode)
 
     if(ap.failsafe) {
         // this is to allow us to fly home without interactive throttle control
-        throttle_mode = THROTTLE_AUTO;
+        set_throttle_mode(THROTTLE_AUTO);
     	ap.manual_throttle = false;
 
         // does not wait for us to be in high throttle, since the
@@ -591,18 +585,12 @@ static void set_mode(byte mode)
     	set_auto_armed(true);
     }
 
-    if(ap.manual_throttle) {
-		desired_climb_rate = 0;
-	}
-
     if(ap.manual_attitude) {
         // We are under manual attitude control
         // remove the navigation from roll and pitch command
         reset_nav_params();
         // remove the wind compenstaion
         reset_wind_I();
-        // Clears the alt hold compensation
-        reset_throttle_I();
     }
 
     Log_Write_Mode(control_mode);