Added climb rate control

removed unused vars

ArduCopter/ArduCopter.pde

@@ -279,7 +279,6 @@ ModeFilter sonar_mode_filter;
 ////////////////////////////////////////////////////////////////////////////////
 // Global variables
 ////////////////////////////////////////////////////////////////////////////////
-static const char *comma = ",";
 
 static const char* flight_mode_strings[] = {
 	"STABILIZE",
@@ -328,7 +327,7 @@ static int16_t y_rate_error;
 ////////////////////////////////////////////////////////////////////////////////
 // This is the state of the flight control system
 // There are multiple states defined such as STABILIZE, ACRO,
-static byte 	control_mode		= STABILIZE;
+static int8_t 	control_mode		= STABILIZE;
 // This is the state of simple mode.
 // Set in the control_mode.pde file when the control switch is read
 static bool		do_simple 			= false;
@@ -444,8 +443,6 @@ static int32_t	target_bearing;
 // This is the angle from the copter to the "next_WP" location
 // with the addition of Crosstrack error in degrees * 100
 static int32_t	nav_bearing;
-// This is the angle from the copter to the "home" location  in degrees * 100
-static int32_t 	home_bearing;
 // Status of the Waypoint tracking mode. Options include:
 // NO_NAV_MODE, WP_MODE, LOITER_MODE, CIRCLE_MODE
 static byte	wp_control;
@@ -573,7 +570,8 @@ static int32_t		baro_alt;
 static int32_t 		old_baro_alt;
 // The climb_rate as reported by Baro in cm/s
 static int16_t		baro_rate;
-
+//
+static boolean 		reset_throttle_flag;
 
 ////////////////////////////////////////////////////////////////////////////////
 // flight modes
@@ -682,12 +680,23 @@ static int16_t	nav_throttle;						// 0-1000 for throttle control
 static uint32_t throttle_integrator;
 // This is a future value for replacing the throttle_cruise setup procedure. It's an average of throttle control
 // that is generated when the climb rate is within a certain threshold
-static float	throttle_avg = THROTTLE_CRUISE;
+//static float	throttle_avg = THROTTLE_CRUISE;
 // This is a flag used to trigger the updating of nav_throttle at 10hz
 static bool 	invalid_throttle;
 // Used to track the altitude offset for climbrate control
-static int32_t 	target_altitude;
+//static int32_t 	target_altitude;
 
+////////////////////////////////////////////////////////////////////////////////
+// Climb rate control
+////////////////////////////////////////////////////////////////////////////////
+// Time when we intiated command in millis - used for controlling decent rate
+// The orginal altitude used to base our new altitude during decent
+static int32_t 	original_altitude;
+// Used to track the altitude offset for climbrate control
+static int32_t 	target_altitude;
+static uint32_t alt_change_timer;
+static int8_t 	alt_change_flag;
+static uint32_t alt_change;
 
 ////////////////////////////////////////////////////////////////////////////////
 // Navigation Yaw control
@@ -742,16 +751,6 @@ static int32_t 	condition_value; // used in condition commands (eg delay, change
 static uint32_t condition_start;
 
 
-////////////////////////////////////////////////////////////////////////////////
-// Auto Landing
-////////////////////////////////////////////////////////////////////////////////
-// Time when we intiated command in millis - used for controlling decent rate
-static int32_t 	land_start;
-// The orginal altitude used to base our new altitude during decent
-static int32_t 	original_alt;
-
-
-
 ////////////////////////////////////////////////////////////////////////////////
 // IMU variables
 ////////////////////////////////////////////////////////////////////////////////
@@ -795,6 +794,8 @@ static float 			dTnav;
 static int16_t			superslow_loopCounter;
 // RTL Autoland Timer
 static uint32_t 		auto_land_timer;
+// disarms the copter while in Acro or Stabilzie mode after 30 seconds of no flight
+static uint8_t 			auto_disarming_counter;
 
 
 // Tracks if GPS is enabled based on statup routine
@@ -803,7 +804,7 @@ static bool				GPS_enabled 	= false;
 // Set true if we have new PWM data to act on from the Radio
 static bool				new_radio_frame;
 // Used to auto exit the in-flight leveler
-static int16_t 	auto_level_counter;
+static int16_t 			auto_level_counter;
 
 // Reference to the AP relay object - APM1 only
 AP_Relay relay;
@@ -866,6 +867,7 @@ void loop()
 
 		counter_one_herz++;
 
+		// trgger our 1 hz loop
 		if(counter_one_herz >= 50){
 			super_slow_loop();
 			counter_one_herz = 0;
@@ -1194,16 +1196,28 @@ static void slow_loop()
 		default:
 			slow_loopCounter = 0;
 			break;
-
 	}
 }
 
+#define AUTO_ARMING_DELAY 60
 // 1Hz loop
 static void super_slow_loop()
 {
 	if (g.log_bitmask & MASK_LOG_CUR)
 		Log_Write_Current();
 
+	// this function disarms the copter if it has been sitting on the ground for any moment of time greater than 30s
+	// but only of the control mode is manual
+	if((control_mode <= ACRO) && (g.rc_3.control_in == 0)){
+		auto_disarming_counter++;
+		if(auto_disarming_counter == AUTO_ARMING_DELAY){
+			init_disarm_motors();
+		}else if (auto_disarming_counter > AUTO_ARMING_DELAY){
+			auto_disarming_counter = AUTO_ARMING_DELAY + 1;
+		}
+	}else{
+		auto_disarming_counter = 0;
+	}
     gcs_send_message(MSG_HEARTBEAT);
     gcs_data_stream_send(1,3);
 	// agmatthews - USERHOOKS
@@ -1559,18 +1573,27 @@ void update_throttle_mode(void)
 					throttle_out = g.throttle_cruise + angle_boost + manual_boost;
 				#endif
 
-				// reset next_WP.alt and don't go below 1 meter
-				next_WP.alt = max(current_loc.alt, 100);
+				//force a reset of the altitude change
+				clear_new_altitude();
 
 				/*
-				Serial.printf("tar_alt: %d, actual_alt: %d \talt_err: %d, \t manb: %d\n",
+				int16_t iterm = g.pi_alt_hold.get_integrator();
+
+				Serial.printf("tar_alt: %d, actual_alt: %d \talt_err: %d, \t manb: %d, iterm %d\n",
 									next_WP.alt,
 									current_loc.alt,
 									altitude_error,
-									manual_boost);
+									manual_boost,
+									iterm);
 				//*/
+				reset_throttle_flag = true;
 
 			}else{
+				if(reset_throttle_flag)	{
+					set_new_altitude(max(current_loc.alt, 100));
+					reset_throttle_flag = false;
+				}
+
 				// 10hz, 			don't run up i term
 				if(invalid_throttle && motor_auto_armed == true){
 
@@ -1583,14 +1606,13 @@ void update_throttle_mode(void)
 					// clear the new data flag
 					invalid_throttle = false;
 					/*
-					Serial.printf("tar_alt: %d, actual_alt: %d \talt_err: %d, \tnav_thr: %d, \talt Int: %d, \trate_int %d \n",
+					Serial.printf("tar_alt: %d, actual_alt: %d \talt_err: %d, \tnav_thr: %d, \talt Int: %d\n",
 										next_WP.alt,
 										current_loc.alt,
 										altitude_error,
 										nav_throttle,
-										(int16_t)g.pi_alt_hold.get_integrator(),
-										(int16_t) g.pi_throttle.get_integrator());
-					*/
+										(int16_t)g.pi_alt_hold.get_integrator());
+					//*/
 				}
 
 				#if FRAME_CONFIG == HELI_FRAME
@@ -1638,23 +1660,26 @@ static void update_navigation()
 			break;
 
 		case RTL:
+			// We have reached Home
 			if((wp_distance <= g.waypoint_radius) || check_missed_wp()){
 				// if this value > 0, we are set to trigger auto_land after 30 seconds
 				set_mode(LOITER);
 				auto_land_timer = millis();
+				break;
+			}
 
-			}else if(current_loc.alt < (next_WP.alt - 300)){
-				// don't navigate if we are below our target alt
-				wp_control = LOITER_MODE;
+			// We wait until we've reached out new altitude before coming home
+			// Arg doesn't work, it
+			//if(alt_change_flag != REACHED_ALT){
+			//	wp_control = NO_NAV_MODE;
+			//}else{
+				wp_control = WP_MODE;
 
-			}else{
 				// calculates desired Yaw
 				#if FRAME_CONFIG ==	HELI_FRAME
 				update_auto_yaw();
 				#endif
-
-				wp_control = WP_MODE;
-			}
+			//}
 
 			// calculates the desired Roll and Pitch
 			update_nav_wp();
@@ -1690,12 +1715,9 @@ static void update_navigation()
 		case LAND:
 			wp_control 		= LOITER_MODE;
 
-			if(verify_land())  {  // JLN fix for auto land in RTL
-				set_mode(STABILIZE);
-			} else {
-				// calculates the desired Roll and Pitch
-				update_nav_wp();
-			}
+			// verify land will override WP_control if we are below
+			// a certain altitude
+			verify_land();
 
 			// calculates the desired Roll and Pitch
 			update_nav_wp();
@@ -1828,7 +1850,8 @@ static void update_altitude()
 			scale = (sonar_alt - 400) / 200;
 			scale = constrain(scale, 0, 1);
 
-			current_loc.alt = ((float)sonar_alt  * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt;
+			// solve for a blended altitude
+			current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt;
 
 			// solve for a blended climb_rate
 			climb_rate 		= ((float)sonar_rate * (1.0 - scale)) + (float)baro_rate * scale;
@@ -1841,7 +1864,6 @@ static void update_altitude()
 		}
 
 	}else{
-
 		// NO Sonar case
 		current_loc.alt = baro_alt + home.alt;
 		climb_rate 		= baro_rate;
@@ -1849,25 +1871,14 @@ static void update_altitude()
 
 	// manage bad data
 	climb_rate = constrain(climb_rate, -300, 300);
+
+	// update the target altitude
+	next_WP.alt = get_new_altitude();
 }
 
 static void
 adjust_altitude()
 {
-	/*
-	// old vert control
-	if(g.rc_3.control_in <= 200){
-		next_WP.alt -= 1;												// 1 meter per second
-		next_WP.alt = max(next_WP.alt, (current_loc.alt - 500));		// don't go less than 4 meters below current location
-		next_WP.alt = max(next_WP.alt, 100);							// don't go less than 1 meter
-		//manual_boost = (g.rc_3.control_in == 0) ? -20 : 0;
-
-	}else if (g.rc_3.control_in > 700){
-		next_WP.alt += 1;												// 1 meter per second
-		next_WP.alt = min(next_WP.alt, (current_loc.alt + 500));		// don't go more than 4 meters below current location
-		//manual_boost = (g.rc_3.control_in == 800) ? 20 : 0;
-	}*/
-
 	if(g.rc_3.control_in <= 180){
 		// we remove 0 to 100 PWM from hover
 		manual_boost = g.rc_3.control_in - 180;