Merge branch 'master' of https://code.google.com/p/ardupilot-mega

ArduCopter/ArduCopter.pde

@@ -260,7 +260,12 @@ static byte 	control_mode		= STABILIZE;
 static byte 	old_control_mode	= STABILIZE;
 static byte 	oldSwitchPosition;					// for remembering the control mode switch
 static int16_t  motor_out[8];
-static bool		do_simple = false;
+static bool		do_simple 			= false;
+
+static int16_t rc_override[8] = {0,0,0,0,0,0,0,0};
+static bool rc_override_active = false;
+static uint32_t rc_override_fs_timer = 0;
+
 
 // Heli
 // ----
@@ -480,6 +485,7 @@ static int32_t 			perf_mon_timer;
 //static float 			imu_health; 						// Metric based on accel gain deweighting
 static int16_t			gps_fix_count;
 static byte				gps_watchdog;
+static int				pmTest1;
 
 // System Timers
 // --------------
@@ -1473,8 +1479,8 @@ static void update_nav_wp()
 		if (circle_angle > 6.28318531)
 			circle_angle -= 6.28318531;
 
-		target_WP.lng = next_WP.lng + (g.loiter_radius * cos(1.57 - circle_angle) * scaleLongUp);
+		target_WP.lng = next_WP.lng + (g.loiter_radius * 100 * cos(1.57 - circle_angle) * scaleLongUp);
-		target_WP.lat = next_WP.lat + (g.loiter_radius * sin(1.57 - circle_angle));
+		target_WP.lat = next_WP.lat + (g.loiter_radius * 100 * sin(1.57 - circle_angle));
 
 		// calc the lat and long error to the target
 		calc_location_error(&target_WP);

ArduCopter/GCS_Mavlink.pde

@@ -980,7 +980,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 			// set frame of waypoint
 			uint8_t frame;
 
-			if (tell_command.options & WP_OPTION_ALT_RELATIVE) {
+			if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) {
 				frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // reference frame
 			} else {
 				frame = MAV_FRAME_GLOBAL; // reference frame
@@ -1294,7 +1294,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 				guided_WP = tell_command;
 
 				// add home alt if needed
-				if (guided_WP.options & WP_OPTION_ALT_RELATIVE){
+				if (guided_WP.options & MASK_OPTIONS_RELATIVE_ALT){
 					guided_WP.alt += home.alt;
 				}
 
@@ -1322,7 +1322,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 					break;
 
 				if(packet.seq != 0)
-					set_command_with_index(tell_command, packet.seq);
+					set_cmd_with_index(tell_command, packet.seq);
 
 				// update waypoint receiving state machine
 				waypoint_timelast_receive = millis();

ArduCopter/Parameters.h

@@ -316,7 +316,7 @@ public:
 	command_index			(0,							k_param_command_index,					PSTR("WP_INDEX")),
 	command_nav_index		(0,							k_param_command_nav_index,				PSTR("WP_MUST_INDEX")),
 	waypoint_radius			(WP_RADIUS_DEFAULT,			k_param_waypoint_radius,				PSTR("WP_RADIUS")),
-	loiter_radius			(LOITER_RADIUS * 100,	    k_param_loiter_radius,					PSTR("WP_LOITER_RAD")),
+	loiter_radius			(LOITER_RADIUS,	    		k_param_loiter_radius,					PSTR("WP_LOITER_RAD")),
 	waypoint_speed_max		(WAYPOINT_SPEED_MAX,		k_param_waypoint_speed_max,				PSTR("WP_SPEED_MAX")),
 	crosstrack_gain			(CROSSTRACK_GAIN,			k_param_crosstrack_gain,				PSTR("XTRK_GAIN_SC")),
 

ArduCopter/commands.pde

@@ -50,7 +50,7 @@ static struct Location get_cmd_with_index(int i)
 	}
 
 	// Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative
-	//if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & WP_OPTION_ALT_RELATIVE){
+	//if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & MASK_OPTIONS_RELATIVE_ALT){
 		//temp.alt += home.alt;
 	//}
 
@@ -65,8 +65,9 @@ static struct Location get_cmd_with_index(int i)
 
 // Setters
 // -------
-static void set_command_with_index(struct Location temp, int i)
+static void set_cmd_with_index(struct Location temp, int i)
 {
+
 	i = constrain(i, 0, g.command_total.get());
 	//Serial.printf("set_command: %d with id: %d\n", i, temp.id);
 
@@ -203,7 +204,7 @@ static void init_home()
 	// Save Home to EEPROM
 	// -------------------
 	// no need to save this to EPROM
-	set_command_with_index(home, 0);
+	set_cmd_with_index(home, 0);
 	print_wp(&home, 0);
 
 	// Save prev loc this makes the calcs look better before commands are loaded

ArduCopter/commands_logic.pde

@@ -222,7 +222,7 @@ static void do_takeoff()
 	Location temp = current_loc;
 
 	// command_nav_queue.alt is a relative altitude!!!
-	if (command_nav_queue.options & WP_OPTION_ALT_RELATIVE) {
+	if (command_nav_queue.options & MASK_OPTIONS_RELATIVE_ALT) {
 		temp.alt = command_nav_queue.alt + home.alt;
 		//Serial.printf("rel alt: %ld",temp.alt);
 	} else {
@@ -242,7 +242,7 @@ static void do_nav_wp()
 	wp_control = WP_MODE;
 
 	// command_nav_queue.alt is a relative altitude!!!
-	if (command_nav_queue.options & WP_OPTION_ALT_RELATIVE) {
+	if (command_nav_queue.options & MASK_OPTIONS_RELATIVE_ALT) {
 		command_nav_queue.alt	+= home.alt;
 	}
 	set_next_WP(&command_nav_queue);
@@ -258,7 +258,7 @@ static void do_nav_wp()
 	loiter_time_max = command_nav_queue.p1 * 1000;
 
 	// if we don't require an altitude minimum, we save this flag as passed (1)
-	if((next_WP.options & WP_OPTION_ALT_REQUIRED) == 0){
+	if((next_WP.options & MASK_OPTIONS_RELATIVE_ALT) == 0){
 		// we don't need to worry about it
 		wp_verify_byte |= NAV_ALTITUDE;
 	}
@@ -383,7 +383,7 @@ static bool verify_land()
 static bool verify_nav_wp()
 {
 	// Altitude checking
-	if(next_WP.options & WP_OPTION_ALT_REQUIRED){
+	if(next_WP.options & MASK_OPTIONS_RELATIVE_ALT){
 		// we desire a certain minimum altitude
 		if (current_loc.alt > next_WP.alt){
 			// we have reached that altitude

ArduCopter/control_modes.pde

@@ -107,7 +107,7 @@ static void read_trim_switch()
 				current_loc.id = MAV_CMD_NAV_WAYPOINT;
 
 				// save command
-				set_command_with_index(current_loc, CH7_wp_index);
+				set_cmd_with_index(current_loc, CH7_wp_index);
 
 				// save the index
 				g.command_total.set_and_save(CH7_wp_index + 1);

ArduCopter/defines.h

@@ -120,6 +120,8 @@
 #define POSITION 8			// AUTO control
 #define NUM_MODES 9
 
+#define INITIALISING 9     // in startup routines
+
 #define SIMPLE_1 1
 #define SIMPLE_2 2
 #define SIMPLE_3 4
@@ -166,14 +168,14 @@
 #define CIRCLE_MODE 3
 
 // Waypoint options
-#define WP_OPTION_ALT_RELATIVE 		1
+#define MASK_OPTIONS_RELATIVE_ALT 		1
-#define WP_OPTION_ALT_CHANGE 		2
+#define WP_OPTION_ALT_CHANGE 			2
-#define WP_OPTION_YAW 				4
+#define WP_OPTION_YAW 					4
-#define WP_OPTION_ALT_REQUIRED		8
+#define WP_OPTION_ALT_REQUIRED			8
-#define WP_OPTION_RELATIVE			16
+#define WP_OPTION_RELATIVE				16
 //#define WP_OPTION_					32
 //#define WP_OPTION_					64
-#define WP_OPTION_NEXT_CMD			128
+#define WP_OPTION_NEXT_CMD				128
 
 //repeating events
 #define NO_REPEAT 0

ArduCopter/test.pde

@@ -970,38 +970,38 @@ static int8_t
 
 	// clear home
 	{Location t = {0,   	0,      0, 		0, 		0, 			0};
-	set_command_with_index(t,0);}
+	set_cmd_with_index(t,0);}
 
 	// CMD										opt						pitch   	alt/cm
 	{Location t = {MAV_CMD_NAV_TAKEOFF,   		WP_OPTION_RELATIVE,      0, 		100, 		0, 			0};
-	set_command_with_index(t,1);}
+	set_cmd_with_index(t,1);}
 
 	if (!strcmp_P(argv[1].str, PSTR("wp"))) {
 
 		// CMD											opt
 		{Location t = {MAV_CMD_NAV_WAYPOINT,  			WP_OPTION_RELATIVE,		15, 0, 0, 0};
-		set_command_with_index(t,2);}
+		set_cmd_with_index(t,2);}
 		// CMD											opt
 		{Location t = {MAV_CMD_NAV_RETURN_TO_LAUNCH,   	WP_OPTION_YAW,      0, 		0, 		0,		0};
-		set_command_with_index(t,3);}
+		set_cmd_with_index(t,3);}
 
 		// CMD											opt
 		{Location t = {MAV_CMD_NAV_LAND,				0,      0, 		0, 		0,		0};
-		set_command_with_index(t,4);}
+		set_cmd_with_index(t,4);}
 
 	} else {
 		//2250 = 25 meteres
 		// CMD										opt		p1		//alt		//NS		//WE
 		{Location t = {MAV_CMD_NAV_LOITER_TIME,   	0,      10,  	0, 			0,			0}; // 19
-		set_command_with_index(t,2);}
+		set_cmd_with_index(t,2);}
 
 		// CMD										opt		dir		angle/deg	deg/s	relative
 		{Location t = {MAV_CMD_CONDITION_YAW,		0,      1, 		360, 		60, 	1};
-		set_command_with_index(t,3);}
+		set_cmd_with_index(t,3);}
 
 		// CMD										opt
 		{Location t = {MAV_CMD_NAV_LAND,			0,      0, 		0, 			0, 		0};
-		set_command_with_index(t,4);}
+		set_cmd_with_index(t,4);}
 
 	}
 

ArduPlane/Log.pde

@@ -376,12 +376,19 @@ static int find_last_log_page(uint16_t log_number)
 		bottom_page_filepage = top_page_filepage;
 		do
 		{
+            int16_t last_bottom_page_file;
 			top_page = bottom_page;
 			bottom_page = bottom_page - bottom_page_filepage;
 			if(bottom_page < 1) bottom_page = 1;
 			DataFlash.StartRead(bottom_page);
+            last_bottom_page_file = bottom_page_file;
 			bottom_page_file = DataFlash.GetFileNumber();
 			bottom_page_filepage = DataFlash.GetFilePage();
+            if (bottom_page_file == last_bottom_page_file &&
+                bottom_page_filepage == 0) {
+                /* no progress can be made - give up. The log may be corrupt */
+                return -1;
+            }
 		} while (bottom_page_file != log_number && bottom_page != 1);
 
 		// Deal with stepping down too far due to overwriting a file

Tools/ArduPPM/ATMega32U2/Projects/arduino-usbserial/Arduino-usbserial.c

@@ -35,7 +35,7 @@
  */
 
 #include "Arduino-usbserial.h"
-#include "..\..\..\Libraries\ppm_encoder.h"
+#include "../../../Libraries/PPM_Encoder.h"
 
 
 /** Circular buffer to hold data from the host before it is sent to the device via the serial port. */

Tools/ArduPPM/Libraries/PPM_Encoder.h

@@ -243,18 +243,33 @@ const uint16_t failsafe_ppm[ PPM_ARRAY_MAX ] =
 #define USB_PORT              PORTC
 #define    USB_PIN            PC2
 
+// true if we have received a USB device connect event
+static bool usb_connected;
+
 // USB connected event
 void EVENT_USB_Device_Connect(void)
 {
     // Toggle USB pin high if USB is connected
     USB_PORT |= (1 << USB_PIN);
+
+    usb_connected = true;
+
+    // this unsets the pin connected to PA1 on the 2560
+    // when the bit is clear, USB is connected
+    PORTD &= ~1;
 }
 
-// USB dosconnect event
+// USB disconnect event
 void EVENT_USB_Device_Disconnect(void)
 {
     // toggle USB pin low if USB is disconnected
     USB_PORT &= ~(1 << USB_PIN);
+
+    usb_connected = false;
+
+    // this sets the pin connected to PA1 on the 2560
+    // when the bit is clear, USB is connected
+    PORTD |= 1;
 }
 
 // AVR parameters for ArduPilot MEGA v1.4 PPM Encoder (ATmega328P)
@@ -676,6 +691,17 @@ void ppm_encoder_init( void )
     // Activate pullups on all input pins
     SERVO_PORT |= 0xFF;
 
+#if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__)
+    // on 32U2 set PD0 to be an output, and clear the bit. This tells
+    // the 2560 that USB is connected. The USB connection event fires
+    // later to set the right value
+    DDRD |= 1;
+    if (usb_connected) {
+	    PORTD     &= ~1;
+    } else {
+	    PORTD     |= 1;
+    }
+#endif
 
     // SERVO/PPM INPUT - PIN CHANGE INTERRUPT
     // ------------------------------------------------------------------------------

Tools/autotest/mission2.txt

@@ -15,7 +15,7 @@ QGC WPL 110
 
 # MAV_CMD_NAV_LOITER_TIME
 #					seconds		empty		rad			Yaw	per		Lat			lon			Alt			continue
-4	0	3	19		35			20			0			1			0  			0			0			1
+4	0	3	19		35			0			0			1			0  			0			20			1
 
 # MAV_CMD_NAV_WAYPOINT	B
 #					Hold sec	Hit rad		empty		Yaw angle	lat			lon			alt			continue

libraries/Desktop/support/DataFlash.cpp

@@ -16,7 +16,7 @@
 static int flash_fd;
 static uint8_t buffer[2][DF_PAGE_SIZE];
 
-#define OVERWRITE_DATA 0 // 0: When reach the end page stop, 1: Start overwritten from page 1
+#define OVERWRITE_DATA 1 // 0: When reach the end page stop, 1: Start overwritten from page 1
 
 // Constructors ////////////////////////////////////////////////////////////////
 DataFlash_Class::DataFlash_Class()
@@ -120,7 +120,7 @@ void DataFlash_Class::ChipErase ()
 void DataFlash_Class::StartWrite(int16_t PageAdr)
 {
 	df_BufferNum  = 1;
-	df_BufferIdx  = 0;
+	df_BufferIdx  = 4;
 	df_PageAdr    = PageAdr;
 	df_Stop_Write = 0;
 
@@ -137,12 +137,12 @@ void DataFlash_Class::FinishWrite(void)
 	df_PageAdr++;
 	if (OVERWRITE_DATA==1)
 	    {
-        if (df_PageAdr>=4096)  // If we reach the end of the memory, start from the begining
+        if (df_PageAdr>=DF_LAST_PAGE)  // If we reach the end of the memory, start from the begining
 		  df_PageAdr = 1;
 	    }
 	else
 	    {
-        if (df_PageAdr>=4096)  // If we reach the end of the memory, stop here
+        if (df_PageAdr>=DF_LAST_PAGE)  // If we reach the end of the memory, stop here
 		  df_Stop_Write=1;
 	    }
 
@@ -167,17 +167,17 @@ void DataFlash_Class::WriteByte(byte data)
     df_BufferIdx++;
     if (df_BufferIdx >= df_PageSize)  // End of buffer?
       {
-      df_BufferIdx=0;
+      df_BufferIdx=4;
 	  BufferToPage(df_BufferNum,df_PageAdr,0);  // Write Buffer to memory, NO WAIT
       df_PageAdr++;
 	  if (OVERWRITE_DATA==1)
 	    {
-        if (df_PageAdr>=4096)  // If we reach the end of the memory, start from the begining
+        if (df_PageAdr>=DF_LAST_PAGE)  // If we reach the end of the memory, start from the begining
 		  df_PageAdr = 1;
 	    }
       else
 	    {
-        if (df_PageAdr>=4096)  // If we reach the end of the memory, stop here
+        if (df_PageAdr>=DF_LAST_PAGE)  // If we reach the end of the memory, stop here
 		  df_Stop_Write=1;
 	    }
 
@@ -218,7 +218,7 @@ int16_t DataFlash_Class::GetPage()
 void DataFlash_Class::StartRead(int16_t PageAdr)
 {
   df_Read_BufferNum=1;
-  df_Read_BufferIdx=0;
+  df_Read_BufferIdx=4;
   df_Read_PageAdr=PageAdr;
   WaitReady();
   PageToBuffer(df_Read_BufferNum,df_Read_PageAdr);  // Write Memory page to buffer
@@ -234,10 +234,10 @@ byte DataFlash_Class::ReadByte()
   df_Read_BufferIdx++;
   if (df_Read_BufferIdx >= df_PageSize)  // End of buffer?
     {
-    df_Read_BufferIdx=0;
+    df_Read_BufferIdx=4;
     PageToBuffer(df_Read_BufferNum,df_Read_PageAdr);  // Write memory page to Buffer
     df_Read_PageAdr++;
-    if (df_Read_PageAdr>=4096)  // If we reach the end of the memory, start from the begining
+    if (df_Read_PageAdr>=DF_LAST_PAGE)  // If we reach the end of the memory, start from the begining
       {
       df_Read_PageAdr = 0;
       df_Read_END = true;