From 3a90fc77f96b561881794bd23c01fdc45ad831f6 Mon Sep 17 00:00:00 2001
From: rmackay9 <rmackay9@yahoo.com>
Date: Fri, 7 Dec 2012 00:57:08 +0900
Subject: [PATCH] ArduCopter: add dataflash logging of camera events

---
 ArduCopter/Log.pde            | 51 ++++++++++++++++++++++++++++++++++-
 ArduCopter/commands_logic.pde |  6 ++++-
 ArduCopter/config.h           |  5 +++-
 ArduCopter/defines.h          | 15 ++---------
 4 files changed, 61 insertions(+), 16 deletions(-)

diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde
index dedec9d0e5..e5df669c80 100644
--- a/ArduCopter/Log.pde
+++ b/ArduCopter/Log.pde
@@ -88,6 +88,7 @@ print_log_menu(void)
         if (g.log_bitmask & MASK_LOG_PID) cliSerial->printf_P(PSTR(" PID"));
         if (g.log_bitmask & MASK_LOG_ITERM) cliSerial->printf_P(PSTR(" ITERM"));
         if (g.log_bitmask & MASK_LOG_INAV) cliSerial->printf_P(PSTR(" INAV"));
+        if (g.log_bitmask & MASK_LOG_CAMERA) cliSerial->printf_P(PSTR(" CAMERA"));
     }
 
     cliSerial->println();
@@ -205,6 +206,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
         TARG(PID);
         TARG(ITERM);
         TARG(INAV);
+        TARG(CAMERA);
  #undef TARG
     }
 
@@ -1038,7 +1040,7 @@ static void Log_Write_DMP()
 #endif
 }
 
-// Read an attitude packet
+// Read a DMP attitude packet
 static void Log_Read_DMP()
 {
     int16_t temp1   = DataFlash.ReadInt();
@@ -1058,6 +1060,49 @@ static void Log_Read_DMP()
                     (unsigned)temp6);
 }
 
+// Write a Camera packet. Total length : 26 bytes
+static void Log_Write_Camera()
+{
+#if CAMERA == ENABLED
+    DataFlash.WriteByte(HEAD_BYTE1);
+    DataFlash.WriteByte(HEAD_BYTE2);
+    DataFlash.WriteByte(LOG_CAMERA_MSG);
+
+    DataFlash.WriteLong(g_gps->time);           		 // 1
+    DataFlash.WriteLong(current_loc.lat);       		 // 2
+    DataFlash.WriteLong(current_loc.lng);       		 // 3
+    DataFlash.WriteLong(current_loc.alt);       		 // 4
+    DataFlash.WriteInt((int16_t)ahrs.roll_sensor);       // 5
+    DataFlash.WriteInt((int16_t)ahrs.pitch_sensor);      // 6
+    DataFlash.WriteInt((uint16_t)ahrs.yaw_sensor);       // 7
+    DataFlash.WriteByte(END_BYTE);
+#endif
+}
+
+// Read a camera packet
+static void Log_Read_Camera()
+{
+    int32_t temp1   = DataFlash.ReadLong();             // 1 time
+    int32_t temp2   = DataFlash.ReadLong();             // 2 lat
+    int32_t temp3   = DataFlash.ReadLong();             // 3 lon
+    float temp4     = DataFlash.ReadLong() / 100.0;     // 4 altitude
+    int16_t temp5   = DataFlash.ReadInt();              // 5 roll in centidegrees
+    int16_t temp6   = DataFlash.ReadInt();              // 6 pitch in centidegrees
+    uint16_t temp7  = DataFlash.ReadInt();              // 7 yaw in centidegrees
+
+                                     // 1
+    cliSerial->printf_P(PSTR("CAMERA, %ld, "),(long)temp1);  // 1 time
+    print_latlon(cliSerial, temp2);                         // 2 lat
+    cliSerial->print_P(PSTR(", "));
+    print_latlon(cliSerial, temp3);                         // 3 lon
+                                 // 4   5   6   7
+    cliSerial->printf_P(PSTR(", %4.4f, %d, %d, %u\n"),
+                    temp4,                                  // 4 altitude
+                    (int)temp5,                             // 5 roll in centidegrees
+                    (int)temp6,                             // 6 pitch in centidegrees
+                    (unsigned int)temp7);                   // 7 yaw in centidegrees
+}
+
 // Read the DataFlash log memory
 static void Log_Read(int16_t start_page, int16_t end_page)
 {
@@ -1193,6 +1238,10 @@ static int16_t Log_Read_Process(int16_t start_page, int16_t end_page)
 					case LOG_INAV_MSG:
 						Log_Read_INAV();
 						break;
+
+					case LOG_CAMERA_MSG:
+						Log_Read_Camera();
+						break;
 				}
 				break;
 		case 3:
diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde
index 2ab431ba2f..663e7d4cb5 100644
--- a/ArduCopter/commands_logic.pde
+++ b/ArduCopter/commands_logic.pde
@@ -920,6 +920,10 @@ static void do_nav_roi()
 // do_take_picture - take a picture with the camera library
 static void do_take_picture()
 {
+#if CAMERA == ENABLED
     g.camera.trigger_pic();
-    Serial.print_P(PSTR("Camera!!"));
+    if (g.log_bitmask & MASK_LOG_CAMERA) {
+        Log_Write_Camera();
+    }
+#endif
 }
diff --git a/ArduCopter/config.h b/ArduCopter/config.h
index 82fa494161..8799e457f4 100644
--- a/ArduCopter/config.h
+++ b/ArduCopter/config.h
@@ -1003,7 +1003,10 @@
  # define LOG_ITERM                     ENABLED
 #endif
 #ifndef LOG_INAV
- # define LOG_INAV                              DISABLED
+ # define LOG_INAV                      DISABLED
+#endif
+#ifndef LOG_CAMERA
+ # define LOG_CAMERA                    DISABLED
 #endif
 
 // calculate the default log_bitmask
diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index 6036065f9d..1c97221e3b 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -307,6 +307,7 @@ enum gcs_severity {
 #define LOG_ITERM_MSG                   0x0F
 #define LOG_DMP_MSG                     0x10
 #define LOG_INAV_MSG                    0x11
+#define LOG_CAMERA_MSG                  0x12
 #define LOG_INDEX_MSG                   0xF0
 #define MAX_NUM_LOGS                    50
 
@@ -325,19 +326,7 @@ enum gcs_severity {
 #define MASK_LOG_PID                    (1<<12)
 #define MASK_LOG_ITERM                  (1<<13)
 #define MASK_LOG_INAV                   (1<<14)
-
-
-#define MASK_LOG_CTUN                   (1<<4)
-#define MASK_LOG_NTUN                   (1<<5)
-#define MASK_LOG_MODE                   (1<<6)
-#define MASK_LOG_RAW                    (1<<7)
-#define MASK_LOG_CMD                    (1<<8)
-#define MASK_LOG_CUR                    (1<<9)
-#define MASK_LOG_MOTORS                 (1<<10)
-#define MASK_LOG_OPTFLOW                (1<<11)
-#define MASK_LOG_PID                    (1<<12)
-#define MASK_LOG_ITERM                  (1<<13)
-
+#define MASK_LOG_CAMERA                 (1<<15)
 
 // DATA - event logging
 #define DATA_MAVLINK_FLOAT              1