Final changes for Beta release

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

ArduCopterMega/ArduCopterMega.pde

@@ -1,7 +1,7 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
 /*
-ArduCopterMega Version 0.1.3 Experimental
+ArduCopter Version 2.0 Beta
 Authors:	Jason Short
 Based on code and ideas from the Arducopter team: Jose Julio, Randy Mackay, Jani Hirvinen
 Thanks to:	Chris Anderson, Mike Smith, Jordi Munoz, Doug Weibel, James Goppert, Benjamin Pelletier
@@ -324,7 +324,7 @@ boolean	land_complete;
 int		landing_distance;					// meters;
 long 	old_alt;							// used for managing altitude rates
 int		velocity_land;
-byte 	yaw_tracking = TRACK_NEXT_WP;			// no tracking, point at next wp, or at a target
+byte 	yaw_tracking = MAV_ROI_WPNEXT;			// no tracking, point at next wp, or at a target
 
 // Loiter management
 // -----------------
@@ -1113,7 +1113,7 @@ void update_navigation()
 		limit_nav_pitch_roll(g.pitch_max.get());
 
 		// this tracks a location so the copter is always pointing towards it.
-		if(yaw_tracking & TRACK_TARGET_WP){
+		if(yaw_tracking == MAV_ROI_LOCATION){
 			nav_yaw = get_bearing(&current_loc, &target_WP);
 		}
 
@@ -1191,9 +1191,6 @@ void update_alt()
 
 		//sonar_alt = sonar.read();
 
-		// output a light to show sonar is working
-		update_sonar_light(sonar_alt > 100);
-
 		// decide if we're using sonar
 		if ((baro_alt < 1200) || sonar_alt < 300){
 

ArduCopterMega/Attitude.pde

@@ -216,7 +216,7 @@ void output_manual_yaw()
 
 void auto_yaw()
 {
-	if(yaw_tracking & TRACK_NEXT_WP){
+	if(yaw_tracking == MAV_ROI_LOCATION){
 		nav_yaw = target_bearing;
 	}
 

ArduCopterMega/Log.pde

@@ -375,17 +375,19 @@ void Log_Write_Nav_Tuning()
 
 	DataFlash.WriteInt((int)(dcm.yaw_sensor/100));		// 1
 	DataFlash.WriteInt((int)wp_distance);				// 2
-	DataFlash.WriteInt((int)(target_bearing/100));		// 3
-	DataFlash.WriteInt((int)(nav_bearing/100));			// 4
+	DataFlash.WriteByte(wp_verify_byte);				// 3
+	DataFlash.WriteInt((int)(target_bearing/100));		// 4
+	DataFlash.WriteInt((int)(nav_bearing/100));			// 5
 
-	DataFlash.WriteInt((int)(g.rc_3.servo_out));		// 5
-	DataFlash.WriteByte(altitude_sensor);				// 6
-	DataFlash.WriteInt((int)sonar_alt);					// 7
-	DataFlash.WriteInt((int)baro_alt);					// 8
+	DataFlash.WriteInt((int)(g.rc_3.servo_out));		// 6
+	DataFlash.WriteByte(altitude_sensor);				// 7
+	DataFlash.WriteInt((int)sonar_alt);					// 8
+	DataFlash.WriteInt((int)baro_alt);					// 9
 
-	DataFlash.WriteInt((int)home.alt);					// 9
-	DataFlash.WriteInt((int)next_WP.alt);				// 10
-	DataFlash.WriteInt((int)altitude_error);			// 11
+
+	DataFlash.WriteInt((int)home.alt);					// 10
+	DataFlash.WriteInt((int)next_WP.alt);				// 11
+	DataFlash.WriteInt((int)altitude_error);			// 12
 
 	DataFlash.WriteByte(END_BYTE);
 }
@@ -399,6 +401,7 @@ void Log_Read_Nav_Tuning()
 	Serial.printf_P(PSTR("NTUN, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n"),
 				DataFlash.ReadInt(),	//yaw sensor
 				DataFlash.ReadInt(),	//distance
+				DataFlash.ReadByte(),	//bitmask
 				DataFlash.ReadInt(),	//target bearing
 				DataFlash.ReadInt(),	//nav bearing
 

ArduCopterMega/command description.txt

@@ -16,10 +16,6 @@ Command Structure in bytes
 11 0x0C	..
 11 0x0D	..
 
-Update:
-MAV_CMD_NAV_ORIENTATION_TARGET > MAV_CMD_DO_ORIENTATION
-Remove MAV_CMD_NAV_TARGET
-
 
 ***********************************
 Commands below MAV_CMD_NAV_LAST are commands that have a end criteria, eg "reached waypoint" or "reached altitude"
@@ -74,7 +70,4 @@ Command ID		Name						Parameter 1			Parameter 2			Parameter 3			Parameter 4
 				Note:  Max cycle time = 60 sec, A repeat relay or repeat servo command will cancel any current repeating event
 
 // need to add command
-0xB7 / 185		MAV_CMD_DO_ORIENTATION		Yaw_Mode			altitude			lat					lon
-											TRACK_NONE 		1
-											TRACK_NEXT_WP 	2
-											TRACK_TARGET_WP 4
+0xB7 / 185		MAV_CMD_DO_SET_ROI			Yaw_Mode			altitude			lat					lon

ArduCopterMega/commands_logic.pde

@@ -97,7 +97,7 @@ void handle_process_now()
 			do_repeat_relay();
 			break;
 
-		//case MAV_CMD_DO_ORIENTATION:
+		//case MAV_CMD_DO_SET_ROI:
 		//	do_target_yaw();
 	}
 }
@@ -257,16 +257,13 @@ void do_nav_wp()
 		// we don't need to worry about it
 		wp_verify_byte |= NAV_ALTITUDE;
 	}
-
-	// lets ignore this for today.
-	wp_verify_byte |= NAV_ALTITUDE;
 }
 
 void do_land()
 {
 	wp_control = LOITER_MODE;
 
-	Serial.println("dlnd ");
+	//Serial.println("dlnd ");
 
 	// not really used right now, might be good for debugging
 	land_complete		= false;
@@ -330,20 +327,20 @@ void do_loiter_time()
 
 bool verify_takeoff()
 {
-	Serial.printf("vt c_alt:%ld, n_alt:%ld\n", current_loc.alt, next_WP.alt);
+	//Serial.printf("vt c_alt:%ld, n_alt:%ld\n", current_loc.alt, next_WP.alt);
 
 	// wait until we are ready!
 	if(g.rc_3.control_in == 0)
 		return false;
 
 	if (current_loc.alt > next_WP.alt){
-		Serial.println("Y");
+		//Serial.println("Y");
 		takeoff_complete = true;
 		return true;
 
 	}else{
 
-		Serial.println("N");
+		//Serial.println("N");
 		return false;
 	}
 }
@@ -360,7 +357,7 @@ bool verify_land()
 		// decide which sensor we're using
 		if(sonar_alt < 40){
 			land_complete = true;
-			Serial.println("Y");
+			//Serial.println("Y");
 			//return true;
 		}
 	}
@@ -370,7 +367,7 @@ bool verify_land()
 		//return true;
 	}
 	//Serial.printf("N, %d\n", velocity_land);
-	Serial.printf("N_alt, %ld\n", next_WP.alt);
+	//Serial.printf("N_alt, %ld\n", next_WP.alt);
 
 	//update_crosstrack();
 	return false;
@@ -394,8 +391,6 @@ bool verify_nav_wp()
 
 		// if we have a distance calc error, wp_distance may be less than 0
 		if(wp_distance > 0){
-
-			// XXX does this work?
 			wp_verify_byte |= NAV_LOCATION;
 
 			if(loiter_time == 0){
@@ -418,7 +413,8 @@ bool verify_nav_wp()
 		}
 	}
 
-	if(wp_verify_byte == 7){
+	//if(wp_verify_byte >= 7){
+	if(wp_verify_byte & NAV_LOCATION){
 		char message[30];
 		sprintf(message,"Reached Command #%i",command_must_index);
 		gcs.send_text(SEVERITY_LOW,message);
@@ -461,7 +457,7 @@ bool verify_RTL()
 
 void do_wait_delay()
 {
-	Serial.print("dwd ");
+	//Serial.print("dwd ");
 	condition_start = millis();
 	condition_value	 = next_command.lat * 1000; // convert to milliseconds
 	Serial.println(condition_value,DEC);
@@ -483,8 +479,8 @@ void do_within_distance()
 
 void do_yaw()
 {
-	Serial.println("dyaw ");
-	yaw_tracking = TRACK_NONE;
+	//Serial.println("dyaw ");
+	yaw_tracking = MAV_ROI_NONE;
 
 	// target angle in degrees
 	command_yaw_start		= nav_yaw; // current position
@@ -543,13 +539,13 @@ void do_yaw()
 
 bool verify_wait_delay()
 {
-	Serial.print("vwd");
+	//Serial.print("vwd");
 	if ((millis() - condition_start) > condition_value){
-		Serial.println("y");
+		//Serial.println("y");
 		condition_value		= 0;
 		return true;
 	}
-	Serial.println("n");
+	//Serial.println("n");
 	return false;
 }
 
@@ -582,13 +578,13 @@ bool verify_within_distance()
 
 bool verify_yaw()
 {
-	Serial.print("vyaw ");
+	//Serial.print("vyaw ");
 
 	if((millis() - command_yaw_start_time) > command_yaw_time){
 		// time out
 		// make sure we hold at the final desired yaw angle
 		nav_yaw = command_yaw_end;
-		Serial.println("Y");
+		//Serial.println("Y");
 		return true;
 
 	}else{
@@ -598,7 +594,7 @@ bool verify_yaw()
 
 		nav_yaw		= command_yaw_start + ((float)command_yaw_delta * power * command_yaw_dir);
 		nav_yaw		= wrap_360(nav_yaw);
-		Serial.printf("ny %ld\n",nav_yaw);
+		//Serial.printf("ny %ld\n",nav_yaw);
 		return false;
 	}
 }
@@ -611,7 +607,7 @@ void do_target_yaw()
 {
 	yaw_tracking = next_command.p1;
 
-	if(yaw_tracking & TRACK_TARGET_WP){
+	if(yaw_tracking == MAV_ROI_LOCATION){
 		target_WP = next_command;
 	}
 }

ArduCopterMega/defines.h

@@ -129,10 +129,6 @@
 #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
 
 #define LOITER_MODE 1
 #define WP_MODE 2
@@ -305,3 +301,25 @@
 #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
+
+/*
+#ifndef MAV_CMD_DO_ROI
+# define MAV_CMD_DO_ROI 201
+#endif
+#ifndef MAV_ROI_NONE
+# define MAV_ROI_NONE 0
+#endif
+#ifndef MAV_ROI_WPNEXT
+# define MAV_ROI_WPNEXT 1
+#endif
+#ifndef MAV_ROI_WPINDEX
+# define MAV_ROI_WPINDEX 2
+#endif
+#ifndef MAV_ROI_LOCATION
+# define MAV_ROI_LOCATION 3
+#endif
+#ifndef MAV_ROI_TARGET
+# define MAV_ROI_TARGET 4
+#endif
+
+*/

ArduCopterMega/navigation.pde

@@ -141,7 +141,6 @@ void calc_rate_nav()
 	// remember our old speed
 	last_ground_speed = g_gps->ground_speed;
 
-
 	// dampen our response
 	nav_lat -= constrain(dampening, 	-1800, 1800); // +- 20m max error
 }
@@ -160,20 +159,20 @@ void calc_rate_nav()
 
 	// change to rate error
 	// we want to be going 450cm/s
-	int error = WAYPOINT_SPEED - groundspeed;
+	int error 			= WAYPOINT_SPEED - groundspeed;
 
 	// Scale response by kP
-	long nav_lat 	= g.pid_nav_wp.kP() * error;
-	int dampening	= g.pid_nav_wp.kD() * (groundspeed - last_ground_speed);
+	long nav_lat 		= g.pid_nav_wp.kP() * error;
+	int dampening		= g.pid_nav_wp.kD() * (groundspeed - last_ground_speed);
 
 	// remember our old speed
-	last_ground_speed = groundspeed;
+	last_ground_speed	= groundspeed;
 
 	// dampen our response
 	nav_lat -= constrain(dampening, 	-1800, 1800); // +- max error
 
 	// limit our output
-	nav_lat	= constrain(nav_lat, 	-2500, 2500); // +- max error
+	nav_lat	= constrain(nav_lat, 	-1800, 1800); // +- max error
 }
 
 #endif

ArduCopterMega/setup.pde

@@ -215,19 +215,18 @@ setup_esc(uint8_t argc, const Menu::arg *argv)
 void
 init_esc()
 {
-	Serial.printf_P(PSTR("\nCalibrate ESC\nRestart when done."));
-
+	//Serial.printf_P(PSTR("\nCalibrate ESC"));
 	g.esc_calibrate.set_and_save(0);
-
 	while(1){
 		read_radio();
-		delay(20);
-
+		delay(100);
+		update_esc_light();
 		APM_RC.OutputCh(CH_1, g.rc_3.radio_in);
 		APM_RC.OutputCh(CH_2, g.rc_3.radio_in);
 		APM_RC.OutputCh(CH_3, g.rc_3.radio_in);
 		APM_RC.OutputCh(CH_4, g.rc_3.radio_in);
-
+		APM_RC.OutputCh(CH_7, g.rc_3.radio_in);
+		APM_RC.OutputCh(CH_8, g.rc_3.radio_in);
 	}
 }
 

ArduCopterMega/system.pde

@@ -37,7 +37,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
 };
 
 // Create the top-level menu object.
-MENU(main_menu, "ACM", main_menu_commands);
+MENU(main_menu, "AC 2.0 Beta", main_menu_commands);
 
 void init_ardupilot()
 {
@@ -186,19 +186,19 @@ void init_ardupilot()
 	// menu; they must reset in order to fly.
 	//
 	if (digitalRead(SLIDE_SWITCH_PIN) == 0) {
+		digitalWrite(A_LED_PIN,HIGH);		// turn on setup-mode LED
+		Serial.printf_P(PSTR("\n"
+							 "Entering interactive setup mode...\n"
+							 "\n"
+							 "Type 'help' to list commands, 'exit' to leave a submenu.\n"
+							 "Visit the 'setup' menu for first-time configuration.\n\n"));
+		for (;;) {
+			//Serial.println_P(PSTR("\nMove the slide switch and reset to FLY.\n"));
+			main_menu.run();
+		}
+	}else{
 		if(g.esc_calibrate == 1){
 			init_esc();
-		}else{
-			digitalWrite(A_LED_PIN,HIGH);		// turn on setup-mode LED
-			Serial.printf_P(PSTR("\n"
-								 "Entering interactive setup mode...\n"
-								 "\n"
-								 "Type 'help' to list commands, 'exit' to leave a submenu.\n"
-								 "Visit the 'setup' menu for first-time configuration.\n\n"));
-			for (;;) {
-				//Serial.println_P(PSTR("\nMove the slide switch and reset to FLY.\n"));
-				main_menu.run();
-			}
 		}
 	}
 
@@ -307,7 +307,7 @@ void set_mode(byte mode)
 
 
 	// clear our tracking behaviors
-	yaw_tracking = TRACK_NONE;
+	yaw_tracking = MAV_ROI_NONE;
 
 	//send_text_P(SEVERITY_LOW,PSTR("control mode"));
 	//Serial.printf("set mode: %d old: %d\n", (int)mode, (int)control_mode);
@@ -422,12 +422,29 @@ void update_motor_light(void)
 	}
 }
 
-void update_sonar_light(bool light)
+void update_esc_light()
 {
-	if(light == true){
-		digitalWrite(B_LED_PIN, HIGH);
-	}else{
-		digitalWrite(B_LED_PIN, LOW);
+	static byte step;
+
+	if (step++ == 3)
+		step = 0;
+
+	switch(step)
+	{
+		case 0:
+			digitalWrite(C_LED_PIN, LOW);
+			digitalWrite(A_LED_PIN, HIGH);
+			break;
+
+		case 1:
+			digitalWrite(A_LED_PIN, LOW);
+			digitalWrite(B_LED_PIN, HIGH);
+			break;
+
+		case 2:
+			digitalWrite(B_LED_PIN, LOW);
+			digitalWrite(C_LED_PIN, HIGH);
+			break;
 	}
 }