From afa8be20085d30b8976b68c4f3668e5b09c4c5ac Mon Sep 17 00:00:00 2001
From: jasonshort <jasonshort@f9c3cf11-9bcb-44bc-f272-b75c42450872>
Date: Sat, 5 Mar 2011 04:56:58 +0000
Subject: [PATCH] Added support for WP options

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1742 f9c3cf11-9bcb-44bc-f272-b75c42450872
---
 ArduCopterMega/commands.pde   | 22 ++++++++++++++++++++--
 ArduCopterMega/defines.h      | 10 +++++++++-
 ArduCopterMega/navigation.pde |  7 ++++---
 3 files changed, 33 insertions(+), 6 deletions(-)

diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde
index 89f7be5afe..5729d5f2a3 100644
--- a/ArduCopterMega/commands.pde
+++ b/ArduCopterMega/commands.pde
@@ -39,19 +39,27 @@ struct Location get_wp_with_index(int i)
 		// read WP position
 		mem = (WP_START_BYTE) + (i * WP_SIZE);
 		temp.id = eeprom_read_byte((uint8_t*)mem);
+
+		mem++;
+		temp.options = eeprom_read_byte((uint8_t*)mem);
+
 		mem++;
 		temp.p1 = eeprom_read_byte((uint8_t*)mem);
+
 		mem++;
 		temp.alt = (long)eeprom_read_dword((uint32_t*)mem);
+
 		mem += 4;
 		temp.lat = (long)eeprom_read_dword((uint32_t*)mem);
+
 		mem += 4;
 		temp.lng = (long)eeprom_read_dword((uint32_t*)mem);
 	}
 
 	// Add on home altitude if we are a nav command
-	if(temp.id < 50)
+	if(temp.options & WP_OPT_ALT_RELATIVE){
 		temp.alt += home.alt;
+	}
 
 	return temp;
 }
@@ -62,15 +70,25 @@ void set_wp_with_index(struct Location temp, int i)
 {
 	i = constrain(i, 0, g.waypoint_total.get());
 
-	if(i > 0 && temp.id < 50){
+	//if(i > 0 && temp.id < 50){
+	//	temp.options & WP_OPT_LOCATION
 		// remove home altitude if we are a nav command
+	//	temp.alt -= home.alt;
+	//}
+
+	// Store the location relatove to home
+	if(temp.options & WP_OPT_ALT_RELATIVE){
 		temp.alt -= home.alt;
 	}
 
+
 	uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
 
 	eeprom_write_byte((uint8_t *)	mem, temp.id);
 
+	mem++;
+	eeprom_write_byte((uint8_t *)	mem, temp.options);
+
 	mem++;
 	eeprom_write_byte((uint8_t *)	mem, temp.p1);
 
diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h
index 8ed5a44d87..bcaaad20e4 100644
--- a/ArduCopterMega/defines.h
+++ b/ArduCopterMega/defines.h
@@ -94,7 +94,15 @@
 #define LAND 9				// controlled decent rate
 #define NUM_MODES 10
 
-// Commands - Note that APM now uses a subset of the MAVLink protocol commands.  See enum MAV_CMD in the GCS_Mavlink library
+
+#define WP_OPT_ALT_RELATIVE 	(1<<0)
+#define WP_OPT_ALT_CHANGE 		(1<<1)
+#define WP_OPT_YAW				(1<<2)
+// ..
+#define WP_OPT_CMD_WAIT			(1<<7)
+
+
+// Commands - Note that APM (1<<9)now uses a subset of the MAVLink protocol commands.  See enum MAV_CMD in the GCS_Mavlink library
 #define CMD_BLANK 0 // there is no command stored in the mem location requested
 #define NO_COMMAND 0
 
diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde
index b224626f48..20aed72e5b 100644
--- a/ArduCopterMega/navigation.pde
+++ b/ArduCopterMega/navigation.pde
@@ -100,7 +100,10 @@ void calc_bearing_error()
 
 void calc_altitude_error()
 {
-	if(control_mode == AUTO && offset_altitude != 0) {
+	//if(control_mode == AUTO && offset_altitude != 0) {
+	if(next_WP.options & WP_OPT_ALT_CHANGE) {
+		target_altitude = next_WP.alt;
+	}else{
 		// limit climb rates - we draw a straight line between first location and edge of waypoint_radius
 		target_altitude = next_WP.alt - ((wp_distance * offset_altitude) / (wp_totalDistance - g.waypoint_radius));
 
@@ -110,8 +113,6 @@ void calc_altitude_error()
 		}else{
 			target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt);
 		}
-	}else{
-		target_altitude = next_WP.alt;
 	}
 
 	altitude_error 	= target_altitude - current_loc.alt;