Merge https://code.google.com/p/ardupilot-mega

ArduCopter/APM_Config.h

@@ -39,7 +39,7 @@
 	*/
 
 #define ACCEL_ALT_HOLD 0		// disabled by default, work in progress
-#define ACCEL_ALT_HOLD_GAIN 12.0
+#define ACCEL_ALT_HOLD_GAIN 20.0
 // ACCEL_ALT_HOLD 1 to enable experimental alt_hold_mode
 
 // See the config.h and defines.h files for how to set this up!

ArduCopter/ArduCopter.pde

@@ -1,6 +1,6 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#define THISFIRMWARE "ArduCopter V2.0.49 Beta"
+#define THISFIRMWARE "ArduCopter V2.0.50 Beta"
 /*
 ArduCopter Version 2.0 Beta
 Authors:	Jason Short
@@ -1022,8 +1022,10 @@ void update_throttle_mode(void)
 
 		case THROTTLE_MANUAL:
 			if (g.rc_3.control_in > 0){
-				g.rc_3.servo_out = g.rc_3.control_in + get_angle_boost();
+				g.rc_3.servo_out = g.rc_3.control_in + get_angle_boost(g.rc_3.control_in);
 			}else{
+				g.pi_stabilize_roll.reset_I();
+				g.pi_stabilize_pitch.reset_I();
 				g.pi_rate_roll.reset_I();
 				g.pi_rate_pitch.reset_I();
 				g.rc_3.servo_out = 0;
@@ -1050,8 +1052,7 @@ void update_throttle_mode(void)
 				invalid_throttle = false;
 			}
 
-			// apply throttle control at 200 hz
+			g.rc_3.servo_out = g.throttle_cruise + nav_throttle + get_angle_boost(g.throttle_cruise);
-			g.rc_3.servo_out = g.throttle_cruise + nav_throttle + get_angle_boost() + alt_hold_velocity();
 			break;
 	}
 }
@@ -1171,14 +1172,6 @@ static void update_trig(void){
 	// 90° = cos_yaw:  1.00, sin_yaw:  0.00,
 	// 180 = cos_yaw:  0.00, sin_yaw: -1.00,
 	// 270 = cos_yaw: -1.00, sin_yaw:  0.00,
-
-
-	#if ACCEL_ALT_HOLD == 1
-	Vector3f accel_filt	= imu.get_accel_filtered();
-	accels_rot 	= dcm.get_dcm_matrix() * imu.get_accel_filtered();
-	accels_rot_sum += accels_rot.z;
-	accels_rot_count++;
-	#endif
 }
 
 // updated at 10hz

ArduCopter/Attitude.pde

@@ -98,28 +98,41 @@ get_nav_throttle(long z_error)
 	return (int)g.pi_throttle.get_pi(rate_error, .1);
 }
 
+#define ALT_ERROR_MAX2 300
+static int
+get_nav_throttle2(long z_error)
+{
+	if (z_error > ALT_ERROR_MAX2){
+		return g.pi_throttle.kP() * 80;
+
+	}else if (z_error < -ALT_ERROR_MAX2){
+		return g.pi_throttle.kP() * -60;
+
+	} else{
+		// limit error to prevent I term run up
+		z_error 		= constrain(z_error, -ALT_ERROR_MAX2, ALT_ERROR_MAX2);
+		int rate_error 	= g.pi_alt_hold.get_pi(z_error, .1); //_p = .85
+
+		rate_error 		= rate_error - altitude_rate;
+
+		// limit the rate
+		rate_error 		= constrain(rate_error, -100, 120);
+		return (int)g.pi_throttle.get_pi(rate_error, .1) + alt_hold_velocity();
+	}
+}
+
 static int
 get_rate_roll(long target_rate)
 {
-	long error;
+	long error	= (target_rate * 3.5) - (long)(degrees(omega.x) * 100.0);
-	target_rate 	= constrain(target_rate, -2500, 2500);
+	return g.pi_acro_roll.get_pi(error, G_Dt);
-	error			= (target_rate * 4.5) - (long)(degrees(omega.x) * 100.0);
-	target_rate 	= g.pi_rate_roll.get_pi(error, G_Dt);
-
-	// output control:
-	return (int)constrain(target_rate, -2500, 2500);
 }
 
 static int
 get_rate_pitch(long target_rate)
 {
-	long error;
+	long error	= (target_rate * 3.5) - (long)(degrees(omega.y) * 100.0);
-	target_rate 	= constrain(target_rate, -2500, 2500);
+	return  g.pi_acro_pitch.get_pi(error, G_Dt);
-	error			= (target_rate * 4.5) - (long)(degrees(omega.y) * 100.0);
-	target_rate 	= g.pi_rate_pitch.get_pi(error, G_Dt);
-
-	// output control:
-	return (int)constrain(target_rate, -2500, 2500);
 }
 
 static int
@@ -139,7 +152,7 @@ get_rate_yaw(long target_rate)
 static void reset_hold_I(void)
 {
 	g.pi_loiter_lat.reset_I();
-	g.pi_loiter_lat.reset_I();
+	g.pi_loiter_lon.reset_I();
 	g.pi_crosstrack.reset_I();
 }
 
@@ -189,19 +202,21 @@ get_nav_yaw_offset(int yaw_input, int reset)
 static int alt_hold_velocity()
 {
 	#if ACCEL_ALT_HOLD == 1
+		Vector3f accel_filt;
+		float error;
+
 		// subtract filtered Accel
-		float error	= abs(next_WP.alt - current_loc.alt);
+		error		= abs(next_WP.alt - current_loc.alt) - 25;
-
+		error		= min(error, 50.0);
-		error -= 100;
-		error = min(error, 200.0);
 		error		= max(error, 0.0);
-		error = 1 - (error/ 200.0);
+		error		= 1 - (error/ 50.0);
-		float sum = accels_rot_sum / (float)accels_rot_count;
 
-		accels_rot_sum = 0;
+		accel_filt	= imu.get_accel_filtered();
-		accels_rot_count = 0;
+		accels_rot 	= dcm.get_dcm_matrix() * imu.get_accel_filtered();
+		int output = (accels_rot.z + 9.81) * alt_hold_gain * error;   // alt_hold_gain  = 12
 
-		int output = (sum + 9.81) * alt_hold_gain * error;
+		//Serial.printf("s: %1.4f, g:%1.4f, e:%1.4f, o:%d\n",sum, alt_hold_gain, error, output);
+		return constrain(output, -70, 70);
 
 // fast rise
 //s: -17.6241, g:0.0000, e:1.0000, o:0
@@ -209,17 +224,15 @@ static int alt_hold_velocity()
 //s: -19.3193, g:0.0000, e:1.0000, o:0
 //s: -13.1310, g:47.8700, e:1.0000, o:-158
 
-		//Serial.printf("s: %1.4f, g:%1.4f, e:%1.4f, o:%d\n",sum, alt_hold_gain, error, output);
-		return output;
 	#else
 		return 0;
 	#endif
 }
 
-static int get_angle_boost()
+static int get_angle_boost(int value)
 {
 	float temp = cos_pitch_x * cos_roll_x;
 	temp = 1.0 - constrain(temp, .5, 1.0);
-	return (int)(temp * g.throttle_cruise);
+	return (int)(temp * value);
 }
 

ArduCopter/Parameters.h

@@ -17,7 +17,7 @@ public:
 	// The increment will prevent old parameters from being used incorrectly
 	// by newer code.
 	//
-	static const uint16_t k_format_version = 111;
+	static const uint16_t k_format_version = 112;
 
 	// The parameter software_type is set up solely for ground station use
 	// and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
@@ -156,7 +156,7 @@ public:
 	//
 	// 240: PI/D Controllers
 	//
-	k_param_pi_rate_roll = 240,
+	k_param_pi_rate_roll = 235,
 	k_param_pi_rate_pitch,
 	k_param_pi_rate_yaw,
 	k_param_pi_stabilize_roll,
@@ -169,6 +169,8 @@ public:
 	k_param_pi_alt_hold,
 	k_param_pi_throttle,
 	k_param_pi_crosstrack,
+	k_param_pi_acro_roll,
+	k_param_pi_acro_pitch,
 
 
     // 254,255: reserved
@@ -286,6 +288,9 @@ public:
 	APM_PI		pi_throttle;
 	APM_PI		pi_crosstrack;
 
+	APM_PI		pi_acro_roll;
+	APM_PI		pi_acro_pitch;
+
 	uint8_t		junk;
 
 	// Note: keep initializers here in the same order as they are declared above.
@@ -396,6 +401,9 @@ public:
 	pi_throttle			(k_param_pi_throttle,			PSTR("THR_RATE_"),	THROTTLE_P,			THROTTLE_I,			THROTTLE_IMAX),
 	pi_crosstrack		(k_param_pi_crosstrack,			PSTR("XTRACK_"),	XTRACK_P,			XTRACK_I,			XTRACK_IMAX),
 
+	pi_acro_roll		(k_param_pi_acro_roll,			PSTR("ACRO_RLL_"),	ACRO_ROLL_P,		ACRO_ROLL_I,		ACRO_ROLL_IMAX * 100),
+	pi_acro_pitch		(k_param_pi_acro_pitch,			PSTR("ACRO_PIT_"),	ACRO_PITCH_P,		ACRO_PITCH_I,		ACRO_PITCH_IMAX * 100),
+
 	junk(0)		// XXX just so that we can add things without worrying about the trailing comma
 	{
 	}

ArduCopter/config.h

@@ -395,7 +395,30 @@
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
-// Rate Control
+// Acro Rate Control
+//
+#ifndef ACRO_ROLL_P
+# define ACRO_ROLL_P         0.145
+#endif
+#ifndef ACRO_ROLL_I
+# define ACRO_ROLL_I         0.0
+#endif
+#ifndef ACRO_ROLL_IMAX
+# define ACRO_ROLL_IMAX	 	15			// degrees
+#endif
+
+#ifndef ACRO_PITCH_P
+# define ACRO_PITCH_P       0.145
+#endif
+#ifndef ACRO_PITCH_I
+# define ACRO_PITCH_I		0 //0.18
+#endif
+#ifndef ACRO_PITCH_IMAX
+# define ACRO_PITCH_IMAX   	15			// degrees
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Stabilize Rate Control
 //
 #ifndef RATE_ROLL_P
 # define RATE_ROLL_P         0.145

ArduCopter/control_modes.pde

@@ -139,8 +139,12 @@ static void auto_trim()
 			led_mode = NORMAL_LEDS;
 			clear_leds();
 			imu.save();
+
 			//Serial.println("Done");
 			auto_level_counter = 0;
+
+			// set TC
+			init_throttle_cruise();
 		}
 	}
 }

ArduCopter/motors.pde

@@ -2,8 +2,8 @@
 
 #define ARM_DELAY 10	// one second
 #define DISARM_DELAY 10	// one second
-#define LEVEL_DELAY 120 // twelve seconds
+#define LEVEL_DELAY 70 // twelve seconds
-#define AUTO_LEVEL_DELAY 150 // twentyfive seconds
+#define AUTO_LEVEL_DELAY 90 // twentyfive seconds
 
 
 // called at 10hz

ArduCopter/navigation.pde

@@ -92,19 +92,49 @@ static void calc_loiter(int x_error, int y_error)
 	nav_lon			= constrain(nav_lon, -3500, 3500);
 }
 
+static void calc_loiter2(int x_error, int y_error)
+{
+	static int last_x_error = 0;
+	static int last_y_error = 0;
+
+	x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);
+	y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX);
+
+	int x_target_speed = g.pi_loiter_lon.get_pi(x_error, dTnav);
+	int y_target_speed = g.pi_loiter_lat.get_pi(y_error, dTnav);
+
+	// find the rates:
+	x_actual_speed 	= (float)(last_x_error - x_error)/dTnav;
+	y_actual_speed 	= (float)(last_y_error - y_error)/dTnav;
+
+	// save speeds
+	last_x_error	= x_error;
+	last_y_error	= y_error;
+
+	y_rate_error 	= y_target_speed - y_actual_speed; // 413
+	y_rate_error 	= constrain(y_rate_error, -250, 250);	// added a rate error limit to keep pitching down to a minimum
+	nav_lat		 	= g.pi_nav_lat.get_pi(y_rate_error, dTnav);
+	nav_lat			= constrain(nav_lat, -3500, 3500);
+
+	x_rate_error 	= x_target_speed - x_actual_speed;
+	x_rate_error 	= constrain(x_rate_error, -250, 250);
+	nav_lon		 	= g.pi_nav_lon.get_pi(x_rate_error, dTnav);
+	nav_lon			= constrain(nav_lon, -3500, 3500);
+}
+
 // nav_roll, nav_pitch
 static void calc_loiter_pitch_roll()
 {
 
-	float temp  	 = radians((float)(9000 - (dcm.yaw_sensor))/100.0);
+	//float temp  	 = radians((float)(9000 - (dcm.yaw_sensor))/100.0);
-	float _cos_yaw_x = cos(temp);
+	//float _cos_yaw_x = cos(temp);
-	float _sin_yaw_y = sin(temp);
+	//float _sin_yaw_y = sin(temp);
 
-//	Serial.printf("ys %ld, cyx %1.4f, _cyx %1.4f\n", dcm.yaw_sensor, cos_yaw_x, _cos_yaw_x);
+	//Serial.printf("ys %ld, cx %1.4f, _cx %1.4f | sy %1.4f, _sy %1.4f\n", dcm.yaw_sensor, cos_yaw_x, _cos_yaw_x, sin_yaw_y, _sin_yaw_y);
 
 	// rotate the vector
-	nav_roll 	=  (float)nav_lon * _sin_yaw_y - (float)nav_lat * _cos_yaw_x;
+	nav_roll 	=  (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;
-	nav_pitch 	=  (float)nav_lon * _cos_yaw_x + (float)nav_lat * _sin_yaw_y;
+	nav_pitch 	=  (float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y;
 
 	// flip pitch because forward is negative
 	nav_pitch = -nav_pitch;

ArduCopter/radio.pde

@@ -4,6 +4,13 @@
 // ----------------------------------------------------------------------------
 static byte failsafeCounter = 0;		// we wait a second to take over the throttle and send the plane circling
 
+static void default_dead_zones()
+{
+	g.rc_1.set_dead_zone(60);
+	g.rc_2.set_dead_zone(60);
+	g.rc_3.set_dead_zone(60);
+	g.rc_4.set_dead_zone(200);
+}
 
 static void init_rc_in()
 {
@@ -29,14 +36,9 @@ static void init_rc_in()
 	g.rc_4.dead_zone = 300;
 	*/
 
-	g.rc_1.set_dead_zone(60);
-	g.rc_2.set_dead_zone(60);
-	g.rc_3.set_dead_zone(60);
-	g.rc_4.set_dead_zone(200);
 
 	//set auxiliary ranges
 	g.rc_5.set_range(0,1000);
-	g.rc_5.set_filter(false);
 	g.rc_6.set_range(0,1000);
 	g.rc_7.set_range(0,1000);
 	g.rc_8.set_range(0,1000);

ArduCopter/system.pde

@@ -134,6 +134,9 @@ static void init_ardupilot()
 		// save the new format version
 		g.format_version.set_and_save(Parameters::k_format_version);
 
+		// save default radio values
+		default_dead_zones();
+
 		Serial.printf_P(PSTR("Please Run Setup...\n"));
 		while (true) {
 			delay(1000);
@@ -150,6 +153,8 @@ static void init_ardupilot()
 		}
 
 	}else{
+		// save default radio values
+		//default_dead_zones();
 
 	    // Load all auto-loaded EEPROM variables
 	    AP_Var::load_all();

ArduPlane/ArduPlane.pde

@@ -266,10 +266,11 @@ static long	crosstrack_bearing;					// deg * 100 : 0 to 360 desired angle of pla
 static float	nav_gain_scaler 		= 1;		// Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1?
 static long    hold_course       	 	= -1;		// deg * 100 dir of plane
 
-static byte	command_must_index;					// current command memory location
+static byte	command_index;						// current command memory location
-static byte	command_may_index;					// current command memory location
+static byte	nav_command_index;					// active nav command memory location
-static byte	command_must_ID;					// current command ID
+static byte	non_nav_command_index;				// active non-nav command memory location
-static byte	command_may_ID;						// current command ID
+static byte	nav_command_ID		= NO_COMMAND;	// active nav command ID
+static byte	non_nav_command_ID	= NO_COMMAND;	// active non-nav command ID
 
 // Airspeed
 // --------
@@ -360,8 +361,9 @@ static struct 	Location home;						// home location
 static struct 	Location prev_WP;					// last waypoint
 static struct 	Location current_loc;				// current location
 static struct 	Location next_WP;					// next waypoint
-static struct 	Location next_command;				// command preloaded
 static struct  	Location guided_WP;					// guided mode waypoint
+static struct 	Location next_nav_command;			// command preloaded
+static struct 	Location next_nonnav_command;		// command preloaded
 static long 	target_altitude;					// used for altitude management between waypoints
 static long 	offset_altitude;					// used for altitude management between waypoints
 static bool	home_is_set; 						// Flag for if we have g_gps lock and have set the home location
@@ -739,7 +741,7 @@ static void update_current_flight_mode(void)
 	if(control_mode == AUTO){
 		crash_checker();
 
-		switch(command_must_ID){
+		switch(nav_command_ID){
 			case MAV_CMD_NAV_TAKEOFF:
 				if (hold_course > -1) {
 					calc_nav_roll();

ArduPlane/GCS_Mavlink.pde

@@ -532,7 +532,7 @@ static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
 {
     mavlink_msg_waypoint_current_send(
         chan,
-        g.waypoint_index);
+        g.command_index);
 }
 
 static void NOINLINE send_statustext(mavlink_channel_t chan)
@@ -838,7 +838,7 @@ GCS_MAVLINK::update(void)
     }
 
     if (waypoint_receiving &&
-        waypoint_request_i <= (unsigned)g.waypoint_total) {
+        waypoint_request_i <= (unsigned)g.command_total) {
         send_message(MSG_NEXT_WAYPOINT);
     }
 
@@ -1278,7 +1278,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             mavlink_msg_waypoint_count_send(
                 chan,msg->sysid,
                 msg->compid,
-                g.waypoint_total + 1); // + home
+                g.command_total + 1); // + home
 
             waypoint_timelast_send   = millis();
             waypoint_sending         = true;
@@ -1304,7 +1304,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
  				break;
 
             // send waypoint
-            tell_command = get_wp_with_index(packet.seq);
+            tell_command = get_cmd_with_index(packet.seq);
 
             // set frame of waypoint
             uint8_t frame;
@@ -1320,7 +1320,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             // time that the mav should loiter in milliseconds
             uint8_t current = 0; // 1 (true), 0 (false)
 
-			if (packet.seq == (uint16_t)g.waypoint_index)
+			if (packet.seq == (uint16_t)g.command_index)
             	current = 1;
 
             uint8_t autocontinue = 1; // 1 (true), 0 (false)
@@ -1435,8 +1435,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             mavlink_msg_waypoint_clear_all_decode(msg, &packet);
 			if (mavlink_check_target(packet.target_system, packet.target_component)) break;
 
-            // clear all waypoints
+            // clear all commands
-            g.waypoint_total.set_and_save(0);
+            g.command_total.set_and_save(0);
 
             // note that we don't send multiple acks, as otherwise a
             // GCS that is doing a clear followed by a set may see
@@ -1455,7 +1455,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             // set current command
             change_command(packet.seq);
 
-            mavlink_msg_waypoint_current_send(chan, g.waypoint_index);
+            mavlink_msg_waypoint_current_send(chan, g.command_index);
             break;
         }
 
@@ -1470,7 +1470,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             if (packet.count > MAX_WAYPOINTS) {
                 packet.count = MAX_WAYPOINTS;
             }
-            g.waypoint_total.set_and_save(packet.count - 1);
+            g.command_total.set_and_save(packet.count - 1);
 
             waypoint_timelast_receive = millis();
             waypoint_receiving   = true;
@@ -1645,13 +1645,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                     goto mission_failed;
                 }
 
-                set_wp_with_index(tell_command, packet.seq);
+                set_cmd_with_index(tell_command, packet.seq);
 
 				// update waypoint receiving state machine
 				waypoint_timelast_receive = millis();
 				waypoint_request_i++;
 
-				if (waypoint_request_i > (uint16_t)g.waypoint_total){
+				if (waypoint_request_i > (uint16_t)g.command_total){
 					mavlink_msg_waypoint_ack_send(
 						chan,
 						msg->sysid,
@@ -1988,7 +1988,7 @@ void
 GCS_MAVLINK::queued_waypoint_send()
 {
     if (waypoint_receiving &&
-        waypoint_request_i <= (unsigned)g.waypoint_total) {
+        waypoint_request_i <= (unsigned)g.command_total) {
         mavlink_msg_waypoint_request_send(
             chan,
             waypoint_dest_sysid,

ArduPlane/Log.pde

@@ -380,15 +380,15 @@ static void Log_Write_Startup(byte type)
 	DataFlash.WriteByte(HEAD_BYTE2);
 	DataFlash.WriteByte(LOG_STARTUP_MSG);
 	DataFlash.WriteByte(type);
-	DataFlash.WriteByte(g.waypoint_total);
+	DataFlash.WriteByte(g.command_total);
 	DataFlash.WriteByte(END_BYTE);
 
 	// create a location struct to hold the temp Waypoints for printing
-	struct Location cmd = get_wp_with_index(0);
+	struct Location cmd = get_cmd_with_index(0);
 		Log_Write_Cmd(0, &cmd);
 
-	for (int i = 1; i <= g.waypoint_total; i++){
+	for (int i = 1; i <= g.command_total; i++){
-		cmd = get_wp_with_index(i);
+		cmd = get_cmd_with_index(i);
 		Log_Write_Cmd(i, &cmd);
 	}
 }

ArduPlane/Parameters.h

@@ -162,8 +162,8 @@ public:
         // 220: Waypoint data
         //
         k_param_waypoint_mode = 220,
-        k_param_waypoint_total,
+        k_param_command_total,
-        k_param_waypoint_index,
+        k_param_command_index,
         k_param_waypoint_radius,
         k_param_loiter_radius,
 
@@ -254,8 +254,8 @@ public:
     // Waypoints
     //
     AP_Int8     waypoint_mode;
-    AP_Int8     waypoint_total;
+    AP_Int8     command_total;
-    AP_Int8     waypoint_index;
+    AP_Int8     command_index;
     AP_Int8     waypoint_radius;
     AP_Int8     loiter_radius;
 
@@ -371,8 +371,8 @@ public:
         airspeed_offset			(0,				            k_param_airspeed_offset,		PSTR("ARSPD_OFFSET")),
 
         /* XXX waypoint_mode missing here */
-        waypoint_total          (0,                         k_param_waypoint_total,         PSTR("WP_TOTAL")),
+        command_total           (0,                         k_param_command_total,         PSTR("CMD_TOTAL")),
-        waypoint_index          (0,                         k_param_waypoint_index,         PSTR("WP_INDEX")),
+        command_index           (0,                         k_param_command_index,         PSTR("CMD_INDEX")),
         waypoint_radius         (WP_RADIUS_DEFAULT,         k_param_waypoint_radius,        PSTR("WP_RADIUS")),
         loiter_radius           (LOITER_RADIUS_DEFAULT,     k_param_loiter_radius,          PSTR("WP_LOITER_RAD")),
 

ArduPlane/commands.pde

@@ -1,22 +1,45 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+/* Functions in this file:
+	void init_commands()
+	void update_auto()
+	void reload_commands_airstart()
+	struct Location get_cmd_with_index(int i)
+	void set_cmd_with_index(struct Location temp, int i)
+	void increment_cmd_index()
+	void decrement_cmd_index()
+	long read_alt_to_hold()
+	void set_next_WP(struct Location *wp)
+	void set_guided_WP(void)
+	void init_home()
+************************************************************ 
+*/
+
 static void init_commands()
 {
-	//read_EEPROM_waypoint_info();
+    g.command_index.set_and_save(0);
-    g.waypoint_index.set_and_save(0);
+	nav_command_ID	= NO_COMMAND;
-	command_must_index	= 0;
+	non_nav_command_ID	= NO_COMMAND;
-	command_may_index	= 0;
+	next_nav_command.id 	= CMD_BLANK;
-	next_command.id 	= CMD_BLANK;
 }
 
 static void update_auto()
 {
-	if (g.waypoint_index >= g.waypoint_total) {
+	if (g.command_index >= g.command_total) {
 		handle_no_commands();
-		if(g.waypoint_total == 0) {
+		if(g.command_total == 0) {
 			next_WP.lat 		= home.lat + 1000;	// so we don't have bad calcs
 			next_WP.lng 		= home.lng + 1000;	// so we don't have bad calcs
 		}
+	} else {
+    	if(g.command_index != 0) {
+    		g.command_index = nav_command_index;
+    		nav_command_index--;
+    	}
+		nav_command_ID	= NO_COMMAND;
+		non_nav_command_ID	= NO_COMMAND;
+		next_nav_command.id 	= CMD_BLANK;
+		process_next_command();
 	}
 }
 
@@ -24,20 +47,20 @@ static void update_auto()
 static void reload_commands_airstart()
 {
 	init_commands();
-	g.waypoint_index.load();        // XXX can we assume it's been loaded already by ::load_all?
+	g.command_index.load();        // XXX can we assume it's been loaded already by ::load_all?
-	decrement_WP_index();
+	decrement_cmd_index();
 }
 
 // Getters
 // -------
-static struct Location get_wp_with_index(int i)
+static struct Location get_cmd_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 > g.waypoint_total) {
+	if (i > g.command_total) {
 		temp.id = CMD_BLANK;
 	}else{
 		// read WP position
@@ -70,9 +93,9 @@ static struct Location get_wp_with_index(int i)
 
 // Setters
 // -------
-static void set_wp_with_index(struct Location temp, int i)
+static void set_cmd_with_index(struct Location temp, int i)
 {
-	i = constrain(i, 0, g.waypoint_total.get());
+	i = constrain(i, 0, g.command_total.get());
 	uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
 
 	// Set altitude options bitmask
@@ -101,17 +124,17 @@ static void set_wp_with_index(struct Location temp, int i)
 	eeprom_write_dword((uint32_t *)	mem, temp.lng);
 }
 
-static void increment_WP_index()
+static void increment_cmd_index()
 {
-    if (g.waypoint_index <= g.waypoint_total) {
+    if (g.command_index <= g.command_total) {
-        g.waypoint_index.set_and_save(g.waypoint_index + 1);
+        g.command_index.set_and_save(g.command_index + 1);
 	}
 }
 
-static void decrement_WP_index()
+static void decrement_cmd_index()
 {
-    if (g.waypoint_index > 0) {
+    if (g.command_index > 0) {
-        g.waypoint_index.set_and_save(g.waypoint_index - 1);
+        g.command_index.set_and_save(g.command_index - 1);
     }
 }
 
@@ -225,9 +248,9 @@ void init_home()
 
     gcs_send_text_fmt(PSTR("gps alt: %lu"), (unsigned long)home.alt);
 
-	// Save Home to EEPROM
+	// Save Home to EEPROM - Command 0
 	// -------------------
-	set_wp_with_index(home, 0);
+	set_cmd_with_index(home, 0);
 
 	// Save prev loc
 	// -------------

ArduPlane/commands_logic.pde

@@ -4,13 +4,14 @@
 // Command Event Handlers
 /********************************************************************************/
 static void
-handle_process_must()
+handle_process_nav_cmd()
 {
 	// reset navigation integrators
 	// -------------------------
 	reset_I();
 
-	switch(next_command.id){
+		gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nav_command.id);
+	switch(next_nav_command.id){
 
 		case MAV_CMD_NAV_TAKEOFF:
 			do_takeoff();
@@ -46,9 +47,10 @@ handle_process_must()
 }
 
 static void
-handle_process_may()
+handle_process_condition_command()
 {
-	switch(next_command.id){
+	gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id);
+	switch(next_nonnav_command.id){
 
 		case MAV_CMD_CONDITION_DELAY:
 			do_wait_delay();
@@ -62,15 +64,14 @@ handle_process_may()
 			do_change_alt();
 			break;
 
-	/*	case MAV_CMD_NAV_LAND_OPTIONS:	//    TODO - Add the command or equiv to MAVLink (repair in verify_may() also)
+	/*	case MAV_CMD_NAV_LAND_OPTIONS:	//    TODO - Add the command or equiv to MAVLink (repair in verify_condition() also)
 			gcs_send_text_P(SEVERITY_LOW,PSTR("Landing options set"));
 
 			// pitch in deg, airspeed  m/s, throttle %, track WP 1 or 0
-			landing_pitch 		= next_command.lng * 100;
+			landing_pitch 		= next_nav_command.lng * 100;
-			g.airspeed_cruise   =  next_command.alt * 100;
+			g.airspeed_cruise   =  next_nav_command.alt * 100;
-			g.throttle_cruise   = next_command.lat;
+			g.throttle_cruise   = next_nav_command.lat;
-			landing_distance 	= next_command.p1;
+			landing_distance 	= next_nav_command.p1;
-			//landing_roll 	= command.lng;
 
 			SendDebug_P("MSG: throttle_cruise = ");
 			SendDebugln(g.throttle_cruise,DEC);
@@ -82,9 +83,10 @@ handle_process_may()
 	}
 }
 
-static void handle_process_now()
+static void handle_process_do_command()
 {
-	switch(next_command.id){
+	gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id);
+	switch(next_nonnav_command.id){
 
 		case MAV_CMD_DO_JUMP:
 			do_jump();
@@ -118,18 +120,23 @@ static void handle_process_now()
 
 static void handle_no_commands()
 {
-	next_command = home;
+	gcs_send_text_fmt(PSTR("Returning to Home"));
-	next_command.alt = read_alt_to_hold();
+	next_nav_command = home;
-	next_command.id = MAV_CMD_NAV_LOITER_UNLIM;
+	next_nav_command.alt = read_alt_to_hold();
+	next_nav_command.id = MAV_CMD_NAV_LOITER_UNLIM;
+	nav_command_ID = MAV_CMD_NAV_LOITER_UNLIM;
+	non_nav_command_ID = WAIT_COMMAND;
+	handle_process_nav_cmd();
+	
 }
 
 /********************************************************************************/
 // Verify command Handlers
 /********************************************************************************/
 
-static bool verify_must()
+static bool verify_nav_command()	// Returns true if command complete
 {
-	switch(command_must_ID) {
+	switch(nav_command_ID) {
 
 		case MAV_CMD_NAV_TAKEOFF:
 			return verify_takeoff();
@@ -160,15 +167,15 @@ static bool verify_must()
 			break;
 
 		default:
-			gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_must: Invalid or no current Nav cmd"));
+			gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_nav: Invalid or no current Nav cmd"));
 			return false;
 			break;
 	}
 }
 
-static bool verify_may()
+static bool verify_condition_command()		// Returns true if command complete
 {
-	switch(command_may_ID) {
+	switch(non_nav_command_ID) {
     case NO_COMMAND:
         break;
 
@@ -184,8 +191,13 @@ static bool verify_may()
         return verify_change_alt();
         break;
         
+    case WAIT_COMMAND:
+        return 0;
+        break;
+        
+
     default:
-        gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_may: Invalid or no current Condition cmd"));
+        gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Invalid or no current Condition cmd"));
         break;
 	}
     return false;
@@ -212,12 +224,12 @@ static void do_RTL(void)
 
 static void do_takeoff()
 {
-	set_next_WP(&next_command);
+	set_next_WP(&next_nav_command);
 	// pitch in deg, airspeed  m/s, throttle %, track WP 1 or 0
-	takeoff_pitch 	 	= (int)next_command.p1 * 100;
+	takeoff_pitch 	 	= (int)next_nav_command.p1 * 100;
 			//Serial.printf_P(PSTR("TO pitch:"));	Serial.println(takeoff_pitch);
 			//Serial.printf_P(PSTR("home.alt:"));	Serial.println(home.alt);
-	takeoff_altitude 	= next_command.alt;
+	takeoff_altitude 	= next_nav_command.alt;
 			//Serial.printf_P(PSTR("takeoff_altitude:"));	Serial.println(takeoff_altitude);
 	next_WP.lat 		= home.lat + 1000;	// so we don't have bad calcs
 	next_WP.lng 		= home.lng + 1000;	// so we don't have bad calcs
@@ -227,30 +239,30 @@ static void do_takeoff()
 
 static void do_nav_wp()
 {
-	set_next_WP(&next_command);
+	set_next_WP(&next_nav_command);
 }
 
 static void do_land()
 {
-	set_next_WP(&next_command);
+	set_next_WP(&next_nav_command);
 }
 
 static void do_loiter_unlimited()
 {
-	set_next_WP(&next_command);
+	set_next_WP(&next_nav_command);
 }
 
 static void do_loiter_turns()
 {
-	set_next_WP(&next_command);
+	set_next_WP(&next_nav_command);
-	loiter_total = next_command.p1 * 360;
+	loiter_total = next_nav_command.p1 * 360;
 }
 
 static void do_loiter_time()
 {
-	set_next_WP(&next_command);
+	set_next_WP(&next_nav_command);
 	loiter_time = millis();
-	loiter_time_max = next_command.p1; // units are (seconds * 10)
+	loiter_time_max = next_nav_command.p1; // units are (seconds * 10)
 }
 
 /********************************************************************************/
@@ -287,7 +299,7 @@ static bool verify_takeoff()
 
 static bool verify_land()
 {
-	// we don't verify landing - we never go to a new Must command after Land
+	// we don't verify landing - we never go to a new Nav command after Land
 	if (((wp_distance > 0) && (wp_distance <= (2*g_gps->ground_speed/100)))
 		|| (current_loc.alt <= next_WP.alt + 300)){
 
@@ -317,7 +329,7 @@ static bool verify_nav_wp()
 	hold_course = -1;
 	update_crosstrack();
 	if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
-		gcs_send_text_fmt(PSTR("Reached Waypoint #%i"),command_must_index);
+		gcs_send_text_fmt(PSTR("Reached Waypoint #%i"),nav_command_index);
 		return true;
 	}
 	// add in a more complex case
@@ -341,7 +353,7 @@ static bool verify_loiter_time()
 	update_loiter();
 	calc_bearing_error();
 	if ((millis() - loiter_time) > (unsigned long)loiter_time_max * 10000l) {		// scale loiter_time_max from (sec*10) to milliseconds
-		gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete"));
+		gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER time complete"));
 		return true;
 	}
 	return false;
@@ -353,7 +365,7 @@ static bool verify_loiter_turns()
 	calc_bearing_error();
 	if(loiter_sum > loiter_total) {
 		loiter_total = 0;
-		gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER orbits complete"));
+		gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER orbits complete"));
 		// clear the command queue;
 		return true;
 	}
@@ -377,13 +389,13 @@ static bool verify_RTL()
 static void do_wait_delay()
 {
 	condition_start = millis();
-	condition_value  = next_command.lat * 1000;	// convert to milliseconds
+	condition_value  = next_nonnav_command.lat * 1000;	// convert to milliseconds
 }
 
 static void do_change_alt()
 {
-	condition_rate		= next_command.lat;
+	condition_rate		= next_nonnav_command.lat;
-	condition_value 	= next_command.alt;
+	condition_value 	= next_nonnav_command.alt;
 	target_altitude		= current_loc.alt + (condition_rate / 10);		// Divide by ten for 10Hz update
 	next_WP.alt 		= condition_value;								// For future nav calculations
 	offset_altitude 	= 0;											// For future nav calculations
@@ -391,7 +403,7 @@ static void do_change_alt()
 
 static void do_within_distance()
 {
-	condition_value  = next_command.lat;
+	condition_value  = next_nonnav_command.lat;
 }
 
 /********************************************************************************/
@@ -438,53 +450,55 @@ static void do_loiter_at_location()
 static void do_jump()
 {
 	struct Location temp;
-	if(next_command.lat > 0) {
+	if(next_nonnav_command.lat > 0) {
 
-		command_must_index 	= 0;
+		nav_command_ID 	= NO_COMMAND;
-		command_may_index 	= 0;
+		non_nav_command_ID 	= NO_COMMAND;
-		temp 				= get_wp_with_index(g.waypoint_index);
+		temp 				= get_cmd_with_index(g.command_index);
-		temp.lat 			= next_command.lat - 1;					// Decrement repeat counter
+		temp.lat 			= next_nonnav_command.lat - 1;					// Decrement repeat counter
 
-		set_wp_with_index(temp, g.waypoint_index);
+		set_cmd_with_index(temp, g.command_index);
-		g.waypoint_index.set_and_save(next_command.p1 - 1);
+		g.command_index.set_and_save(next_nonnav_command.p1 - 1);
-	} else if (next_command.lat == -1) {
+	} else if (next_nonnav_command.lat == -1) {								// A repeat count of -1 = repeat forever
-	    g.waypoint_index.set_and_save(next_command.p1 - 1);
+		nav_command_ID 	= NO_COMMAND;
+		non_nav_command_ID 	= NO_COMMAND;
+	    g.command_index.set_and_save(next_nonnav_command.p1 - 1);
 	}
 }
 
 static void do_change_speed()
 {
 	// Note: we have no implementation for commanded ground speed, only air speed and throttle
-	if(next_command.alt > 0)
+	if(next_nonnav_command.alt > 0)
-		g.airspeed_cruise.set_and_save(next_command.alt * 100);
+		g.airspeed_cruise.set_and_save(next_nonnav_command.alt * 100);
 
-	if(next_command.lat > 0)
+	if(next_nonnav_command.lat > 0)
-		g.throttle_cruise.set_and_save(next_command.lat);
+		g.throttle_cruise.set_and_save(next_nonnav_command.lat);
 }
 
 static void do_set_home()
 {
-	if(next_command.p1 == 1 && GPS_enabled) {
+	if(next_nonnav_command.p1 == 1 && GPS_enabled) {
 		init_home();
 	} else {
 		home.id 	= MAV_CMD_NAV_WAYPOINT;
-		home.lng 	= next_command.lng;				// Lon * 10**7
+		home.lng 	= next_nonnav_command.lng;				// Lon * 10**7
-		home.lat 	= next_command.lat;				// Lat * 10**7
+		home.lat 	= next_nonnav_command.lat;				// Lat * 10**7
-		home.alt 	= max(next_command.alt, 0);
+		home.alt 	= max(next_nonnav_command.alt, 0);
 		home_is_set = true;
 	}
 }
 
 static void do_set_servo()
 {
-	APM_RC.OutputCh(next_command.p1 - 1, next_command.alt);
+	APM_RC.OutputCh(next_nonnav_command.p1 - 1, next_nonnav_command.alt);
 }
 
 static void do_set_relay()
 {
-	if (next_command.p1 == 1) {
+	if (next_nonnav_command.p1 == 1) {
 		relay.on();
-	} else if (next_command.p1 == 0) {
+	} else if (next_nonnav_command.p1 == 0) {
 		relay.off();
 	}else{
 		relay.toggle();
@@ -493,16 +507,16 @@ static void do_set_relay()
 
 static void do_repeat_servo()
 {
-	event_id = next_command.p1 - 1;
+	event_id = next_nonnav_command.p1 - 1;
 
-	if(next_command.p1 >= CH_5 + 1 && next_command.p1 <= CH_8 + 1) {
+	if(next_nonnav_command.p1 >= CH_5 + 1 && next_nonnav_command.p1 <= CH_8 + 1) {
 
 		event_timer 	= 0;
-		event_delay 	= next_command.lng * 500.0;	// /2 (half cycle time) * 1000 (convert to milliseconds)
+		event_delay 	= next_nonnav_command.lng * 500.0;	// /2 (half cycle time) * 1000 (convert to milliseconds)
-		event_repeat 	= next_command.lat * 2;
+		event_repeat 	= next_nonnav_command.lat * 2;
-		event_value 	= next_command.alt;
+		event_value 	= next_nonnav_command.alt;
 
-		switch(next_command.p1) {
+		switch(next_nonnav_command.p1) {
 			case CH_5:
 				event_undo_value = g.rc_5.radio_trim;
 				break;
@@ -524,7 +538,7 @@ static void do_repeat_relay()
 {
 	event_id 		= RELAY_TOGGLE;
 	event_timer 	= 0;
-	event_delay 	= next_command.lat * 500.0;	// /2 (half cycle time) * 1000 (convert to milliseconds)
+	event_delay 	= next_nonnav_command.lat * 500.0;	// /2 (half cycle time) * 1000 (convert to milliseconds)
-	event_repeat	= next_command.alt * 2;
+	event_repeat	= next_nonnav_command.alt * 2;
 	update_events();
 }

ArduPlane/commands_process.pde

@@ -4,14 +4,16 @@
 //----------------------------------------
 static void change_command(uint8_t cmd_index)
 { 
-	struct Location temp = get_wp_with_index(cmd_index);
+	struct Location temp = get_cmd_with_index(cmd_index);
 	if (temp.id > MAV_CMD_NAV_LAST ){
 		gcs_send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd"));
 	} else {
-		command_must_index 	= NO_COMMAND;
+		gcs_send_text_fmt(PSTR("Received Request - jump to command #%i"),cmd_index);
-		next_command.id = NO_COMMAND;
+		nav_command_ID 	= NO_COMMAND;
-		g.waypoint_index.set_and_save(cmd_index - 1);
+		next_nav_command.id = NO_COMMAND;
-		load_next_command_from_EEPROM();
+		non_nav_command_ID = NO_COMMAND;
+		nav_command_index = cmd_index - 1;
+		g.command_index.set_and_save(cmd_index - 1);
 		process_next_command();
 	}
 }
@@ -21,136 +23,121 @@ static void change_command(uint8_t cmd_index)
 // --------------------
 static void update_commands(void)
 {
-	// This function loads commands into three buffers
-	// when a new command is loaded, it is processed with process_XXX()
-	// ----------------------------------------------------------------
 	if(home_is_set == false){
 		return;	// don't do commands
 	}
 
 	if(control_mode == AUTO){
-		load_next_command_from_EEPROM();
 		process_next_command();
 	}									// Other (eg GCS_Auto) modes may be implemented here
 }
 
 static void verify_commands(void)
 {
-	if(verify_must()){
+	if(verify_nav_command()){
-		command_must_index 	= NO_COMMAND;
+		nav_command_ID 	= NO_COMMAND;
 	}
 
-	if(verify_may()){
+	if(verify_condition_command()){
-		command_may_index 	= NO_COMMAND;
+		non_nav_command_ID		= NO_COMMAND;
-		command_may_ID		= NO_COMMAND;
 	}
 }
 
-static void load_next_command_from_EEPROM()
-{
-	// fetch next command if the next command queue is empty
-	// -----------------------------------------------------
-	if(next_command.id == NO_COMMAND){
-		next_command = get_wp_with_index(g.waypoint_index + 1);
-	}
-
-	// If the preload failed, return or just Loiter
-	// generate a dynamic command for RTL
-	// --------------------------------------------
-	if(next_command.id == NO_COMMAND){
-		// we are out of commands!
-		gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
-		handle_no_commands();
-	}
-}
 
 static void process_next_command()
 {
+	// This function makes sure that we always have a current navigation command
+	// and loads conditional or immediate commands if applicable
+	
+	struct Location temp;
+	byte old_index;
+	
 	// these are Navigation/Must commands
 	// ---------------------------------
-	if (command_must_index == 0){ // no current command loaded
+	if (nav_command_ID == NO_COMMAND){ // no current navigation command loaded
-		if (next_command.id < MAV_CMD_NAV_LAST ){
+		old_index = nav_command_index;
-			increment_WP_index();
+		temp.id = MAV_CMD_NAV_LAST;
-			//save_command_index();			// TO DO - fix - to Recover from in air Restart
+		while(temp.id >= MAV_CMD_NAV_LAST && nav_command_index <= g.command_total) {
-			command_must_index = g.waypoint_index;
+			nav_command_index++;
+			temp = get_cmd_with_index(nav_command_index);
+		}
+		gcs_send_text_fmt(PSTR("Nav command index updated to #%i"),nav_command_index);
+		if(nav_command_index > g.command_total){
+			// we are out of commands!
+			gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
+			handle_no_commands();
+		} else {
+			next_nav_command = temp;
+			nav_command_ID = next_nav_command.id;
+			non_nav_command_index = NO_COMMAND;			// This will cause the next intervening non-nav command (if any) to be loaded
+			non_nav_command_ID = NO_COMMAND;
 		
-			//SendDebug_P("MSG <process_next_command> new command_must_id ");
+			if (g.log_bitmask & MASK_LOG_CMD) {
-			//SendDebug(next_command.id,DEC);
+				Log_Write_Cmd(g.command_index, &next_nav_command);
-			//SendDebug_P(" index:");
+			}
-			//SendDebugln(command_must_index,DEC);
+			process_nav_cmd();
-			if (g.log_bitmask & MASK_LOG_CMD)
-				Log_Write_Cmd(g.waypoint_index, &next_command);
-			process_must();
 		}
 	}
 
-	// these are Condition/May commands
+	// these are Condition/May and Do/Now commands
-	// ----------------------
+	// -------------------------------------------
-	if (command_may_index == 0){
+	if (non_nav_command_index == NO_COMMAND) {		// If the index is NO_COMMAND then we have just loaded a nav command
-		if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){
+		non_nav_command_index = old_index + 1;
-			increment_WP_index();		// this command is from the command list in EEPROM
+		//gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index);
-			command_may_index = g.waypoint_index;
+	} else if (non_nav_command_ID == NO_COMMAND) {	// If the ID is NO_COMMAND then we have just completed a non-nav command
-				//SendDebug_P("MSG <process_next_command> new command_may_id ");
+		non_nav_command_index++;
-				//SendDebug(next_command.id,DEC);
-				//Serial.printf_P(PSTR("new command_may_index "));
-				//Serial.println(command_may_index,DEC);
-			if (g.log_bitmask & MASK_LOG_CMD)
-				Log_Write_Cmd(g.waypoint_index, &next_command);
-			process_may();
 	}
 	
-		// these are Do/Now commands
+		//gcs_send_text_fmt(PSTR("Nav command index #%i"),nav_command_index);
-		// ---------------------------
+		//gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index);
-		if (next_command.id > MAV_CMD_CONDITION_LAST){
+		//gcs_send_text_fmt(PSTR("Non-Nav command ID #%i"),non_nav_command_ID);
-			increment_WP_index();		// this command is from the command list in EEPROM
+	if(nav_command_index <= (int)g.command_total && non_nav_command_ID == NO_COMMAND) {
-				//SendDebug_P("MSG <process_next_command> new command_now_id ");
+		temp = get_cmd_with_index(non_nav_command_index);
-				//SendDebug(next_command.id,DEC);
+		if(temp.id <= MAV_CMD_NAV_LAST) {		// The next command is a nav command.  No non-nav commands to do
-			if (g.log_bitmask & MASK_LOG_CMD)
+			g.command_index.set_and_save(nav_command_index);
-				Log_Write_Cmd(g.waypoint_index, &next_command);
+			non_nav_command_index = nav_command_index;
-			process_now();
+			non_nav_command_ID = WAIT_COMMAND;
+		gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID);
+		} else {								// The next command is a non-nav command.  Prepare to execute it.
+			g.command_index.set_and_save(non_nav_command_index);
+			next_nonnav_command = temp;
+			non_nav_command_ID = next_nonnav_command.id;
+		gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID);
+			if (g.log_bitmask & MASK_LOG_CMD) {
+				Log_Write_Cmd(g.command_index, &next_nonnav_command);
 			}
+			
+			process_non_nav_command();	
+		}
+
 	}
 }
 
 /**************************************************/
 //  These functions implement the commands.
 /**************************************************/
-static void process_must()
+static void process_nav_cmd()
 {
-	gcs_send_text_P(SEVERITY_LOW,PSTR("New cmd: <process_must>"));
+	//gcs_send_text_P(SEVERITY_LOW,PSTR("New nav command loaded"));
 
-	// clear May indexes
+	// clear non-nav command ID and index
-	command_may_index	= NO_COMMAND;
+	non_nav_command_index	= NO_COMMAND;		// Redundant - remove?
-	command_may_ID		= NO_COMMAND;
+	non_nav_command_ID		= NO_COMMAND;		// Redundant - remove?
 
-	command_must_ID 	= next_command.id;
+	handle_process_nav_cmd();
-	handle_process_must();
 
-	// invalidate command so a new one is loaded
-	// -----------------------------------------
-	next_command.id		= NO_COMMAND;
 }
 
-static void process_may()
+static void process_non_nav_command()
 {
-	gcs_send_text_P(SEVERITY_LOW,PSTR("<process_may>"));
+	//gcs_send_text_P(SEVERITY_LOW,PSTR("new non-nav command loaded"));
 
-	command_may_ID = next_command.id;
+	if(non_nav_command_ID < MAV_CMD_CONDITION_LAST) {
-	handle_process_may();
+		handle_process_condition_command();
-
+	} else { 
-	// invalidate command so a new one is loaded
+		handle_process_do_command();
+		// flag command ID so a new one is loaded
 		// -----------------------------------------
-	next_command.id = NO_COMMAND;
+		non_nav_command_ID = NO_COMMAND;
+	}
 }
-
-static void process_now()
-{
-	handle_process_now();
-
-	// invalidate command so a new one is loaded
-	// -----------------------------------------
-	next_command.id = NO_COMMAND;
-
-	gcs_send_text(SEVERITY_LOW, "<process_now>");
-}
-

ArduPlane/defines.h

@@ -77,6 +77,7 @@
 // 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
+#define WAIT_COMMAND 255
 
 // Command/Waypoint/Location Options Bitmask
 //--------------------
@@ -169,7 +170,7 @@ enum gcs_severity {
 // Events
 // ------
 #define EVENT_WILL_REACH_WAYPOINT 1
-#define EVENT_SET_NEW_WAYPOINT_INDEX 2
+#define EVENT_SET_NEW_COMMAND_INDEX 2
 #define EVENT_LOADED_WAYPOINT 3
 #define EVENT_LOOP 4
 

ArduPlane/navigation.pde

@@ -100,7 +100,7 @@ static void calc_altitude_error()
 		}else{
 			target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt);
 		}
-	} else if (command_may_ID != MAV_CMD_CONDITION_CHANGE_ALT) {
+	} else if (non_nav_command_ID != MAV_CMD_CONDITION_CHANGE_ALT) {
 		target_altitude = next_WP.alt;
 	}
 

ArduPlane/test.pde

@@ -306,12 +306,12 @@ test_wp(uint8_t argc, const Menu::arg *argv)
 		Serial.printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude/100);
 	}
 
-	Serial.printf_P(PSTR("%d waypoints\n"), (int)g.waypoint_total);
+	Serial.printf_P(PSTR("%d waypoints\n"), (int)g.command_total);
 	Serial.printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius);
 	Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius);
 
-	for(byte i = 0; i <= g.waypoint_total; i++){
+	for(byte i = 0; i <= g.command_total; i++){
-		struct Location temp = get_wp_with_index(i);
+		struct Location temp = get_cmd_with_index(i);
 		test_wp_print(&temp, i);
 	}
 

libraries/APM_PI/APM_PI.cpp

@@ -12,7 +12,7 @@ APM_PI::get_pi(int32_t error, float dt)
 {
 	_integrator += ((float)error * _ki) * dt;
 
-if (_integrator < -_imax) {
+	if (_integrator < -_imax) {
 		_integrator = -_imax;
 	} else if (_integrator > _imax) {
 		_integrator = _imax;

libraries/RC_Channel/RC_Channel.cpp

@@ -38,7 +38,7 @@ RC_Channel::set_angle(int angle)
 void
 RC_Channel::set_dead_zone(int dzone)
 {
-	_dead_zone = abs(dzone >>1);
+	_dead_zone.set_and_save(abs(dzone >>1));
 }
 
 void

libraries/RC_Channel/RC_Channel.h

@@ -26,9 +26,10 @@ class RC_Channel{
         radio_trim(&_group, 1, 1500, name ? PSTR("TRIM") : 0),
         radio_max (&_group, 2, 1900, name ? PSTR("MAX") : 0),
 		_high(1),
-		_filter(true),
+		_filter(false),
 		_reverse  (&_group, 3,    1, name ? PSTR("REV") : 0),
-		_dead_zone(0),
+		_dead_zone (&_group, 4,   0, name ? PSTR("DZ") : 0),
+		//_dead_zone(0),
 		scale_output(1.0)
 	{}
 
@@ -97,7 +98,8 @@ class RC_Channel{
 	bool		_filter;
 	AP_Int8 	_reverse;
 
-	int16_t 	_dead_zone; // used to keep noise down and create a dead zone.
+	AP_Int16 	_dead_zone;
+	//int16_t 	_dead_zone; // used to keep noise down and create a dead zone.
 	uint8_t 	_type;
 	int16_t 	_high;
 	int16_t 	_low;