Added new Simple mode - Better version of FBW that is offset by initial yaw angle and not north.

Added new navigation control based on bearing_error so we can leverage Crosstrack correction.
I've done limited testing on this.

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

ArduCopterMega/ArduCopterMega.pde

@@ -185,6 +185,7 @@ const char* flight_mode_strings[] = {
 	"STABILIZE",
 	"ACRO",
 	"ALT_HOLD",
+	"SIMPLE",
 	"FBW",
 	"AUTO",
 	"GCS_AUTO",
@@ -252,6 +253,8 @@ float cos_roll_x 	= 1;
 float cos_pitch_x 	= 1;
 float cos_yaw_x 	= 1;
 float sin_pitch_y, sin_yaw_y, sin_roll_y;
+float sin_nav_y, cos_nav_x;					// used in calc_waypoint_nav
+long initial_simple_bearing;				// used for Simple mode
 
 // Airspeed
 // --------
@@ -526,6 +529,10 @@ void medium_loop()
 		case 1:
 			medium_loopCounter++;
 
+			// hack to stop navigation in Simple mode
+			if (control_mode == SIMPLE)
+				break;
+
 			if(g_gps->new_data){
 				g_gps->new_data 	= false;
 				dTnav 				= millis() - nav_loopTimer;
@@ -540,7 +547,13 @@ void medium_loop()
 			// -----------------------------
 			dTnav2 				= millis() - nav2_loopTimer;
 			nav2_loopTimer 		= millis();
-			calc_nav();
+
+			if(wp_distance < 800){ // 8 meters
+				calc_loiter_nav();
+			}else{
+				calc_waypoint_nav();
+			}
+
 
 			break;
 
@@ -849,12 +862,47 @@ void update_current_flight_mode(void)
 				output_stabilize_pitch();
 				break;
 
+			case SIMPLE:
+				fbw_timer++;
+				// 25 hz
+				if(fbw_timer > 4){
+					Location temp = current_loc;
+					temp.lng += g.rc_1.control_in / 2;
+					temp.lat -= g.rc_2.control_in / 2;
+
+					// calc a new bearing
+					nav_bearing 	= get_bearing(&current_loc, &temp) + initial_simple_bearing;
+					nav_bearing 	= wrap_360(nav_bearing);
+					wp_distance 	= get_distance(&current_loc, &temp);
+
+					calc_bearing_error();
+
+					// get nav_pitch and nav_roll
+					calc_waypoint_nav();
+				}
+
+				// Output Pitch, Roll, Yaw and Throttle
+				// ------------------------------------
+				// Yaw control
+				output_manual_yaw();
+
+				// apply throttle control
+				output_manual_throttle();
+
+				// apply nav_pitch and nav_roll to output
+				fbw_nav_mixer();
+
+				// perform stabilzation
+				output_stabilize_roll();
+				output_stabilize_pitch();
+			break;
+
 			case FBW:
 				// we are currently using manual throttle during alpha testing.
 				fbw_timer++;
 
-				//call at 5 hz
-				if(fbw_timer > 20){
+				// 10 hz
+				if(fbw_timer > 10){
 					fbw_timer = 0;
 
 					if(home_is_set == false){

ArduCopterMega/defines.h

@@ -86,12 +86,13 @@
 #define STABILIZE 0			// hold level position
 #define ACRO 1				// rate control
 #define ALT_HOLD 2			// AUTO control
-#define FBW 3				// AUTO control
-#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
+#define SIMPLE 3			//
+#define FBW 4				// AUTO control
+#define AUTO 5				// AUTO control
+#define GCS_AUTO 6			// AUTO control
+#define LOITER 7			// Hold a single location
+#define RTL 8				// AUTO control
+#define NUM_MODES 9
 
 
 // Commands - Note that APM now uses a subset of the MAVLink protocol commands.  See enum MAV_CMD in the GCS_Mavlink library

ArduCopterMega/navigation.pde

@@ -52,11 +52,8 @@ void navigate()
 }
 
 #define DIST_ERROR_MAX 1800
-void calc_nav()
+void calc_loiter_nav()
 {
-	Vector2f yawvector;
-	Matrix3f temp;
-
 	/*
 	Becuase we are using lat and lon to do our distance errors here's a quick chart:
 	100 	= 1m
@@ -83,7 +80,7 @@ void calc_nav()
 	//nav_lat 	= constrain(nav_lat, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command
 
 	// 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);
 
 	long pmax = g.pitch_max.get();
@@ -92,6 +89,27 @@ void calc_nav()
 	nav_pitch 	= constrain(nav_pitch, -pmax, pmax);
 }
 
+void calc_waypoint_nav()
+{
+	nav_lat	= constrain(wp_distance, -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error
+
+	// get the sin and cos of the bearing error - rotated 90°
+	sin_nav_y 	= sin(radians((float)(9000 - bearing_error) / 100));
+	cos_nav_x 	= cos(radians((float)(bearing_error - 9000) / 100));
+
+	// rotate the vector
+	nav_roll 	= (float)nav_lat * cos_nav_x;
+	nav_pitch 	= (float)nav_lat * sin_nav_y;
+
+	// Scale response by kP
+	nav_roll	*= g.pid_nav_lon.kP();	// 1800 * 2 = 3600 or 36°
+	nav_pitch	*= g.pid_nav_lat.kP();	// 1800 * 2 = 3600 or 36°
+
+	long pmax = g.pitch_max.get();
+	nav_roll 	= constrain(nav_roll,  -pmax, pmax);
+	nav_pitch 	= constrain(nav_pitch, -pmax, pmax);
+}
+
 void calc_bearing_error()
 {
 	bearing_error 	= nav_bearing - dcm.yaw_sensor;

ArduCopterMega/system.pde

@@ -292,6 +292,10 @@ void set_mode(byte mode)
 			update_auto();
 			break;
 
+		case SIMPLE:
+			initial_simple_bearing = dcm.yaw_sensor;
+			break;
+
 		case LOITER:
 			do_loiter_at_location();
 			break;

ArduCopterMega/test.pde

@@ -385,7 +385,7 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
 				ts_num 				= 0;
 				g_gps->longitude 		= 0;
 				g_gps->latitude 		= 0;
-				calc_nav();
+				calc_loiter_nav();
 
 				Serial.printf_P(PSTR("ys:%ld, WP.lat:%ld, WP.lng:%ld, n_lat:%ld, n_lon:%ld, nroll:%ld, npitch:%ld, pmax:%ld, \t- "),
 					dcm.yaw_sensor,
@@ -504,7 +504,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
 								sin_roll_y,
 								cos_yaw_x,	// x
 								sin_yaw_y);	// y
-			*/
+			//*/
 		}
 
 		if(Serial.available() > 0){