implementing yaw tracking.

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

ArduCopterMega/ArduCopterMega.pde

@@ -313,7 +313,7 @@ boolean	land_complete;
 int		landing_distance;					// meters;
 long 	old_alt;							// used for managing altitude rates
 int		velocity_land;
-bool 	nav_yaw_towards_wp;					// point at the next WP
+byte 	yaw_tracking = TRACK_NONE;			// no tracking, point at next wp, or at a target
 
 // Loiter management
 // -----------------
@@ -372,6 +372,7 @@ struct 	Location home;						// home location
 struct 	Location prev_WP;					// last waypoint
 struct 	Location current_loc;				// current location
 struct 	Location next_WP;					// next waypoint
+struct 	Location target_WP;					// where do we want to you towards?
 struct 	Location tell_command;				// command for telemetry
 struct 	Location next_command;				// command preloaded
 long 	target_altitude;					// used for
@@ -447,6 +448,9 @@ void loop()
 		G_Dt 				= (float)delta_ms_fast_loop / 1000.f;		// used by DCM integrator
 		mainLoop_count++;
 
+		if(delta_ms_fast_loop > 11){
+			Serial.println(delta_ms_fast_loop,DEC);
+		}
 		// Execute the fast loop
 		// ---------------------
 		fast_loop();
@@ -1084,6 +1088,10 @@ void update_navigation()
 	if(control_mode == AUTO || control_mode == GCS_AUTO){
 		verify_commands();
 
+		if(yaw_tracking & TRACK_TARGET_WP){
+			nav_yaw = get_bearing(&current_loc, &target_WP);
+		}
+
 	}else{
 		switch(control_mode){
 			case RTL:

ArduCopterMega/Attitude.pde

@@ -269,8 +269,9 @@ void output_manual_yaw()
 
 void auto_yaw()
 {
-	if(nav_yaw_towards_wp){
+	if(yaw_tracking & TRACK_NEXT_WP){
 		nav_yaw = target_bearing;
 	}
+
 	output_yaw_with_hold(true); // hold yaw
 }

ArduCopterMega/commands.pde

@@ -47,7 +47,7 @@ struct Location get_wp_with_index(int i)
 		temp.p1 = eeprom_read_byte((uint8_t*)mem);
 
 		mem++;
-		temp.alt = (long)eeprom_read_dword((uint32_t*)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);
@@ -80,7 +80,7 @@ void set_wp_with_index(struct Location temp, int i)
 	eeprom_write_byte((uint8_t *)	mem, temp.p1);
 
 	mem++;
-	eeprom_write_dword((uint32_t *)	mem, temp.alt);
+	eeprom_write_dword((uint32_t *)	mem, temp.alt);	// alt is stored in CM!
 
 	mem += 4;
 	eeprom_write_dword((uint32_t *)	mem, temp.lat);

ArduCopterMega/commands_logic.pde

@@ -38,7 +38,6 @@ void handle_process_must()
 		case MAV_CMD_NAV_RETURN_TO_LAUNCH:
 			do_RTL();
 			break;
-
 		default:
 			break;
 	}
@@ -100,6 +99,9 @@ void handle_process_now()
 		case MAV_CMD_DO_REPEAT_RELAY:
 			do_repeat_relay();
 			break;
+
+        case MAV_CMD_NAV_ORIENTATION_TARGET:
+            do_target_yaw();
 	}
 }
 
@@ -358,9 +360,7 @@ void do_within_distance()
 
 void do_yaw()
 {
-//	{				// CMD					opt		dir		angle/deg	deg/s		relative
+    yaw_tracking = TRACK_NONE;
-//	Location t = {MAV_CMD_CONDITION_YAW,	0,      1, 		360, 		60, 		1};
-
 
 	// target angle in degrees
 	command_yaw_start		= nav_yaw; // current position
@@ -481,6 +481,16 @@ bool verify_yaw()
 //  Do (Now) commands
 /********************************************************************************/
 
+void do_target_yaw()
+{
+    yaw_tracking = next_command.p1;
+
+    if(yaw_tracking & TRACK_TARGET_WP){
+        target_WP = next_command;
+    }
+
+}
+
 void do_loiter_at_location()
 {
 	next_WP = current_loc;

ArduCopterMega/defines.h

@@ -100,6 +100,11 @@
 #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
+
 //repeating events
 #define NO_REPEAT 0
 #define CH_5_TOGGLE 1

ArduCopterMega/system.pde

@@ -276,6 +276,11 @@ void set_mode(byte mode)
 		// disarm motors temp
 		motor_auto_safe = false;
 	}
+
+
+	// clear our tracking behaviors
+	yaw_tracking = TRACK_NONE;
+
 	//send_text_P(SEVERITY_LOW,PSTR("control mode"));
 	//Serial.printf("set mode: %d old: %d\n", (int)mode, (int)control_mode);
 	switch(control_mode)

ArduCopterMega/test.pde

@@ -309,8 +309,8 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
 			if(Serial.available() > 0){
 				if(g.compass_enabled){
 					compass.save_offsets();
-				}
 					report_compass();
+				}
 				return (0);
 			}
 
@@ -930,28 +930,26 @@ test_mission(uint8_t argc, const Menu::arg *argv)
 	int32_t		lng;				///< param 4 - Longitude * 10**7
 }*/
 
-	{
+	// clear home
-	Location t = {0,   	0,      0, 		0, 		0, 			0};
+	{Location t = {0,   	0,      0, 		0, 		0, 			0};
-	set_wp_with_index(t,0);
+	set_wp_with_index(t,0);}
-	}
 
-	{			// CMD						opt		pitch   alt/cm
+	// CMD										opt		pitch   alt/cm
-	Location t = {MAV_CMD_NAV_TAKEOFF,   	0,      0, 		300, 		0, 			0};
+	{Location t = {MAV_CMD_NAV_TAKEOFF,   		0,      0, 		300, 		0, 			0};
-	set_wp_with_index(t,1);
+	set_wp_with_index(t,1);}
-	}
+
-	{			// CMD						opt		   					time/ms
+	// CMD										opt		   					time/ms
-	Location t = {MAV_CMD_CONDITION_DELAY,   0,      0, 		0, 			3000, 		0};
+	{Location t = {MAV_CMD_CONDITION_DELAY,   	0,      0, 		0, 			3000, 		0};
-	set_wp_with_index(t,2);
+	set_wp_with_index(t,2);}
+
+	// CMD										opt		dir		angle/deg	time/ms		relative
+	{Location t = {MAV_CMD_CONDITION_YAW,		0,      1, 		360, 		1000, 		1};
+	set_wp_with_index(t,3);}
+
+	// CMD										opt
+	{Location t = {MAV_CMD_NAV_LAND,			0,      0, 		0, 			0, 			0};
+	set_wp_with_index(t,4);}
 
-	}
-	{				// CMD					opt		dir		angle/deg	time/ms		relative
-	Location t = {MAV_CMD_CONDITION_YAW,	0,      1, 		360, 		1000, 		1};
-	set_wp_with_index(t,3);
-	}
-	{			// CMD						opt
-	Location t = {MAV_CMD_NAV_LAND,			0,      0, 		0, 			0, 			0};
-	set_wp_with_index(t,4);
-	}
 
 	g.RTL_altitude.set_and_save(300);
 	g.waypoint_total.set_and_save(4);