Fix wp altitude issues

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

ArduCopterMega/ArduCopterMega.pde

@@ -532,7 +532,7 @@ void fast_loop()
 
 	#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_MODE != HIL_MODE_DISABLED
 		// HIL for a copter needs very fast update of the servo values
-		gcs.send_message(MSG_RADIO_OUT);
+		hil.send_message(MSG_RADIO_OUT);
 	#endif
 }
 

ArduCopterMega/commands.pde

@@ -70,6 +70,11 @@ struct Location get_command_with_index(int i)
 		temp.lng = (long)eeprom_read_dword((uint32_t*)mem); // lon is stored in decimal * 10,000,000
 	}
 
+	// Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative
+	if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & WP_OPTION_ALT_RELATIVE){
+		temp.alt += home.alt;
+	}
+
 	if(temp.options & WP_OPTION_RELATIVE){
 		// If were relative, just offset from home
 		temp.lat	+=	home.lat;

ArduCopterMega/commands_logic.pde

@@ -226,7 +226,11 @@ void do_takeoff()
 	Location temp		= current_loc;
 
 	// next_command.alt is a relative altitude!!!
-	temp.alt			= next_command.alt + home.alt;
+	if (next_command.options & WP_OPTION_ALT_RELATIVE) {
+		temp.alt			= next_command.alt + home.alt;
+	} else {
+		temp.alt 			= next_command.alt;
+	}
 
 	takeoff_complete	= false;			// set flag to use g_gps ground course during TO.  IMU will be doing yaw drift correction
 
@@ -238,8 +242,9 @@ void do_nav_wp()
 {
 	wp_control = WP_MODE;
 
+	// no longer needed as get_command_with_index takes care of this
 	// next_command.alt is a relative altitude!!!
-	next_command.alt += home.alt;
+	// next_command.alt += home.alt;
 
 	set_next_WP(&next_command);
 
@@ -467,7 +472,11 @@ void do_change_alt()
 {
 	Location temp	= next_WP;
 	condition_start = current_loc.alt;
-	condition_value = next_command.alt + home.alt;
+	if (next_command.options & WP_OPTION_ALT_RELATIVE) {
+		condition_value		= next_command.alt + home.alt;
+	} else {
+		condition_value		= next_command.alt;
+	}
 	temp.alt		= condition_value;
 	set_next_WP(&temp);
 }