From 9bba55f937b256e39bb6285cb93325387016ecc5 Mon Sep 17 00:00:00 2001
From: Dario Lindo Andres <proyectoire@gmail.com>
Date: Sun, 7 Jun 2015 19:17:47 +0200
Subject: [PATCH] ArduPlane: Added precise camera trigger logging

Added update_trigger and check_digital_pin functions
added camera trigger precise time mark
detect camera feedback pin status
added support for simple digital pin
included support for digital pin. Already included in
added support for TRIGGER MSG
---
 ArduPlane/ArduPlane.cpp      | 14 ++++++++++++++
 ArduPlane/Log.cpp            |  2 ++
 ArduPlane/Plane.h            |  4 +++-
 ArduPlane/commands_logic.cpp | 14 +++++++++++---
 ArduPlane/defines.h          |  3 ++-
 ArduPlane/system.cpp         | 20 +++++++++++++++++++-
 6 files changed, 51 insertions(+), 6 deletions(-)

diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
index af6f838573..322a8d01df 100644
--- a/ArduPlane/ArduPlane.cpp
+++ b/ArduPlane/ArduPlane.cpp
@@ -70,6 +70,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
     SCHED_TASK(rpm_update,             10,    200),
     SCHED_TASK(airspeed_ratio_update,   1,   1000),
     SCHED_TASK(update_mount,           50,   1500),
+    SCHED_TASK(update_trigger,          1,   1500),
     SCHED_TASK(log_perf_info,         0.1,   1000),
     SCHED_TASK(compass_save,        0.016,   2500),
     SCHED_TASK(update_logging1,        10,   1700),
@@ -199,9 +200,22 @@ void Plane::update_mount(void)
 #if MOUNT == ENABLED
     camera_mount.update();
 #endif
+}
 
+/*
+  update camera trigger
+ */
+void Plane::update_trigger(void)
+{
 #if CAMERA == ENABLED
     camera.trigger_pic_cleanup();
+    if(camera._camera_triggered == 0 && camera._feedback_pin != -1 && check_digital_pin(camera._feedback_pin) == 0){
+      gcs_send_message(MSG_CAMERA_FEEDBACK);
+      if (should_log(MASK_LOG_CAMERA)) {
+          DataFlash.Log_Write_Camera(ahrs, gps, current_loc);
+      }
+      camera._camera_triggered = 1;
+    }    
 #endif
 }
 
diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp
index c877b2a608..a119ce8e28 100644
--- a/ArduPlane/Log.cpp
+++ b/ArduPlane/Log.cpp
@@ -49,6 +49,7 @@ bool Plane::print_log_menu(void)
         PLOG(CAMERA);
         PLOG(RC);
         PLOG(SONAR);
+        PLOG(TRIGGER);        
  #undef PLOG
     }
 
@@ -128,6 +129,7 @@ int8_t Plane::select_logs(uint8_t argc, const Menu::arg *argv)
         TARG(CAMERA);
         TARG(RC);
         TARG(SONAR);
+        TARG(TRIGGER);        
  #undef TARG
     }
 
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 5b5d0b64bb..274cde6d7b 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -906,6 +906,7 @@ private:
     void update_notify();
     void resetPerfData(void);
     void check_usb_mux(void);
+    uint8_t check_digital_pin(uint8_t pin);    
     void print_comma(void);
     void servo_write(uint8_t ch, uint16_t pwm);
     bool should_log(uint32_t mask);
@@ -933,6 +934,7 @@ private:
     void one_second_loop(void);
     void airspeed_ratio_update(void);
     void update_mount(void);
+    void update_trigger(void);    
     void log_perf_info(void);
     void compass_save(void);
     void update_logging1(void);
@@ -1054,4 +1056,4 @@ extern Plane plane;
 using AP_HAL::millis;
 using AP_HAL::micros;
 
-#endif // _PLANE_H_
+#endif // _PLANE_H_
\ No newline at end of file
diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp
index 50d36cfe5a..82ac4cf399 100644
--- a/ArduPlane/commands_logic.cpp
+++ b/ArduPlane/commands_logic.cpp
@@ -967,9 +967,17 @@ void Plane::do_parachute(const AP_Mission::Mission_Command& cmd)
 void Plane::log_picture()
 {
 #if CAMERA == ENABLED
-    gcs_send_message(MSG_CAMERA_FEEDBACK);
-    if (should_log(MASK_LOG_CAMERA)) {
-        DataFlash.Log_Write_Camera(ahrs, gps, current_loc);
+    if (camera._feedback_pin == -1 ){
+      gcs_send_message(MSG_CAMERA_FEEDBACK);
+      if (should_log(MASK_LOG_CAMERA)) {
+          DataFlash.Log_Write_Camera(ahrs, gps, current_loc);
+      }
+    }
+    else {
+      camera._camera_triggered = 0;
+      if (should_log(MASK_LOG_TRIGGER)) {
+          DataFlash.Log_Write_Trigger(ahrs, gps, current_loc);
+      }      
     }
 #endif
 }
diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h
index 94ea0c8aed..053b3d9c64 100644
--- a/ArduPlane/defines.h
+++ b/ArduPlane/defines.h
@@ -140,6 +140,7 @@ enum log_messages {
 #define MASK_LOG_ARM_DISARM             (1<<15)
 #define MASK_LOG_WHEN_DISARMED          (1UL<<16)
 #define MASK_LOG_IMU_RAW                (1UL<<19)
+#define MASK_LOG_TRIGGER                (1UL<<20)
 
 // Waypoint Modes
 // ----------------
@@ -193,4 +194,4 @@ enum {
     CRASH_DETECT_ACTION_BITMASK_DISARM = (1<<0),
     // note: next enum will be (1<<1), then (1<<2), then (1<<3)
 };
-#endif // _DEFINES_H
+#endif // _DEFINES_H
\ No newline at end of file
diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp
index bc7290873a..7b60010908 100644
--- a/ArduPlane/system.cpp
+++ b/ArduPlane/system.cpp
@@ -716,6 +716,24 @@ void Plane::servo_write(uint8_t ch, uint16_t pwm)
     hal.rcout->write(ch, pwm);
 }
 
+/*
+  check a digitial pin for high,low (1/0)
+ */
+uint8_t Plane::check_digital_pin(uint8_t pin)
+{
+    int8_t dpin = hal.gpio->analogPinToDigitalPin(pin);
+    if (dpin == -1) {
+        return 0;
+    }
+    // ensure we are in input mode
+    hal.gpio->pinMode(dpin, HAL_GPIO_INPUT);
+
+    // enable pullup
+    hal.gpio->write(dpin, 1);
+
+    return hal.gpio->read(dpin);
+}
+
 /*
   should we log a message type now?
  */
@@ -811,4 +829,4 @@ bool Plane::disarm_motors(void)
     change_arm_state();
 
     return true;
-}
+}
\ No newline at end of file