updated climb rate control

Land detector

ArduCopter/commands_logic.pde

@@ -199,7 +199,7 @@ static void do_RTL(void)
 
 	//so we know where we are navigating from
 	// --------------------------------------
-	next_WP = current_loc;
+	next_WP 		= current_loc;
 
 	// Loads WP from Memory
 	// --------------------
@@ -265,19 +265,13 @@ static void do_land()
 {
 	wp_control = LOITER_MODE;
 
-	//Serial.println("dlnd ");
-
-	// not really used right now, might be good for debugging
 	land_complete		= false;
 
-	// used to limit decent rate
-	land_start 			= millis();
-
-	// used to limit decent rate
-	original_alt		= current_loc.alt;
-
 	// hold at our current location
 	set_next_WP(&current_loc);
+
+	// Set a new target altitude
+	set_new_altitude(0);
 }
 
 static void do_loiter_unlimited()
@@ -339,52 +333,43 @@ static bool verify_takeoff()
 		return false;
 	}
 	// are we above our target altitude?
-	return (current_loc.alt > next_WP.alt);
+	//return (current_loc.alt > next_WP.alt);
+	return (current_loc.alt > target_altitude);
 }
 
 static bool verify_land()
 {
+
 	static int32_t 	old_alt = 0;
 	static int16_t	velocity_land = -1;
 
-	// land at .62 meter per second
+	// landing detector
-	next_WP.alt  = original_alt - ((millis() - land_start) / 32);			// condition_value = our initial
+	if ((current_loc.alt - home.alt) > 300){
-
-	if (old_alt == 0)
 		old_alt = current_loc.alt;
-
-	if (velocity_land == -1)
 		velocity_land = 2000;
-
+	}else{
-
-	if ((current_loc.alt - home.alt) < 300){
 		// a LP filter used to tell if we have landed
 		// will drive to 0 if we are on the ground - maybe, the baro is noisy
-		velocity_land  = ((velocity_land * 7) + (old_alt - current_loc.alt)) / 8;
+		int16_t delta = (old_alt - current_loc.alt) << 3;
+		velocity_land = ((velocity_land * 7) + delta ) / 8;
 	}
 
+	//Serial.printf("velocity_land %d \n", velocity_land);
+
 	// remenber altitude for climb_rate
 	old_alt = current_loc.alt;
 
 	if ((current_loc.alt - home.alt) < 200){
 		// don't bank to hold position
 		wp_control = NO_NAV_MODE;
-
-		// Update by JLN for a safe AUTO landing
-		manual_boost = -10;
-		g.throttle_cruise += g.pi_alt_hold.get_integrator();
-		g.pi_alt_hold.reset_I();
-		g.pi_throttle.reset_I();
 	}
 
-	if((current_loc.alt - home.alt)  < 100 && velocity_land <= 100){
+	if((current_loc.alt - home.alt)  < 100 && velocity_land <= 50){
 		land_complete = true;
-		// reset manual_boost hack
-		manual_boost = 0;
 
 		// reset old_alt
 		old_alt = 0;
-		return false;
+		return true;
 	}
 	return false;
 }
@@ -394,7 +379,9 @@ static bool verify_nav_wp()
 	// Altitude checking
 	if(next_WP.options & MASK_OPTIONS_RELATIVE_ALT){
 		// we desire a certain minimum altitude
-		if (current_loc.alt > next_WP.alt){
+		//if (current_loc.alt > next_WP.alt){
+		if (current_loc.alt > target_altitude){
+
 			// we have reached that altitude
 			wp_verify_byte |= NAV_ALTITUDE;
 		}
@@ -592,7 +579,7 @@ static bool verify_wait_delay()
 static bool verify_change_alt()
 {
 	//Serial.printf("change_alt, ca:%d, na:%d\n", (int)current_loc.alt, (int)next_WP.alt);
-	if (condition_start < next_WP.alt){
+	if ((int32_t)condition_start < next_WP.alt){
 		// we are going higer
 		if(current_loc.alt > next_WP.alt){
 			return true;