Cleaned up guided mode

Reduced ADC filter to 3 from 6 to increase speed of filter.

ArduCopter/ArduCopter.pde

@@ -1464,7 +1464,6 @@ static void tuning(){
 
 static void update_nav_wp()
 {
-	// XXX Guided mode!!!
 	if(wp_control == LOITER_MODE){
 
 		// calc a pitch to the target

ArduCopter/commands.pde

@@ -1,5 +1,18 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+
+/*static void init_auto()
+{
+	//if (g.waypoint_index == g.waypoint_total) {
+	//	do_RTL();
+	//}
+
+	// initialize commands
+	// -------------------
+	init_commands();
+}
+*/
+
 static void init_commands()
 {
 	// zero is home, but we always load the next command (1), in the code.
@@ -20,18 +33,6 @@ static void clear_command_queue(){
 	next_command.id 	= NO_COMMAND;
 }
 
-static void init_auto()
-{
-	//if (g.waypoint_index == g.waypoint_total) {
-	//	Serial.println("ia_f");
-	//	do_RTL();
-	//}
-
-	// initialize commands
-	// -------------------
-	init_commands();
-}
-
 // Getters
 // -------
 static struct Location get_command_with_index(int i)
@@ -41,8 +42,6 @@ static struct Location get_command_with_index(int i)
 	// Find out proper location in memory by using the start_byte position + the index
 	// --------------------------------------------------------------------------------
 	if (i > g.waypoint_total) {
-		Serial.println("XCD");
-
 		// we do not have a valid command to load
 		// return a WP with a "Blank" id
 		temp.id = CMD_BLANK;
@@ -51,7 +50,6 @@ static struct Location get_command_with_index(int i)
 		return temp;
 
 	}else{
-		//Serial.println("LD");
 		// we can load a command, we don't process it yet
 		// read WP position
 		long mem = (WP_START_BYTE) + (i * WP_SIZE);
@@ -75,10 +73,9 @@ static struct Location get_command_with_index(int i)
 	}
 
 	// 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){
+	//if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & WP_OPTION_ALT_RELATIVE){
 		//temp.alt += home.alt;
-	}
+	//}
-	//Serial.println("ADD ALT");
 
 	if(temp.options & WP_OPTION_RELATIVE){
 		// If were relative, just offset from home
@@ -217,14 +214,12 @@ static void init_home()
 	home.lng 	= g_gps->longitude;				// Lon * 10**7
 	home.lat 	= g_gps->latitude;				// Lat * 10**7
 	//home.alt 	= max(g_gps->altitude, 0);		// we sometimes get negatives from GPS, not valid
-	home.alt 	= 0;							// this is a test
+	home.alt 	= 0;							// Home is always 0
 	home_is_set = true;
 
 	// to point yaw towards home until we set it with Mavlink
 	target_WP 	= home;
 
-	//Serial.printf_P(PSTR("gps alt: %ld\n"), home.alt);
-
 	// Save Home to EEPROM
 	// -------------------
 	// no need to save this to EPROM
@@ -233,8 +228,14 @@ static void init_home()
 
 	// Save prev loc this makes the calcs look better before commands are loaded
 	prev_WP = home;
+
 	// this is dangerous since we can get GPS lock at any time.
 	//next_WP = home;
+
+	// Load home for a default guided_WP
+	// -------------
+	guided_WP = home;
+	guided_WP.alt += g.RTL_altitude;
 }
 
 

ArduCopter/commands_logic.pde

@@ -105,17 +105,14 @@ static void handle_process_now()
 
 static void handle_no_commands()
 {
-	// we don't want to RTL yet. Maybe this will change in the future. RTL is kinda dangerous.
-	// use landing commands
 	/*
 	switch (control_mode){
 		default:
-			//set_mode(RTL);
+			set_mode(RTL);
 			break;
-	}
+	}*/
-	return;
+	//return;
-	*/
+	//Serial.println("Handle No CMDs");
-	Serial.println("Handle No CMDs");
 }
 
 /********************************************************************************/

ArduCopter/system.pde

@@ -407,7 +407,8 @@ static void set_mode(byte mode)
 			init_throttle_cruise();
 
 			// loads the commands from where we left off
-			init_auto();
+			//init_auto();
+			init_commands();
 
 			// do crosstrack correction
 			// XXX move to flight commands
@@ -440,6 +441,7 @@ static void set_mode(byte mode)
 			//xtrack_enabled = true;
 			init_throttle_cruise();
 			next_WP = current_loc;
+			set_next_WP(&guided_WP);
 			break;
 
 		case RTL:

libraries/AP_ADC/AP_ADC_ADS7844.h

@@ -9,7 +9,7 @@
 #define ADC_DATAIN      50    // MISO
 #define ADC_SPICLOCK    52    // SCK
 #define ADC_CHIP_SELECT 33    // PC4   9 // PH6  Puerto:0x08 Bit mask : 0x40
-#define ADC_FILTER_SIZE	6
+#define ADC_FILTER_SIZE	3
 
 #include "AP_ADC.h"
 #include <inttypes.h>