Logging updates

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

ArduCopterMega/Log.pde

@@ -319,25 +319,6 @@ int find_last_log_page(int bottom_page)
 	return top_page;
 }
 
-
-
-// Write a command processing packet. Total length : 19 bytes
-//void Log_Write_Cmd(byte num, byte id, byte p1, long alt, long lat, long lng)
-void Log_Write_Cmd(byte num, struct Location *wp)
-{
-	DataFlash.WriteByte(HEAD_BYTE1);
-	DataFlash.WriteByte(HEAD_BYTE2);
-	DataFlash.WriteByte(LOG_CMD_MSG);
-	DataFlash.WriteByte(num);
-	DataFlash.WriteByte(wp->id);
-	DataFlash.WriteByte(wp->p1);
-	DataFlash.WriteByte(wp->options);
-	DataFlash.WriteLong(wp->alt);
-	DataFlash.WriteLong(wp->lat);
-	DataFlash.WriteLong(wp->lng);
-	DataFlash.WriteByte(END_BYTE);
-}
-
 void Log_Write_Nav_Tuning()
 {
 	Matrix3f tempmat = dcm.get_dcm_matrix();
@@ -436,7 +417,7 @@ void Log_Write_GPS()
 	DataFlash.WriteLong(g_gps->altitude);					// 6
 	DataFlash.WriteLong(current_loc.alt);					// 7
 
-	DataFlash.WriteInt(g_gps->ground_speed);				// 8
+	DataFlash.WriteInt(g_gps->ground_speed/100);			// 8
 	DataFlash.WriteInt((int)(g_gps->ground_course/100));	// 9
 
 	DataFlash.WriteByte(END_BYTE);
@@ -444,10 +425,10 @@ void Log_Write_GPS()
 
 // Read a GPS packet
 void Log_Read_GPS()
-{						//	1		 2	3  4			5			 6		   7	   8       9
+{
-					//GPS, 77361250, 1, 9, 40.0584750, -105.2034500, 166.2600, 2.8100, 0.0600, 266.0000
+	Serial.printf_P(PSTR("GPS, %ld, %d, %d, "
-							//	1   2   3   4      5		6	 7		 8		9
+					  "%4.7f, %4.7f, %4.4f, %4.4f, "
-	Serial.printf_P(PSTR("GPS, %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f\n"),
+					  "%4.4f, %d\n"),
 
 						DataFlash.ReadLong(),					// 1 time
 						(int)DataFlash.ReadByte(),				// 2 fix
@@ -458,8 +439,8 @@ void Log_Read_GPS()
 						(float)DataFlash.ReadLong() / 100.0,	// 6 gps alt
 						(float)DataFlash.ReadLong() / 100.0,	// 7 sensor alt
 
-			(float)DataFlash.ReadInt() / 100.0,		// 8 ground speed
+						DataFlash.ReadInt(),					// 8 ground speed
-			(float)DataFlash.ReadInt());			// 9 ground course
+						DataFlash.ReadInt());					// 9 ground course
 }
 
 
@@ -624,24 +605,47 @@ void Log_Read_Performance()
 	Serial.println(" ");
 }
 
+// Write a command processing packet.
+//void Log_Write_Cmd(byte num, byte id, byte p1, long alt, long lat, long lng)
+void Log_Write_Cmd(byte num, struct Location *wp)
+{
+	DataFlash.WriteByte(HEAD_BYTE1);
+	DataFlash.WriteByte(HEAD_BYTE2);
+	DataFlash.WriteByte(LOG_CMD_MSG);
+
+	DataFlash.WriteByte(g.waypoint_total);
+
+	DataFlash.WriteByte(num);
+	DataFlash.WriteByte(wp->id);
+	DataFlash.WriteByte(wp->p1);
+	DataFlash.WriteByte(wp->options);
+
+	DataFlash.WriteLong(wp->alt);
+	DataFlash.WriteLong(wp->lat);
+	DataFlash.WriteLong(wp->lng);
+
+	DataFlash.WriteByte(END_BYTE);
+}
+
+
 // Read a command processing packet
 void Log_Read_Cmd()
 {
-	byte logvarb;
+	Serial.printf_P(PSTR( "CMD, %d, %d, %d, %d, %d, %ld, %ld, %ld\n"),
-	long logvarl;
 
-	Serial.printf_P(PSTR("CMD:"));
+				// WP total
-	for(int i = 1; i <= 4; i++) {
+				DataFlash.ReadByte(),
-		logvarb = DataFlash.ReadByte();
+
-		Serial.print(logvarb, DEC);
+				// num, id, p1, options
-		Serial.print(comma);
+				DataFlash.ReadByte(),
-	}
+				DataFlash.ReadByte(),
-	for(int i = 1; i <= 3; i++) {
+				DataFlash.ReadByte(),
-		logvarl = DataFlash.ReadLong();
+				DataFlash.ReadByte(),
-		Serial.print(logvarl, DEC);
+
-		Serial.print(comma);
+				// Alt, lat long
-	}
+				DataFlash.ReadLong(),
-	Serial.println(" ");
+				DataFlash.ReadLong(),
+				DataFlash.ReadLong());
 }
 
 // Write an attitude packet. Total length : 10 bytes

ArduCopterMega/commands_logic.pde

@@ -423,6 +423,7 @@ bool verify_nav_wp()
 		char message[30];
 		sprintf(message,"Reached Command #%i",command_must_index);
 		gcs.send_text(SEVERITY_LOW,message);
+		wp_verify_byte = 0;
 		return true;
 	}else{
 		return false;

ArduCopterMega/commands_process.pde

@@ -6,7 +6,7 @@ void change_command(uint8_t index)
 {
 	struct Location temp = get_command_with_index(index);
 
-	if (next_command.id > MAV_CMD_NAV_LAST ){
+	if (temp.id > MAV_CMD_NAV_LAST ){
 		gcs.send_text_P(SEVERITY_LOW,PSTR("error: non-Nav cmd"));
 	} else {
 		command_must_index 	= NO_COMMAND;