From 0629bb01b82fbfe45c1270b4d4fb9fb459faf5c0 Mon Sep 17 00:00:00 2001
From: jasonshort <jasonshort@f9c3cf11-9bcb-44bc-f272-b75c42450872>
Date: Thu, 12 May 2011 21:37:38 +0000
Subject: [PATCH] Logging updates

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2250 f9c3cf11-9bcb-44bc-f272-b75c42450872
---
 ArduCopterMega/Log.pde              | 98 +++++++++++++++--------------
 ArduCopterMega/commands_logic.pde   |  1 +
 ArduCopterMega/commands_process.pde |  2 +-
 3 files changed, 53 insertions(+), 48 deletions(-)

diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde
index 4372fd1454..829408d0ad 100644
--- a/ArduCopterMega/Log.pde
+++ b/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,22 +425,22 @@ 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
-							//	1   2   3   4      5		6	 7		 8		9
-	Serial.printf_P(PSTR("GPS, %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f\n"),
+{
+	Serial.printf_P(PSTR("GPS, %ld, %d, %d, "
+					  "%4.7f, %4.7f, %4.4f, %4.4f, "
+					  "%4.4f, %d\n"),
 
-			DataFlash.ReadLong(),					// 1 time
-			(int)DataFlash.ReadByte(),				// 2 fix
-			(int)DataFlash.ReadByte(),				// 3 sats
+						DataFlash.ReadLong(),					// 1 time
+						(int)DataFlash.ReadByte(),				// 2 fix
+						(int)DataFlash.ReadByte(),				// 3 sats
 
-			(float)DataFlash.ReadLong() / t7,		// 4 lat
-			(float)DataFlash.ReadLong() / t7,		// 5 lon
-			(float)DataFlash.ReadLong() / 100.0,	// 6 gps alt
-			(float)DataFlash.ReadLong() / 100.0,	// 7 sensor alt
+						(float)DataFlash.ReadLong() / t7,		// 4 lat
+						(float)DataFlash.ReadLong() / t7,		// 5 lon
+						(float)DataFlash.ReadLong() / 100.0,	// 6 gps alt
+						(float)DataFlash.ReadLong() / 100.0,	// 7 sensor alt
 
-			(float)DataFlash.ReadInt() / 100.0,		// 8 ground speed
-			(float)DataFlash.ReadInt());			// 9 ground course
+						DataFlash.ReadInt(),					// 8 ground speed
+						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;
-	long logvarl;
+	Serial.printf_P(PSTR( "CMD, %d, %d, %d, %d, %d, %ld, %ld, %ld\n"),
 
-	Serial.printf_P(PSTR("CMD:"));
-	for(int i = 1; i <= 4; i++) {
-		logvarb = DataFlash.ReadByte();
-		Serial.print(logvarb, DEC);
-		Serial.print(comma);
-	}
-	for(int i = 1; i <= 3; i++) {
-		logvarl = DataFlash.ReadLong();
-		Serial.print(logvarl, DEC);
-		Serial.print(comma);
-	}
-	Serial.println(" ");
+				// WP total
+				DataFlash.ReadByte(),
+
+				// num, id, p1, options
+				DataFlash.ReadByte(),
+				DataFlash.ReadByte(),
+				DataFlash.ReadByte(),
+				DataFlash.ReadByte(),
+
+				// Alt, lat long
+				DataFlash.ReadLong(),
+				DataFlash.ReadLong(),
+				DataFlash.ReadLong());
 }
 
 // Write an attitude packet. Total length : 10 bytes
diff --git a/ArduCopterMega/commands_logic.pde b/ArduCopterMega/commands_logic.pde
index b191ddbe2c..0c71af4b49 100644
--- a/ArduCopterMega/commands_logic.pde
+++ b/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;
diff --git a/ArduCopterMega/commands_process.pde b/ArduCopterMega/commands_process.pde
index 3b7f28ae9b..72af331fba 100644
--- a/ArduCopterMega/commands_process.pde
+++ b/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;