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

@@ -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
 }
 

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
     }
 

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_

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
 }

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

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;
-}
+}