Updates to guided mode from Michael O.

removed or commented unused static vars

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.48 Beta"
+#define THISFIRMWARE "ArduCopter V2.0.49 Beta"
 /*
 ArduCopter Version 2.0 Beta
 Authors:	Jason Short
@@ -425,13 +425,13 @@ static int 		event_repeat;						// how many times to fire : 0 = forever, 1 = do
 static int 		event_value; 						// per command value, such as PWM for servos
 static int 		event_undo_value;					// the value used to undo commands
 //static byte 	repeat_forever;
-static byte 	undo_event;							// counter for timing the undo
+//static byte 	undo_event;							// counter for timing the undo
 
 // delay command
 // --------------
 static long 	condition_value;					// used in condition commands (eg delay, change alt, etc.)
 static long 	condition_start;
-static int 		condition_rate;
+//static int 		condition_rate;
 
 // land command
 // ------------
@@ -445,12 +445,10 @@ static struct 	Location prev_WP;					// last waypoint
 static struct 	Location current_loc;				// current location
 static struct 	Location next_WP;					// next waypoint
 static struct 	Location target_WP;					// where do we want to you towards?
-static struct 	Location simple_WP;					//
 static struct 	Location next_command;				// command preloaded
 static struct   Location guided_WP;					// guided mode waypoint
 static long 	target_altitude;					// used for
 static boolean	home_is_set; 						// Flag for if we have g_gps lock and have set the home location
-static boolean  new_location;						// flag to tell us if location has been updated
 
 // IMU variables
 // -------------
@@ -1047,9 +1045,14 @@ static void update_navigation()
 		case GUIDED:
 			wp_control = WP_MODE;
             // check if we are close to point > loiter
+			wp_verify_byte = 0;
 			verify_nav_wp();
 
+			if (wp_control == WP_MODE) {
 				update_auto_yaw();
+			} else {
+				set_mode(LOITER);
+			}
 			update_nav_wp();
 			break;
 

ArduCopter/control_modes.pde

@@ -42,7 +42,9 @@ static void reset_control_switch()
 	read_control_switch();
 }
 
+#if CH7_OPTION == CH7_SET_HOVER
 static boolean trim_flag;
+#endif
 
 // read at 10 hz
 // set this to your trainer switch

ArduCopter/heli.pde

@@ -136,9 +136,11 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o
 			APM_RC.OutputCh(CH_7, g.heli_ext_gyro_gain);
 		}
 
-		// InstantPWM - force message to the servos
+		#if INSTANT_PWM == 1
+		// InstantPWM
 		APM_RC.Force_Out0_Out1();
 		APM_RC.Force_Out2_Out3();
+		#endif
 
 		// reset the averaging
 		heli_servo_out_count = 0;
@@ -149,6 +151,15 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o
 	}
 }
 
+static void init_motors_out()
+{
+	#if INSTANT_PWM == 0
+	ICR5 = 5000;	// 400 hz output 	CH 1, 2, 9
+	ICR1 = 5000;	// 400 hz output	CH 3, 4, 10
+	ICR3 = 40000;	// 50 hz output		CH 7, 8, 11
+	#endif
+}
+
 // these are not really motors, they're servos but we don't rename the function because it fits with the rest of the code better
 static void output_motors_armed()
 {

ArduCopter/navigation.pde

@@ -95,9 +95,16 @@ static void calc_loiter(int x_error, int y_error)
 // nav_roll, nav_pitch
 static void calc_loiter_pitch_roll()
 {
+
+	float temp  	 = radians((float)(9000 - (dcm.yaw_sensor))/100.0);
+	float _cos_yaw_x = cos(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);
+
 	// 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;
@@ -232,7 +239,7 @@ static void reset_crosstrack()
 	crosstrack_bearing 	= get_bearing(&current_loc, &next_WP);	// Used for track following
 }
 */
-static long get_altitude_above_home(void)
+/*static long get_altitude_above_home(void)
 {
 	// This is the altitude above the home location
 	// The GPS gives us altitude at Sea Level
@@ -240,7 +247,7 @@ static long get_altitude_above_home(void)
 	// -------------------------------------------------------------
 	return current_loc.alt - home.alt;
 }
-
+*/
 // distance is returned in meters
 static long get_distance(struct Location *loc1, struct Location *loc2)
 {
@@ -252,12 +259,12 @@ static long get_distance(struct Location *loc1, struct Location *loc2)
 	float dlong		= ((float)(loc2->lng - loc1->lng)) * scaleLongDown;
 	return sqrt(sq(dlat) + sq(dlong)) * .01113195;
 }
-
+/*
 static long get_alt_distance(struct Location *loc1, struct Location *loc2)
 {
 	return abs(loc1->alt - loc2->alt);
 }
-
+*/
 static long get_bearing(struct Location *loc1, struct Location *loc2)
 {
 	long off_x = loc2->lng - loc1->lng;

ArduCopter/radio.pde

@@ -184,11 +184,3 @@ static void trim_radio()
 	g.rc_4.save_eeprom();
 }
 
-static void trim_yaw()
-{
-	for (byte i = 0; i < 30; i++){
-		read_radio();
-	}
-	g.rc_4.trim();	// yaw
-}
-

ArduCopter/read_commands.pde

@@ -1,112 +0,0 @@
-/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-#if 0
-
-#define INPUT_BUF_LEN 40
-char input_buffer[INPUT_BUF_LEN];
-
-static void readCommands(void)
-{
-	static byte header[2];
-	const byte read_GS_header[] 	= {0x21, 0x21}; //!! Used to verify the payload msg header
-
-	if(Serial.available()){
-		//Serial.println("Serial.available");
-		byte bufferPointer;
-
-		header[0] = Serial.read();
-		header[1] = Serial.read();
-
-		if((header[0] == read_GS_header[0]) && (header[1] == read_GS_header[1])){
-
-			// Block until we read full command
-			// --------------------------------
-			delay(20);
-			byte incoming_val = 0;
-
-			// Ground Station communication
-			// ----------------------------
-			while(Serial.available() > 0)
-			{
-				incoming_val = Serial.read();
-
-				if (incoming_val != 13 && incoming_val != 10 ) {
-					input_buffer[bufferPointer++] = incoming_val;
-				}
-
-				if(bufferPointer >= INPUT_BUF_LEN){
-					Serial.println("Big buffer overrun");
-					bufferPointer = 0;
-					input_buffer[0] = 1;
-					Serial.flush();
-					memset(input_buffer,0,sizeof(input_buffer));
-					return;
-				}
-			}
-			parseCommand(input_buffer);
-
-			// clear buffer of old data
-			// ------------------------
-			memset(input_buffer,0,sizeof(input_buffer));
-
-		}else{
-			Serial.flush();
-		}
-	}
-}
-
-// Commands can be sent as !!a:100|b:200|c:1
-// -----------------------------------------
-static void parseCommand(char *buffer)
-{
-	Serial.println("got cmd ");
-	Serial.println(buffer);
-	char *token, *saveptr1, *saveptr2;
-
-	for (int j = 1;; j++, buffer = NULL) {
-		token = strtok_r(buffer, "|", &saveptr1);
-		if (token == NULL) break;
-
-		char * cmd 		= strtok_r(token, ":", &saveptr2);
-		long value		= strtol(strtok_r (NULL,":", &saveptr2), NULL,0);
-
-		///*
-		Serial.print("cmd ");
-		Serial.print(cmd[0]);
-		Serial.print("\tval ");
-		Serial.println(value);
-		Serial.println("");
-		//*/
-		///*
-		switch(cmd[0]){
-			case 'P':
-				g.pi_stabilize_roll.kP((float)value / 1000);
-				g.pi_stabilize_pitch.kP((float)value / 1000);
-				g.pi_stabilize_pitch.save_gains();
-				break;
-
-			case 'I':
-				g.pi_stabilize_roll.kI((float)value / 1000);
-				g.pi_stabilize_pitch.kI((float)value / 1000);
-				g.pi_stabilize_pitch.save_gains();
-				break;
-
-			case 'D':
-				//g.pi_stabilize_roll.kD((float)value / 1000);
-				//g.pi_stabilize_pitch.kD((float)value / 1000);
-				break;
-
-			case 'X':
-				g.pi_stabilize_roll.imax(value * 100);
-				g.pi_stabilize_pitch.imax(value * 100);
-				g.pi_stabilize_pitch.save_gains();
-				break;
-
-			case 'R':
-				//g.stabilize_dampener.set_and_save((float)value / 1000);
-				break;
-		}
-		//*/
-	}
-}
-
-#endif

ArduCopter/system.pde

@@ -83,7 +83,6 @@ static void init_ardupilot()
 	//
 	report_version();
 
-
 	// setup IO pins
 	pinMode(C_LED_PIN, OUTPUT);				// GPS status LED
 	pinMode(A_LED_PIN, OUTPUT);				// GPS status LED
@@ -143,6 +142,7 @@ static void init_ardupilot()
 		}
 
 	}else{
+
 	    // Load all auto-loaded EEPROM variables
 	    AP_Var::load_all();
 	}
@@ -157,7 +157,6 @@ static void init_ardupilot()
 	//
 	Serial3.begin(map_baudrate(g.serial3_baud,SERIAL3_BAUD), 128, 128);
 
-
 	#ifdef RADIO_OVERRIDE_DEFAULTS
 	{
 		int16_t rc_override[8] = RADIO_OVERRIDE_DEFAULTS;
@@ -216,11 +215,7 @@ static void init_ardupilot()
 	//
 	if (check_startup_for_CLI()) {
 		digitalWrite(A_LED_PIN,HIGH);		// turn on setup-mode LED
-		Serial.printf_P(PSTR("\n"
+		Serial.printf_P(PSTR("\nCLI:\n\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();
@@ -544,6 +539,6 @@ static uint32_t map_baudrate(int8_t rate, uint32_t default_baud)
     case 111:  return 111100;
     case 115:  return 115200;
     }
-    Serial.println_P(PSTR("Invalid SERIAL3_BAUD"));
+    //Serial.println_P(PSTR("Invalid SERIAL3_BAUD"));
     return default_baud;
 }

ArduCopter/test.pde

@@ -29,7 +29,7 @@ static int8_t	test_sonar(uint8_t argc, 		const Menu::arg *argv);
 #ifdef OPTFLOW_ENABLED
 static int8_t	test_optflow(uint8_t argc, 		const Menu::arg *argv);
 #endif
-static int8_t	test_xbee(uint8_t argc, 		const Menu::arg *argv);
+//static int8_t	test_xbee(uint8_t argc, 		const Menu::arg *argv);
 static int8_t	test_eedump(uint8_t argc, 		const Menu::arg *argv);
 static int8_t	test_rawgps(uint8_t argc, 		const Menu::arg *argv);
 //static int8_t	test_mission(uint8_t argc, 		const Menu::arg *argv);

libraries/Desktop/Desktop.mk

@@ -171,7 +171,7 @@ else
 endif
 
 # these are library objects we don't want in the desktop build (maybe we'll add them later)
-NODESKTOP		:= FastSerial/FastSerial.cpp AP_Compass/AP_Compass_HMC5843.cpp APM_BMP085/APM_BMP085.cpp AP_IMU/AP_IMU_Oilpan.cpp AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp
+NODESKTOP		:= FastSerial/FastSerial.cpp AP_Compass/AP_Compass_HMC5843.cpp APM_BMP085/APM_BMP085.cpp AP_IMU/AP_IMU_Oilpan.cpp AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp RC_Channel/RC_Channel_aux.cpp
 
 #
 # Find sketchbook libraries referenced by the sketch.

libraries/Desktop/Makefile.desktop

@@ -19,3 +19,6 @@ hilnocli:
 
 heli:
 	make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DFRAME_CONFIG=HELI_FRAME"
+
+helihil:
+	make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DFRAME_CONFIG=HELI_FRAME -DHIL_MODE=HIL_MODE_ATTITUDE -DCLI_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED"