This is how I right waypoints to EEPROM for now.

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

WaypointWriterCopterMega/EEPROM.pde

@@ -0,0 +1,105 @@
+
+void writePoints()
+{
+    int mem;
+	struct Location loc;
+
+
+    for (byte i = 0; i < waypoint_total; i++){
+
+		loc.id 			= (uint8_t) mission[i][0];
+		loc.options 	= (uint8_t) mission[i][1];
+		loc.p1 			= (uint8_t) mission[i][2];
+	    loc.alt 		= (long)(mission[i][3] * 100);
+		loc.lat 		= (long)(mission[i][4] * t7);
+    	loc.lng 		= (long)(mission[i][5] * t7);
+
+		switch(loc.id){
+			case MAV_CMD_NAV_WAYPOINT:
+    			//loc.p1 = (byte)mission[i][4];// wp_radius
+    			//loc.p1 = WP_RADIUS;
+    			break;
+
+			case MAV_CMD_CONDITION_YAW:
+		    	loc.alt = (long)mission[i][3];	// speed
+			    loc.lat = (long)mission[i][4];	// rotation direction
+    			loc.lng = (long)mission[i][5];	// target yaw in deg
+    			break;
+		}
+
+		set_wp_with_index(loc,(i+1));
+
+		/*
+    	Serial.print((i+1),DEC);
+    	Serial.print(": ");
+    	Serial.print(loc.id,DEC);
+    	Serial.print(", ");
+    	Serial.print(loc.p1,DEC);
+    	Serial.print(", ");
+    	Serial.print(loc.alt,DEC);
+    	Serial.print(", ");
+    	Serial.print(loc.lat,DEC);
+    	Serial.print(", ");
+    	Serial.println(loc.lng,DEC);
+		*/
+	}
+}
+
+
+struct Location get_wp_with_index(int i)
+{
+	struct Location temp;
+	long mem;
+
+	// Find out proper location in memory by using the start_byte position + the index
+	// --------------------------------------------------------------------------------
+	if (i > waypoint_total) {
+		temp.id = CMD_BLANK;
+	}else{
+		// 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);	// alt is stored in CM!
+
+		mem += 4;
+		temp.lat = (long)eeprom_read_dword((uint32_t*)mem);
+
+		mem += 4;
+		temp.lng = (long)eeprom_read_dword((uint32_t*)mem);
+	}
+	return temp;
+}
+
+void set_wp_with_index(struct Location temp, int i)
+{
+	i = constrain(i, 0, waypoint_total.get());
+	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);
+
+	mem++;
+	eeprom_write_dword((uint32_t *)	mem, temp.alt);	// alt is stored in CM!
+
+	mem += 4;
+	eeprom_write_dword((uint32_t *)	mem, temp.lat);
+
+	mem += 4;
+	eeprom_write_dword((uint32_t *)	mem, temp.lng);
+}
+
+
+

WaypointWriterCopterMega/WaypointWriterCopterMega.pde

@@ -0,0 +1,108 @@
+/*
+Ardupilot 1.0.1a MEGA Waypoint writer
+Use this release to manually upload waypoints
+*/
+#include "defines.h"
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <GCS_MAVLink.h>    // MAVLink GCS definitions
+
+FastSerialPort0(Serial);        // FTDI/console
+
+// EEPROM addresses
+#define EEPROM_MAX_ADDR		4096
+// parameters get the first 1KiB of EEPROM, remainder is for waypoints
+#define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP
+#define WP_SIZE 15
+
+// you can keep your missions stored here, just uncommment the mission to load: (only 1 at a time.)
+//#include "mission_example.h"
+#include "sparkfun.h"
+//#include "takeoff.h"
+
+//#include "SFO_T3.h"
+//#include "SFO_landMe.h"
+//#include "Basic_mission_example.h"
+
+const long t7 = 10000000;	// used to scale GPS values for EEPROM storage
+
+byte 	yaw_tracking = TRACK_NONE;			// no tracking, point at next wp, or at a target
+
+
+#define k_param_RTL_altitude 163
+#define k_param_waypoint_total 221
+#define k_param_waypoint_radius 224
+
+AP_Int16	RTL_altitude      ((int8_t)9,					k_param_RTL_altitude,     NULL);
+AP_Int8		waypoint_total    ((int8_t)0,                   k_param_waypoint_total,   NULL);
+AP_Int8		waypoint_radius   ((int8_t)WP_RADIUS,			k_param_waypoint_radius,  NULL);
+
+
+// You DON'T need to edit below this line
+// ----------------------------------
+#include <avr/io.h>
+#include <avr/eeprom.h>
+
+void setup()
+{
+	Serial.begin(38400);
+	delay(1000);
+	Serial.println("\nACM Waypoint writer 1.0.3 Public Alpha \n\n");
+
+	Serial.printf("Test: %d\n", (int)MAV_CMD_NAV_LAND);
+
+	//*
+	// number of waypoints, add 1 for home
+	waypoint_total.set_and_save((sizeof(mission) / 24));
+	waypoint_radius.set_and_save(WP_RADIUS);
+	RTL_altitude.set_and_save(ALT_TO_HOLD * 100);
+
+	writePoints();  			// saves Waypoint Array
+
+	delay(1000);
+
+	if(RTL_altitude < 0){
+		Serial.print("Hold current altitude above home after RTL.\n");
+	}else{
+		Serial.printf("Hold this altitude over home: %ld meters\n", (long)RTL_altitude.get());
+	}
+
+	Serial.printf("WP Radius: %ld meters\n", (long)RTL_altitude.get());
+	Serial.printf("WP Radius: %d meters\n", (int)waypoint_radius.get());
+	Serial.printf("total # of commands: %d\n", (int)waypoint_total.get());
+
+	report_wp(255);
+
+	//*/
+}
+
+void loop()
+{
+
+}
+
+
+void report_wp(byte index)
+{
+	if(index == 255){
+		for(byte i = 0; i <= waypoint_total; i++){
+			struct Location temp = get_wp_with_index(i);
+			print_wp(&temp, i);
+		}
+	}else{
+		struct Location temp = get_wp_with_index(index);
+		print_wp(&temp, index);
+	}
+}
+
+void print_wp(struct Location *cmd, byte index)
+{
+	Serial.printf_P(PSTR("command #: %d \tid:%d\top:%d\tp1:%d\tp2:%ld\tp3:%ld\tp4:%ld \n"),
+		(int)index,
+		(int)cmd->id,
+		(int)cmd->options,
+		(int)cmd->p1,
+		cmd->alt,
+		cmd->lat,
+		cmd->lng);
+}

WaypointWriterCopterMega/defines.h

@@ -0,0 +1,298 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+
+// ACM:
+// Motors
+#define RIGHT CH_1
+#define LEFT CH_2
+#define FRONT CH_3
+#define BACK CH_4
+#define RIGHTFRONT CH_7
+#define LEFTBACK CH_8
+#define MAX_SERVO_OUTPUT 2700
+
+// active altitude sensor
+// ----------------------
+#define SONAR 0
+#define BARO 1
+
+// Frame types
+#define PLUS_FRAME 0
+#define X_FRAME 1
+#define TRI_FRAME 2
+#define HEXAX_FRAME 3
+#define Y6_FRAME 4
+#define HEXAP_FRAME 5
+
+// Internal defines, don't edit and expect things to work
+// -------------------------------------------------------
+
+#define TRUE 1
+#define FALSE 0
+#define ToRad(x) (x*0.01745329252)	// *pi/180
+#define ToDeg(x) (x*57.2957795131)	// *180/pi
+
+#define DEBUG 0
+#define LOITER_RANGE 60 // for calculating power outside of loiter radius
+
+#define T6 1000000
+#define T7 10000000
+
+// GPS type codes - use the names, not the numbers
+#define GPS_PROTOCOL_NONE	-1
+#define GPS_PROTOCOL_NMEA	0
+#define GPS_PROTOCOL_SIRF	1
+#define GPS_PROTOCOL_UBLOX	2
+#define GPS_PROTOCOL_IMU	3
+#define GPS_PROTOCOL_MTK	4
+#define GPS_PROTOCOL_HIL	5
+#define GPS_PROTOCOL_MTK16	6
+
+// Radio channels
+// Note channels are from 0!
+//
+// XXX these should be CH_n defines from RC.h at some point.
+#define CH_1 0
+#define CH_2 1
+#define CH_3 2
+#define CH_4 3
+#define CH_5 4
+#define CH_6 5
+#define CH_7 6
+#define CH_8 7
+
+#define CH_ROLL CH_1
+#define CH_PITCH CH_2
+#define CH_THROTTLE CH_3
+#define CH_RUDDER CH_4
+#define CH_YAW CH_4
+
+// HIL enumerations
+#define HIL_PROTOCOL_XPLANE			1
+#define HIL_PROTOCOL_MAVLINK		2
+
+#define HIL_MODE_DISABLED			0
+#define HIL_MODE_ATTITUDE			1
+#define HIL_MODE_SENSORS			2
+
+// GCS enumeration
+#define GCS_PROTOCOL_STANDARD		0	// standard APM protocol
+#define GCS_PROTOCOL_LEGACY			1	// legacy ArduPilot protocol
+#define GCS_PROTOCOL_XPLANE			2	// X-Plane HIL simulation
+#define GCS_PROTOCOL_DEBUGTERMINAL	3	//Human-readable debug interface for use with a dumb terminal
+#define GCS_PROTOCOL_MAVLINK	        4	// binary protocol for qgroundcontrol
+#define GCS_PROTOCOL_NONE			-1	// No GCS output
+
+// Auto Pilot modes
+// ----------------
+#define STABILIZE 0			// hold level position
+#define ACRO 1				// rate control
+#define ALT_HOLD 2			// AUTO control
+#define SIMPLE 3			//
+#define AUTO 4				// AUTO control
+#define GCS_AUTO 5			// AUTO control
+#define LOITER 6			// Hold a single location
+#define RTL 7				// AUTO control
+#define NUM_MODES 8
+
+// YAW debug
+// ---------
+#define YAW_HOLD 0
+#define YAW_BRAKE 1
+#define YAW_RATE 2
+
+// CH_6 Tuning
+// -----------
+#define CH6_NONE 0
+#define CH6_STABLIZE_KP 1
+#define CH6_STABLIZE_KD 2
+#define CH6_BARO_KP 3
+#define CH6_BARO_KD 4
+#define CH6_SONAR_KP 5
+#define CH6_SONAR_KD 6
+#define CH6_Y6_SCALING 7
+
+// nav byte mask
+// -------------
+#define NAV_LOCATION 1
+#define NAV_ALTITUDE 2
+#define NAV_DELAY    4
+
+
+// Commands - Note that APM 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
+
+// Nav Yaw Tracking
+#define TRACK_NONE 		1
+#define TRACK_NEXT_WP 	2
+#define TRACK_TARGET_WP 4
+
+// Waypoint options
+#define WP_OPTION_ALT_RELATIVE 		1
+#define WP_OPTION_ALT_CHANGE 		2
+#define WP_OPTION_YAW 				4
+#define WP_OPTION_ALT_REQUIRED		8
+#define WP_OPTION_RELATIVE			16
+//#define WP_OPTION_					32
+//#define WP_OPTION_					64
+#define WP_OPTION_NEXT_CMD			128
+
+//repeating events
+#define NO_REPEAT 0
+#define CH_5_TOGGLE 1
+#define CH_6_TOGGLE 2
+#define CH_7_TOGGLE 3
+#define CH_8_TOGGLE 4
+#define RELAY_TOGGLE 5
+#define STOP_REPEAT 10
+
+//#define MAV_CMD_CONDITION_YAW 23
+
+//  GCS Message ID's
+#define MSG_ACKNOWLEDGE 0x00
+#define MSG_HEARTBEAT 0x01
+#define MSG_ATTITUDE 0x02
+#define MSG_LOCATION 0x03
+#define MSG_PRESSURE 0x04
+#define MSG_STATUS_TEXT 0x05
+#define MSG_PERF_REPORT 0x06
+#define MSG_MODE_CHANGE 0x07 //This is different than HEARTBEAT because it occurs only when the mode is actually changed
+#define MSG_VERSION_REQUEST 0x08
+#define MSG_VERSION 0x09
+#define MSG_EXTENDED_STATUS 0x0a
+#define MSG_CPU_LOAD 0x0b
+
+#define MSG_COMMAND_REQUEST 0x20
+#define MSG_COMMAND_UPLOAD 0x21
+#define MSG_COMMAND_LIST 0x22
+#define MSG_COMMAND_MODE_CHANGE 0x23
+#define MSG_CURRENT_WAYPOINT 0x24
+
+#define MSG_VALUE_REQUEST 0x30
+#define MSG_VALUE_SET 0x31
+#define MSG_VALUE 0x32
+
+#define MSG_PID_REQUEST 0x40
+#define MSG_PID_SET 0x41
+#define MSG_PID 0x42
+#define MSG_VFR_HUD 0x4a
+
+#define MSG_TRIM_STARTUP 0x50
+#define MSG_TRIM_MIN 0x51
+#define MSG_TRIM_MAX 0x52
+#define MSG_RADIO_OUT 0x53
+#define MSG_RADIO_IN  0x54
+
+#define MSG_RAW_IMU 0x60
+#define MSG_GPS_STATUS 0x61
+#define MSG_GPS_RAW 0x62
+
+#define MSG_SERVO_OUT 0x70
+
+#define MSG_PIN_REQUEST 0x80
+#define MSG_PIN_SET 0x81
+
+#define MSG_DATAFLASH_REQUEST 0x90
+#define MSG_DATAFLASH_SET 0x91
+
+#define MSG_EEPROM_REQUEST 0xa0
+#define MSG_EEPROM_SET 0xa1
+
+#define MSG_POSITION_CORRECT 0xb0
+#define MSG_ATTITUDE_CORRECT 0xb1
+#define MSG_POSITION_SET 0xb2
+#define MSG_ATTITUDE_SET 0xb3
+#define MSG_LOCAL_LOCATION 0xb4
+
+#define SEVERITY_LOW 1
+#define SEVERITY_MEDIUM 2
+#define SEVERITY_HIGH 3
+#define SEVERITY_CRITICAL 4
+
+//  Logging parameters
+#define LOG_INDEX_MSG			0xF0
+#define LOG_ATTITUDE_MSG		0x01
+#define LOG_GPS_MSG				0x02
+#define LOG_MODE_MSG			0X03
+#define LOG_CONTROL_TUNING_MSG	0X04
+#define LOG_NAV_TUNING_MSG		0X05
+#define LOG_PERFORMANCE_MSG		0X06
+#define LOG_RAW_MSG				0x07
+#define LOG_CMD_MSG				0x08
+#define LOG_CURRENT_MSG 		0x09
+#define LOG_STARTUP_MSG 		0x0A
+#define TYPE_AIRSTART_MSG		0x00
+#define TYPE_GROUNDSTART_MSG	0x01
+#define MAX_NUM_LOGS			50
+
+#define MASK_LOG_ATTITUDE_FAST 	(1<<0)
+#define MASK_LOG_ATTITUDE_MED 	(1<<1)
+#define MASK_LOG_GPS 			(1<<2)
+#define MASK_LOG_PM 			(1<<3)
+#define MASK_LOG_CTUN 			(1<<4)
+#define MASK_LOG_NTUN			(1<<5)
+#define MASK_LOG_MODE			(1<<6)
+#define MASK_LOG_RAW			(1<<7)
+#define MASK_LOG_CMD			(1<<8)
+#define MASK_LOG_CURRENT		(1<<9)
+#define MASK_LOG_SET_DEFAULTS	(1<<15)
+
+// Waypoint Modes
+// ----------------
+#define ABS_WP 0
+#define REL_WP 1
+
+// Command Queues
+// ---------------
+#define COMMAND_MUST 0
+#define COMMAND_MAY 1
+#define COMMAND_NOW 2
+
+// Events
+// ------
+#define EVENT_WILL_REACH_WAYPOINT 1
+#define EVENT_SET_NEW_WAYPOINT_INDEX 2
+#define EVENT_LOADED_WAYPOINT 3
+#define EVENT_LOOP 4
+
+// Climb rate calculations
+#define	ALTITUDE_HISTORY_LENGTH 8	//Number of (time,altitude) points to regress a climb rate from
+
+
+#define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO
+
+#define CURRENT_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*CURR_VOLT_DIV_RATIO
+#define CURRENT_AMPS(x) (x*(INPUT_VOLTAGE/1024.0))*CURR_AMP_DIV_RATIO
+
+#define	AIRSPEED_CH 7			// The external ADC channel for the airspeed sensor
+#define BATTERY_PIN1 0		        // These are the pins for the voltage dividers
+#define BATTERY_PIN2 1
+#define BATTERY_PIN3 2
+#define BATTERY_PIN4 3
+
+#define VOLTAGE_PIN_0 0 // These are the pins for current sensor: voltage
+#define CURRENT_PIN_1 1 // and current
+
+#define RELAY_PIN 47
+
+
+// sonar
+#define SonarToCm(x) (x*1.26)   // Sonar raw value to centimeters
+
+// Hardware Parameters
+#define SLIDE_SWITCH_PIN 40
+#define PUSHBUTTON_PIN 41
+
+#define A_LED_PIN 37			//36 = B,	37 = A,	35 = C
+#define B_LED_PIN 36
+#define C_LED_PIN 35
+
+
+// EEPROM addresses
+#define EEPROM_MAX_ADDR		4096
+// parameters get the first 1KiB of EEPROM, remainder is for waypoints
+#define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP
+#define WP_SIZE 15
+#define ONBOARD_PARAM_NAME_LENGTH 15
+#define MAX_WAYPOINTS  ((EEPROM_MAX_ADDR - WP_START_BYTE) / WP_SIZE) - 1 // - 1 to be safe

WaypointWriterCopterMega/event_example.h

@@ -0,0 +1,15 @@
+// Mission example:
+
+#define WP_RADIUS 3 		// What is the minimum distance to reach a waypoint?
+#define ALT_TO_HOLD	-1		// Altitude to hold above home in meters
+							// Enter -1 to maintain current altitude when returning to home
+
+
+// The mission:
+float mission[][5] = {
+{MAV_CMD_NAV_TAKEOFF,           0, 		6, 		0, 			0},					// pitch 20, Altitude meters
+{MAV_CMD_NAV_WAYPOINT, 			0, 		6, 		37.776849, -122.405752},		// go to waypoint
+{MAV_CMD_CONDITION_YAW, 		180, 	5,	 	1, 			1},					// 180°, 5 seconds, clockwise, relative
+{MAV_CMD_NAV_RETURN_TO_LAUNCH,	0, 		0, 		0, 			0},					// no options for RTL
+{MAV_CMD_NAV_LAND, 				0, 		0, 		0,          0},		//
+};

WaypointWriterCopterMega/mission_example.h

@@ -0,0 +1,35 @@
+// Mission example:
+
+#define WP_RADIUS 3 		// What is the minimum distance to reach a waypoint?
+#define ALT_TO_HOLD	-1		// Altitude to hold above home in meters
+							// Enter -1 to maintain current altitude when returning to home
+
+// The mission:
+float mission[][6] = {
+	//	CMD					options	P1		Alt		Lat			Long
+	{MAV_CMD_NAV_TAKEOFF,  	0, 		0,  	3.2,	0, 		  	0},					// 1
+	{MAV_CMD_NAV_WAYPOINT, 	0, 		0,		3.2, 	37.716899,	-122.381898},		// 2 turn 1
+	{MAV_CMD_NAV_WAYPOINT, 	0, 		0,		3.2, 	37.717475,	-122.381394},		// 3 turn 2
+	{MAV_CMD_NAV_WAYPOINT, 	0, 		0,		3.2, 	37.717149,	-122.380819},		// 4 turn 3
+	{MAV_CMD_NAV_WAYPOINT, 	0, 		2,		3.2, 	37.716592,	-122.381358},		// 5 turn 4 with delay
+	{MAV_CMD_NAV_WAYPOINT, 	0, 		5,		3.2, 	37.716752,	-122.381632},		// 6 Land WP with delay
+	{MAV_CMD_NAV_LAND, 	 	0,		0,		0,		0,          0},					// 7 LAND!
+};
+
+
+/*
+command #: 0 id:16 op:0 p1:0 p2:5007 	p3:377659180 p4:-1224329500
+command #: 1 id:22 op:0 p1:0 p2:220 	p3:0 		 p4:0
+command #: 2 id:16 op:0 p1:0 p2:220 	p3:377168992 p4:-1223819008
+command #: 3 id:16 op:0 p1:0 p2:220 	p3:377174752 p4:-1223813888
+command #: 4 id:16 op:0 p1:0 p2:220 	p3:377171488 p4:-1223808256
+command #: 5 id:16 op:0 p1:2 p2:220 	p3:377165920 p4:-1223813504
+command #: 6 id:16 op:0 p1:5 p2:220 	p3:377167520 p4:-1223816320
+command #: 7 id:21 op:0 p1:0 p2:0 		p3:0 		 p4:0
+
+*/
+
+
+/*
+Sparkfun
+*/

WaypointWriterCopterMega/sparkfun.h

@@ -0,0 +1,53 @@
+// Mission example:
+
+#define WP_RADIUS 5 		// What is the minimum distance to reach a waypoint?
+#define ALT_TO_HOLD	-1		// Altitude to hold above home in meters
+
+/*							// Enter -1 to maintain current altitude when returning to home
+// The mission:
+float mission[][6] = {
+	//	CMD					options	P1		Alt		Lat			Long
+	{MAV_CMD_NAV_TAKEOFF,  	0, 		0,  	3.0,	0, 		  	0},					// 1
+	{MAV_CMD_NAV_WAYPOINT, 	0, 		0,		3.0, 	40.065219,	-105.209760},		// 2 turn 1
+	{MAV_CMD_NAV_WAYPOINT, 	0, 		0,		3.0, 	40.064561,	-105.209798},		// 3 turn 2
+	{MAV_CMD_NAV_WAYPOINT, 	0, 		0,		3.0, 	40.064511,	-105.210402},		// 4 turn 3
+	{MAV_CMD_NAV_WAYPOINT, 	0, 		0,		3.0, 	40.065167,	-105.210453},		// 5 turn 4 with delay
+	{MAV_CMD_NAV_WAYPOINT, 	0, 		5,		3.0, 	40.065189,	-105.210007},		// 6 Land WP with delay
+	{MAV_CMD_NAV_LAND, 	 	0,		0,		0,		0,          0},					// 7 LAND!
+};
+*/
+///*
+							// Enter -1 to maintain current altitude when returning to home
+// The mission:
+float mission[][6] = {
+	//	CMD					options	P1		Alt		Lat			Long
+	{MAV_CMD_NAV_TAKEOFF,  	0, 		0,  	3.0,	0, 		  	0},					// 1
+	{MAV_CMD_NAV_WAYPOINT, 	0, 		0,		3.0, 	40.065219,	-105.209760},		//  turn 1
+
+	{MAV_CMD_NAV_WAYPOINT, 	0, 		4,		3.0, 	40.064561,	-105.209798},		//  turn 2 pause
+	{MAV_CMD_NAV_WAYPOINT, 	0, 		0,		3.0, 	40.064508,	-105.209808},		//  turn 2
+
+	{MAV_CMD_NAV_WAYPOINT, 	0, 		4,		3.0, 	40.064507,	-105.210303},		//  turn 3 pause
+	{MAV_CMD_NAV_WAYPOINT, 	0, 		0,		3.0, 	40.064524,	-105.210464},		//  turn 3
+
+	{MAV_CMD_NAV_WAYPOINT, 	0, 		1,		3.0, 	40.065092,	-105.210483},		//  turn 4 pause
+	{MAV_CMD_NAV_WAYPOINT, 	0, 		0,		3.0, 	40.065191,	-105.210349},		//  turn 4
+
+	{MAV_CMD_NAV_WAYPOINT, 	0, 		4,		3.0, 	40.065189,	-105.210007},		//  Land WP with delay
+	{MAV_CMD_NAV_LAND, 	 	0,		0,		0,		0,          0},					//  LAND!
+};
+//*/
+
+/*
+// The mission:
+float mission[][6] = {
+	//	CMD					options	P1		Alt		Lat			Long
+	{MAV_CMD_NAV_TAKEOFF,  	0, 		0,  	6.0,	0, 		  	0},					// 1
+	{MAV_CMD_NAV_WAYPOINT, 	0, 		0,		6.0, 	40.065219,	-105.209760},		// 2 turn 1
+	{MAV_CMD_NAV_WAYPOINT, 	0, 		0,		6.0, 	40.064561,	-105.209798},		// 3 turn 2
+	{MAV_CMD_NAV_WAYPOINT, 	0, 		0,		6.0, 	40.064511,	-105.210402},		// 4 turn 3
+	{MAV_CMD_NAV_WAYPOINT, 	0, 		0,		6.0, 	40.065167,	-105.210453},		// 5 turn 4 with delay
+	{MAV_CMD_NAV_WAYPOINT, 	0, 		5,		6.0, 	40.065189,	-105.210007},		// 6 Land WP with delay
+	{MAV_CMD_NAV_LAND, 	 	0,		0,		0,		0,          0},					// 7 LAND!
+};
+//*/

WaypointWriterCopterMega/takeoff.h

@@ -0,0 +1,12 @@
+// Mission example:
+
+#define WP_RADIUS 5 		// What is the minimum distance to reach a waypoint?
+#define ALT_TO_HOLD	-1		// Altitude to hold above home in meters
+
+							// Enter -1 to maintain current altitude when returning to home
+// The mission:
+float mission[][6] = {
+	//	CMD					options	P1		Alt		Lat			Long
+	{MAV_CMD_NAV_TAKEOFF,  	0, 		0,  	3.0,	0, 		  	0},					// 1
+	{MAV_CMD_NAV_LAND, 	 	0,		0,		0,		0,          0},					// 7 LAND!
+};