Update to get SIMPLE mode running.

Seems to work!

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

ArduCopterMega/ArduCopterMega.pde

@@ -407,17 +407,18 @@ byte 			slow_loopCounter;
 byte 			superslow_loopCounter;
 byte			fbw_timer;					// for limiting the execution of FBW input
 
-unsigned long 	nav_loopTimer;				// used to track the elapsed ime for GPS nav
+//unsigned long 	nav_loopTimer;				// used to track the elapsed ime for GPS nav
 unsigned long 	nav2_loopTimer;				// used to track the elapsed ime for GPS nav
 
-unsigned long 	dTnav;						// Delta Time in milliseconds for navigation computations
+//unsigned long 	dTnav;						// Delta Time in milliseconds for navigation computations
 unsigned long 	dTnav2;						// Delta Time in milliseconds for navigation computations
 unsigned long 	elapsedTime;				// for doing custom events
 float 			load;						// % MCU cycles used
 
 byte			counter_one_herz;
 
-byte			GPS_failure_counter = 255;
+byte			GPS_failure_counter = 3;
+bool			GPS_disabled 		= false;
 
 ////////////////////////////////////////////////////////////////////////////////
 // Top-level logic
@@ -498,7 +499,6 @@ void medium_loop()
 	// ----------
 	read_radio();			// read the radio first
 
-
 	// reads all of the necessary trig functions for cameras, throttle, etc.
 	update_trig();
 
@@ -506,13 +506,16 @@ void medium_loop()
 	// -----------------------------------------
 	switch(medium_loopCounter) {
 
-		// This case deals with the GPS
-		//-------------------------------
+		// This case deals with the GPS and Compass
+		//-----------------------------------------
 		case 0:
 			medium_loopCounter++;
 
 			if(GPS_failure_counter > 0){
 				update_GPS();
+
+			}else if(GPS_failure_counter == 0){
+				GPS_disabled = true;
 			}
 			//readCommands();
 
@@ -529,32 +532,39 @@ void medium_loop()
 		case 1:
 			medium_loopCounter++;
 
+			// calc pitch and roll to target
+			// -----------------------------
+			dTnav2 				= millis() - nav2_loopTimer;
+			nav2_loopTimer 		= millis();
+
 			// hack to stop navigation in Simple mode
 			if (control_mode == SIMPLE)
 				break;
 
+			if (control_mode == FBW)
+				break;
+
+			// Auto control modes:
 			if(g_gps->new_data){
 				g_gps->new_data 	= false;
-				dTnav 				= millis() - nav_loopTimer;
-				nav_loopTimer 		= millis();
 
-				// calculate the plane's desired bearing
-				// -------------------------------------
+				// we are not tracking I term on navigation, so this isn't needed
+				//dTnav 				= millis() - nav_loopTimer;
+				//nav_loopTimer 		= millis();
+
+				// calculate the copter's desired bearing and WP distance
+				// ------------------------------------------------------
 				navigate();
 			}
 
-			// calc pitch and roll to target
-			// -----------------------------
-			dTnav2 				= millis() - nav2_loopTimer;
-			nav2_loopTimer 		= millis();
-
+			// we call these regardless of GPS because of the rapid nature of the yaw sensor
+			// -----------------------------------------------------------------------------
 			if(wp_distance < 800){ // 8 meters
 				calc_loiter_nav();
 			}else{
 				calc_waypoint_nav();
 			}
 
-
 			break;
 
 		// command processing
@@ -563,7 +573,7 @@ void medium_loop()
 			medium_loopCounter++;
 
 			// Read altitude from sensors
-			// ------------------
+			// --------------------------
 			update_alt();
 
 			// perform next command
@@ -581,41 +591,55 @@ void medium_loop()
 			if (g.log_bitmask & MASK_LOG_ATTITUDE_MED && (g.log_bitmask & MASK_LOG_ATTITUDE_FAST == 0))
 				Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (int)dcm.yaw_sensor);
 
-#if HIL_MODE != HIL_MODE_ATTITUDE
+			#if HIL_MODE != HIL_MODE_ATTITUDE
 			if (g.log_bitmask & MASK_LOG_CTUN)
 				Log_Write_Control_Tuning();
-#endif
+			#endif
 
 			if (g.log_bitmask & MASK_LOG_NTUN)
 				Log_Write_Nav_Tuning();
 
 			if (g.log_bitmask & MASK_LOG_GPS){
-				if(home_is_set)
-					Log_Write_GPS(g_gps->time, current_loc.lat, current_loc.lng, g_gps->altitude, current_loc.alt, (long) g_gps->ground_speed, g_gps->ground_course, g_gps->fix, g_gps->num_sats);
+				if(home_is_set){
+					Log_Write_GPS(g_gps->time,
+						current_loc.lat,
+						current_loc.lng,
+						g_gps->altitude,
+						current_loc.alt,
+						(long)g_gps->ground_speed,
+						g_gps->ground_course,
+						g_gps->fix,
+						g_gps->num_sats);
+				}
 			}
+
             gcs.send_message(MSG_ATTITUDE);     // Sends attitude data
 			break;
 
 		// This case controls the slow loop
 		//---------------------------------
 		case 4:
+			medium_loopCounter = 0;
+
 			if (g.current_enabled){
 				read_current();
 			}
 
-			// shall we trim the copter?
-			// ------------------------
+			// Accel trims 		= hold > 2 seconds
+			// Throttle cruise  = switch less than 1 second
+			// --------------------------------------------
 			read_trim_switch();
 
-			// shall we check for engine start?
-			// --------------------------------
+			// Check for engine arming
+			// -----------------------
 			arm_motors();
 
-			medium_loopCounter = 0;
 			slow_loop();
 			break;
 
 		default:
+			// this is just a catch all
+			// ------------------------
 			medium_loopCounter = 0;
 			break;
 	}
@@ -632,10 +656,10 @@ void medium_loop()
 	if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
 		Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (int)dcm.yaw_sensor);
 
-#if HIL_MODE != HIL_MODE_ATTITUDE
-	if (g.log_bitmask & MASK_LOG_RAW)
-		Log_Write_Raw();
-#endif
+	#if HIL_MODE != HIL_MODE_ATTITUDE
+		if (g.log_bitmask & MASK_LOG_RAW)
+			Log_Write_Raw();
+	#endif
 
 	#if GCS_PROTOCOL == 6		// This is here for Benjamin Pelletier.	Please do not remove without checking with me.	Doug W
 		readgcsinput();
@@ -688,6 +712,9 @@ void slow_loop()
 			slow_loopCounter = 0;
 			update_events();
 
+			// blink if we are armed
+			update_motor_light();
+
 			// XXX this should be a "GCS slow loop" interface
 			#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
 				gcs.data_stream_send(1,5);
@@ -724,7 +751,7 @@ void update_GPS(void)
 	update_GPS_light();
 
     if (g_gps->new_data && g_gps->fix) {
-    	GPS_failure_counter = 255;
+    	GPS_failure_counter = 3;
 
 		// XXX We should be sending GPS data off one of the regular loops so that we send
 		// no-GPS-fix data too
@@ -755,7 +782,7 @@ void update_GPS(void)
 					Log_Write_Startup(TYPE_GROUNDSTART_MSG);
 
 				// reset our nav loop timer
-				nav_loopTimer = millis();
+				//nav_loopTimer = millis();
 				init_home();
 
 				// init altitude
@@ -866,17 +893,28 @@ void update_current_flight_mode(void)
 				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;
+					fbw_timer = 0;
+
+					current_loc.lat = 0;
+					current_loc.lng = 0;
+
+					next_WP.lng =   (float)g.rc_1.control_in *.4;  // X: 4500 / 2 = 2250 = 25 meteres
+					next_WP.lat = -((float)g.rc_2.control_in *.4); // Y: 4500 / 2 = 2250 = 25 meteres
 
 					// calc a new bearing
-					nav_bearing 	= get_bearing(&current_loc, &temp) + initial_simple_bearing;
+					nav_bearing 	= get_bearing(&current_loc, &next_WP) + initial_simple_bearing;
 					nav_bearing 	= wrap_360(nav_bearing);
-					wp_distance 	= get_distance(&current_loc, &temp);
-
+					wp_distance 	= get_distance(&current_loc, &next_WP);
 					calc_bearing_error();
-
+					/*
+					Serial.printf("lat: %ld lon:%ld, bear:%ld, dist:%ld, init:%ld, err:%ld ",
+							next_WP.lat,
+							next_WP.lng,
+							nav_bearing,
+							wp_distance,
+							initial_simple_bearing,
+							bearing_error);
+					*/
 					// get nav_pitch and nav_roll
 					calc_waypoint_nav();
 				}
@@ -905,20 +943,15 @@ void update_current_flight_mode(void)
 				if(fbw_timer > 10){
 					fbw_timer = 0;
 
-					if(home_is_set == false){
-						scaleLongDown = 1;
-						// we are not using GPS
-						// reset the location
-						// RTL won't function
+					if(GPS_disabled){
 						current_loc.lat = home.lat = 0;
 						current_loc.lng = home.lng = 0;
-						// set dTnav manually
-						dTnav = 200;
 					}
 
 					next_WP.lng = home.lng + g.rc_1.control_in / 2; // X: 4500 / 2 = 2250 = 25 meteres
-					// forward is negative so we reverse it to get a positive North translation
 					next_WP.lat = home.lat - g.rc_2.control_in / 2; // Y: 4500 / 2 = 2250 = 25 meteres
+
+					calc_loiter_nav();
 				}
 
 				// Output Pitch, Roll, Yaw and Throttle

ArduCopterMega/Attitude.pde

@@ -144,7 +144,7 @@ output_yaw_with_hold(boolean hold)
 		long rate		= degrees(omega.z) * 100; 											// 3rad = 17188 , 6rad = 34377
 		rate			= constrain(rate, -36000, 36000);									// limit to something fun!
 
-		long error		= ((long)g.rc_4.control_in * 6) - rate;									// control is += 6000 * 6 = 36000
+		long error		= ((long)g.rc_4.control_in * 6) - rate;								// control is += 6000 * 6 = 36000
 																							// -error = CCW, 	+error = CW
 
 		if(g.rc_4.control_in == 0){
@@ -231,7 +231,7 @@ void calc_nav_throttle()
 	float scaler = 1.0;
 
 	if(error < 0){
-		scaler = (altitude_sensor == BARO) ? .5 : .8;
+		scaler = (altitude_sensor == BARO) ? .5 : .5;
 	}
 
 	if(altitude_sensor == BARO){

ArduCopterMega/config.h

@@ -287,7 +287,7 @@
 #define ACRO_RATE_PITCH_IMAX_CENTIDEGREE ACRO_RATE_PITCH_IMAX * 100
 
 #ifndef ACRO_RATE_YAW_P
-# define ACRO_RATE_YAW_P         .5
+# define ACRO_RATE_YAW_P         .1
 #endif
 #ifndef ACRO_RATE_YAW_I
 # define ACRO_RATE_YAW_I         0.0
@@ -394,7 +394,7 @@
 # define NAV_P					2.0
 #endif
 #ifndef NAV_I
-# define NAV_I					0.00
+# define NAV_I					0.1
 #endif
 #ifndef NAV_D
 # define NAV_D					0.00

ArduCopterMega/motors.pde

@@ -13,12 +13,14 @@ void arm_motors()
 			if (arming_counter > ARM_DELAY) {
 				motor_armed = true;
 			} else{
+				//Serial.printf("arm %d\n", arming_counter);
 				arming_counter++;
 			}
 		}else if (g.rc_4.control_in < -2700) {
 			if (arming_counter > DISARM_DELAY){
 				motor_armed = false;
 			}else{
+				//Serial.printf("arm %d\n", arming_counter);
 				arming_counter++;
 			}
 		}else{
@@ -67,10 +69,10 @@ set_servos_4()
 			motor_out[CH_3]		= g.rc_3.radio_out + g.rc_2.pwm_out;
 			motor_out[CH_4] 	= g.rc_3.radio_out - g.rc_2.pwm_out;
 
-			motor_out[CH_1]		+=  g.rc_4.pwm_out; 	// CCW
-			motor_out[CH_2]		+=  g.rc_4.pwm_out; 	// CCW
-			motor_out[CH_3]		-=  g.rc_4.pwm_out; 	// CW
-			motor_out[CH_4] 	-=  g.rc_4.pwm_out; 	// CW
+			motor_out[CH_1]		+=	g.rc_4.pwm_out; 	// CCW
+			motor_out[CH_2]		+=	g.rc_4.pwm_out; 	// CCW
+			motor_out[CH_3]		-=	g.rc_4.pwm_out; 	// CW
+			motor_out[CH_4] 	-=	g.rc_4.pwm_out; 	// CW
 
 		}else if(g.frame_type == X_FRAME){
 			//Serial.println("X_FRAME");
@@ -123,11 +125,11 @@ set_servos_4()
 			//left side
 			motor_out[CH_2]		= g.rc_3.radio_out + g.rc_1.pwm_out;		// CCW
 			motor_out[CH_3]		= g.rc_3.radio_out + roll_out + pitch_out;	// CW
-			motor_out[CH_8]     = g.rc_3.radio_out + roll_out - pitch_out;	// CW
+			motor_out[CH_8]		 = g.rc_3.radio_out + roll_out - pitch_out;	// CW
 
 			//right side
 			motor_out[CH_1]		= g.rc_3.radio_out - g.rc_1.pwm_out;		// CW
-            motor_out[CH_7] 	= g.rc_3.radio_out - roll_out + pitch_out;	// CCW
+						motor_out[CH_7] 	= g.rc_3.radio_out - roll_out + pitch_out;	// CCW
 			motor_out[CH_4] 	= g.rc_3.radio_out - roll_out - pitch_out;	// CCW
 
 			motor_out[CH_7]		+= g.rc_4.pwm_out;	// CCW
@@ -136,9 +138,9 @@ set_servos_4()
 
 			motor_out[CH_3]		-= g.rc_4.pwm_out;	// CW
 			motor_out[CH_1]		-= g.rc_4.pwm_out;	// CW
-			motor_out[CH_8]     -= g.rc_4.pwm_out;  // CW
+			motor_out[CH_8]		 -= g.rc_4.pwm_out;	// CW
 
-    	}else{
+			}else{
 
 			Serial.print("frame error");
 
@@ -157,6 +159,7 @@ set_servos_4()
 		}
 
 		num++;
+
 		if (num > 50){
 			num = 0;
 			/*
@@ -172,8 +175,6 @@ set_servos_4()
 			}
 			*/
 
-
-
 			//Serial.print("!");
 			//debugging with Channel 6
 
@@ -196,7 +197,7 @@ set_servos_4()
 			init_pids();
 			//*/
 
-			/*
+			///*
 			write_int(motor_out[CH_1]);
 			write_int(motor_out[CH_2]);
 			write_int(motor_out[CH_3]);
@@ -214,14 +215,13 @@ set_servos_4()
 			write_int((int)nav_roll);
 			write_int((int)nav_pitch);
 
-			//24
-			write_long(current_loc.lat);	//28
-			write_long(current_loc.lng);	//32
-			write_int((int)current_loc.alt);	//34
+			write_long(current_loc.lat);		// 28
+			write_long(current_loc.lng);		// 32
+			write_int((int)current_loc.alt);	// 34
 
 			write_long(next_WP.lat);
 			write_long(next_WP.lng);
-			write_int((int)next_WP.alt);		//44
+			write_int((int)next_WP.alt);		// 44
 
 			flush(10);
 			//*/

ArduCopterMega/navigation.pde

@@ -70,14 +70,12 @@ void calc_loiter_nav()
 	lat_error	= constrain(lat_error,  -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error
 
 	// Convert distance into ROLL X
-	nav_lon		= long_error * g.pid_nav_lon.kP();						// 1800 * 2 = 3600 or 36°
-	//nav_lon	= g.pid_nav_lon.get_pid(long_error, dTnav2, 1.0);
-	//nav_lon 	= constrain(nav_lon, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command
+	//nav_lon		= long_error * g.pid_nav_lon.kP();						// 1800 * 2 = 3600 or 36°
+	nav_lon		= g.pid_nav_lon.get_pid(long_error, dTnav2, 1.0);
 
 	// PITCH	Y
-	//nav_lat 	= g.pid_nav_lat.get_pid(lat_error, dTnav2, 1.0);
-	nav_lat		= lat_error * g.pid_nav_lat.kP();							// 1800 * 2 = 3600 or 36°
-	//nav_lat 	= constrain(nav_lat, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command
+	//nav_lat		= lat_error * g.pid_nav_lat.kP();						// 1800 * 2 = 3600 or 36°
+	nav_lat 	= g.pid_nav_lat.get_pid(lat_error, dTnav2, 1.0);
 
 	// rotate the vector
 	nav_roll 	=   (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;
@@ -91,23 +89,30 @@ void calc_loiter_nav()
 
 void calc_waypoint_nav()
 {
-	nav_lat	= constrain(wp_distance, -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error
+	nav_lat	= constrain((wp_distance * 100), -1800, 1800); // +- 20m max error
+	//nav_lat = max(wp_distance, -DIST_ERROR_MAX);
+	//nav_lat = min(wp_distance, DIST_ERROR_MAX);
+
+	//Serial.printf("nav_lat %ld, ", nav_lat);
+	// Scale response by kP
+	nav_lat	*= g.pid_nav_lat.kP();	// 1800 * 2 = 3600 or 36°
+	//Serial.printf("%ld, ",nav_lat);
 
 	// 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));
+	//Serial.printf("X%2.4f, Y%2.4f ", cos_nav_x, sin_nav_y);
 
 	// 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°
+	//Serial.printf("R%ld, P%ld ", nav_roll, nav_pitch);
 
 	long pmax = g.pitch_max.get();
 	nav_roll 	= constrain(nav_roll,  -pmax, pmax);
 	nav_pitch 	= constrain(nav_pitch, -pmax, pmax);
+	//Serial.printf("R%ld, P%ld \n", nav_roll, nav_pitch);
 }
 
 void calc_bearing_error()
@@ -205,10 +210,10 @@ long get_altitude_above_home(void)
 
 long get_distance(struct Location *loc1, struct Location *loc2)
 {
-	if(loc1->lat == 0 || loc1->lng == 0)
-		return -1;
-	if(loc2->lat == 0 || loc2->lng == 0)
-		return -1;
+	//if(loc1->lat == 0 || loc1->lng == 0)
+	//	return -1;
+	//if(loc2->lat == 0 || loc2->lng == 0)
+	//	return -1;
 	float dlat 		= (float)(loc2->lat - loc1->lat);
 	float dlong		= ((float)(loc2->lng - loc1->lng)) * scaleLongDown;
 	return sqrt(sq(dlat) + sq(dlong)) * .01113195;

ArduCopterMega/system.pde

@@ -82,7 +82,7 @@ void init_ardupilot()
 	//
 	Serial3.begin(SERIAL3_BAUD, 128, 128);
 
-	Serial.printf_P(PSTR("\n\nInit ArduPilotMega (unstable development version)"
+	Serial.printf_P(PSTR("\n\nInit ArduCopterMega"
 						 "\n\nFree RAM: %lu\n"),
 						 freeRAM());
 
@@ -92,9 +92,11 @@ void init_ardupilot()
 	if (!g.format_version.load() ||
 	     g.format_version != Parameters::k_format_version) {
 
-		Serial.printf_P(PSTR("\n\nEEPROM format version  %d not compatible with this firmware (requires %d)"
+		/*Serial.printf_P(PSTR("\n\nEEPROM format version  %d not compatible with this firmware (requires %d)"
 		                     "\n\nForcing complete parameter reset..."),
-		                     g.format_version.get(), Parameters::k_format_version);
+		                     g.format_version.get(),
+		                     Parameters::k_format_version);
+		*/
 
 		// erase all parameters
 		AP_Var::erase_all();
@@ -102,14 +104,14 @@ void init_ardupilot()
 		// save the new format version
 		g.format_version.set_and_save(Parameters::k_format_version);
 
-		Serial.println_P(PSTR("done."));
+		//Serial.println_P(PSTR("done."));
 	}else{
 	    unsigned long before = micros();
 	    // Load all auto-loaded EEPROM variables
 	    AP_Var::load_all();
 
-	    Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before);
-	    Serial.printf_P(PSTR("using %u bytes of memory\n"), AP_Var::get_memory_use());
+	  //  Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before);
+	  //  Serial.printf_P(PSTR("using %u bytes of memory\n"), AP_Var::get_memory_use());
 	}
 
 #ifdef RADIO_OVERRIDE_DEFAULTS
@@ -171,11 +173,10 @@ void init_ardupilot()
 		Serial.printf_P(PSTR("\n"
 							 "Entering interactive setup mode...\n"
 							 "\n"
-							 "If using the Arduino Serial Monitor, ensure Line Ending is set to Carriage Return.\n"
 							 "Type 'help' to list commands, 'exit' to leave a submenu.\n"
 							 "Visit the 'setup' menu for first-time configuration.\n"));
 		for (;;) {
-			Serial.println_P(PSTR("\nMove the slide switch and reset to FLY.\n"));
+			//Serial.println_P(PSTR("\nMove the slide switch and reset to FLY.\n"));
 			main_menu.run();
 		}
 	}
@@ -341,7 +342,8 @@ void update_GPS_light(void)
 {
 	// GPS LED on if we have a fix or Blink GPS LED if we are receiving data
 	// ---------------------------------------------------------------------
-	switch (g_gps->status()) {
+	switch (g_gps->status()){
+
 		case(2):
 			digitalWrite(C_LED_PIN, HIGH);  //Turn LED C on when gps has valid fix.
 			break;
@@ -362,7 +364,10 @@ void update_GPS_light(void)
 			digitalWrite(C_LED_PIN, LOW);
 			break;
 	}
+}
 
+void update_motor_light(void)
+{
 	if(motor_armed == true){
 		motor_light = !motor_light;