minor navigation updates, added new copter friendly altitude control for navigating waypoints.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1518 f9c3cf11-9bcb-44bc-f272-b75c42450872

ArduCopterMega/commands.pde

@@ -81,11 +81,9 @@ void increment_WP_index()
 {
 	if(wp_index < wp_total){
 		wp_index++;
-		Serial.print("MSG WP index is incremented to ");
+		Serial.printf_P(PSTR("WP index is incremented to %d\n"),wp_index);
-		Serial.println(wp_index,DEC);
 	}else{
-		Serial.print("MSG Failed to increment WP index of ");
+		Serial.printf_P(PSTR("WP Failed to incremented WP index of %d\n"),wp_index);
-		Serial.println(wp_index,DEC);
 	}
 }
 void decrement_WP_index()
@@ -104,6 +102,9 @@ void decrement_WP_index()
 void
 return_to_launch(void)
 {
+	//so we know where we are navigating from
+	next_WP = current_loc;
+
 	// home is WP 0
 	// ------------
 	wp_index = 0;
@@ -142,9 +143,7 @@ It looks to see what the next command type is and finds the last command.
 void 
 set_next_WP(struct Location *wp)
 {
-	//send_message(SEVERITY_LOW,"load WP");
+	Serial.printf_P(PSTR("set_next_WP, wp_index %d\n"), wp_index);
-	Serial.print("MSG set_next_WP, wp_index: ");
-	Serial.println(wp_index,DEC);
 	send_message(MSG_COMMAND, wp_index);
 
 	// copy the current WP into the OldWP slot
@@ -194,17 +193,14 @@ set_next_WP(struct Location *wp)
 // -------------------------------
 void init_home()
 {
-	Serial.println("MSG: init home");
+	Serial.printf_P(PSTR("init home\n"));
-
-	// Extra read just in case
-	// -----------------------
-	//GPS.Read();
 
 	// block until we get a good fix
 	// -----------------------------
 	while (!GPS.new_data || !GPS.fix) {
 		GPS.update();
 	}
+	
 	home.id 	= CMD_WAYPOINT;
 	home.lng 	= GPS.longitude;				// Lon * 10**7
 	home.lat 	= GPS.latitude;				// Lat * 10**7
@@ -213,8 +209,6 @@ void init_home()
 
 	// ground altitude in centimeters for pressure alt calculations
 	// ------------------------------------------------------------
-	//ground_alt 			= GPS.altitude;
-	//pressure_altitude 	= GPS.altitude;  // Set initial value for filter
 	save_EEPROM_pressure();
 
 	// Save Home to EEPROM

ArduCopterMega/navigation.pde

@@ -123,22 +123,21 @@ void calc_distance_error()
 // calculated at 50 hz
 void calc_altitude_error() 
 {
-	altitude_error 		= next_WP.alt - current_loc.alt;
+	if(control_mode == AUTO && offset_altitude != 0) {
+		// limit climb rates - we draw a straight line between first location and edge of wp_radius
+		target_altitude = next_WP.alt - ((wp_distance * offset_altitude) / (wp_totalDistance - wp_radius));
 		
-	// limit climb rates
+		// stay within a certain range
-	//target_altitude = next_WP.alt - ((float)((wp_distance -30) * offset_altitude) / (float)(wp_totalDistance - 30));
+		if(prev_WP.alt > next_WP.alt){
+			target_altitude = constrain(target_altitude, next_WP.alt, prev_WP.alt);
+		}else{
+			target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt);
+		}
+	} else {
+		target_altitude = next_WP.alt;
+	}
 
-	//if(prev_WP.alt > next_WP.alt){
+	altitude_error 	= target_altitude - current_loc.alt;
-//		target_altitude = constrain(target_altitude, next_WP.alt, prev_WP.alt);
-//	}else{
-//		target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt);
-//	}
-	
-	// calc the GPS/Abs pressure altitude
-	//if(GPS.fix)
-	//	pressure_altitude 	+= altitude_gain * (float)(GPS.altitude - pressure_altitude);
-	//current_loc.alt		= pressure_altitude;
-//	altitude_error 		= target_altitude - current_loc.alt;
 
 	//Serial.printf("s: %d %d t_alt %d\n", (int)current_loc.alt, (int)altitude_error, (int)target_altitude);
 }

ArduCopterMega/system.pde

@@ -211,6 +211,9 @@ void startup_ground(void)
 		Serial.println("*")
 	}
 	*/
+	// read the radio to set trims
+	// ---------------------------
+	trim_radio();
 
 	if (log_bitmask & MASK_LOG_CMD)
 		Log_Write_Startup(TYPE_GROUNDSTART_MSG);
@@ -228,13 +231,7 @@ void startup_ground(void)
 	
 	//IMU ground start
 	//------------------------
-#if GPS_PROTOCOL != GPS_PROTOCOL_IMU
 	init_pressure_ground();
-#endif
-
-	// read the radio to set trims
-	// ---------------------------
-	trim_radio();
 
 	// Warm up and read Gyro offsets
 	// -----------------------------