From a69c9ff674bbe7b548e40938b789cd7576e6d90a Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 2 Apr 2013 13:08:23 +1100
Subject: [PATCH] Plane: added logging of DIGICAM events

---
 ArduPlane/Log.pde            | 51 ++++++++++++++++++++++++++++++++++++
 ArduPlane/commands_logic.pde | 11 +++++++-
 ArduPlane/defines.h          |  1 +
 3 files changed, 62 insertions(+), 1 deletion(-)

diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde
index f61866d027..76325650a4 100644
--- a/ArduPlane/Log.pde
+++ b/ArduPlane/Log.pde
@@ -331,6 +331,53 @@ static void Log_Read_Cmd()
         (long)pkt.waypoint_longitude);
 }
 
+struct log_Camera {
+    LOG_PACKET_HEADER;
+    uint32_t gps_time;
+    int32_t  latitude;
+    int32_t  longitude;
+    int32_t  altitude;
+    int16_t  roll;
+    int16_t  pitch;
+    uint16_t yaw;
+};
+
+// Write a Camera packet. Total length : 26 bytes
+static void Log_Write_Camera()
+{
+#if CAMERA == ENABLED
+    struct log_Camera pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_CAMERA_MSG),
+        gps_time    : g_gps->time,
+        latitude    : current_loc.lat,
+        longitude   : current_loc.lng,
+        altitude    : current_loc.alt,
+        roll        : (int16_t)ahrs.roll_sensor,
+        pitch       : (int16_t)ahrs.pitch_sensor,
+        yaw         : (uint16_t)ahrs.yaw_sensor
+    };
+    DataFlash.WriteBlock(&pkt, sizeof(pkt));
+#endif
+}
+
+// Read a camera packet
+static void Log_Read_Camera()
+{
+    struct log_Camera pkt;
+    DataFlash.ReadPacket(&pkt, sizeof(pkt));
+                                     // 1
+    cliSerial->printf_P(PSTR("CAMERA, %lu, "),(unsigned long)pkt.gps_time); // 1 time
+    print_latlon(cliSerial, pkt.latitude);              // 2 lat
+    cliSerial->print_P(PSTR(", "));
+    print_latlon(cliSerial, pkt.longitude);             // 3 lon
+                               // 4   5   6   7
+    cliSerial->printf_P(PSTR(", %ld, %d, %d, %u\n"),
+                    (long)pkt.altitude,                 // 4 altitude
+                    (int)pkt.roll,                      // 5 roll in centidegrees
+                    (int)pkt.pitch,                     // 6 pitch in centidegrees
+                    (unsigned)pkt.yaw);                 // 7 yaw in centidegrees
+}
+
 struct log_Startup {
     LOG_PACKET_HEADER;
     uint8_t startup_type;
@@ -643,6 +690,9 @@ static void log_callback(uint8_t msgid)
     case LOG_GPS_MSG:
         Log_Read_GPS();
         break;
+    case LOG_CAMERA_MSG:
+        Log_Read_Camera();
+        break;
     }
 }
 
@@ -660,6 +710,7 @@ static void Log_Write_Performance() {}
 static void Log_Write_Attitude() {}
 static void Log_Write_Control_Tuning() {}
 static void Log_Write_IMU() {}
+static void Log_Write_Camera() {}
 
 static int8_t process_logs(uint8_t argc, const Menu::arg *argv) {
     return 0;
diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde
index c7d246228e..29a614f28d 100644
--- a/ArduPlane/commands_logic.pde
+++ b/ArduPlane/commands_logic.pde
@@ -114,7 +114,7 @@ static void handle_process_do_command()
         break;
 
     case MAV_CMD_DO_DIGICAM_CONTROL:                    // Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty|
-        camera.trigger_pic();
+        do_take_picture();
         break;
 #endif
 
@@ -641,3 +641,12 @@ static void do_repeat_relay()
     event_state.repeat          = next_nonnav_command.alt * 2;
     update_events();
 }
+
+// do_take_picture - take a picture with the camera library
+static void do_take_picture()
+{
+#if CAMERA == ENABLED
+    camera.trigger_pic();
+    Log_Write_Camera();
+#endif
+}
diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h
index 4246fe97b0..447b801675 100644
--- a/ArduPlane/defines.h
+++ b/ArduPlane/defines.h
@@ -160,6 +160,7 @@ enum log_messages {
     LOG_STARTUP_MSG,
     TYPE_AIRSTART_MSG,
     TYPE_GROUNDSTART_MSG,
+    LOG_CAMERA_MSG,
     MAX_NUM_LOGS
 };