ArduCopter: 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
APMrover2.
added support for TRIGGER MSG
corrected according to defines.h

ArduCopter/ArduCopter.cpp

@@ -127,6 +127,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
     SCHED_TASK(gcs_send_deferred,     50,    550),
     SCHED_TASK(gcs_data_stream_send,  50,    550),
     SCHED_TASK(update_mount,          50,     75),
+    SCHED_TASK(update_trigger,         8,     75),
     SCHED_TASK(ten_hz_logging_loop,   10,    350),
     SCHED_TASK(fifty_hz_logging_loop, 50,    110),
     SCHED_TASK(full_rate_logging_loop,400,    100),
@@ -339,9 +340,20 @@ void Copter::update_mount()
     // update camera mount's position
     camera_mount.update();
 #endif
+}
 
+// update camera trigger
+void Copter::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
 }
 
@@ -644,4 +656,4 @@ void Copter::update_altitude()
     }
 }
 
-AP_HAL_MAIN_CALLBACKS(&copter);
+AP_HAL_MAIN_CALLBACKS(&copter);

ArduCopter/Copter.h

@@ -570,6 +570,7 @@ private:
     void rc_loop();
     void throttle_loop();
     void update_mount();
+    void update_trigger(void);
     void update_batt_compass(void);
     void ten_hz_logging_loop();
     void fifty_hz_logging_loop();
@@ -958,6 +959,7 @@ private:
     bool optflow_position_ok();
     void update_auto_armed();
     void check_usb_mux(void);
+    uint8_t check_digital_pin(uint8_t pin);
     void frsky_telemetry_send(void);
     bool should_log(uint32_t mask);
     bool current_mode_has_user_takeoff(bool must_navigate);
@@ -1053,4 +1055,4 @@ extern Copter copter;
 using AP_HAL::millis;
 using AP_HAL::micros;
 
-#endif // _COPTER_H_
+#endif // _COPTER_H_

ArduCopter/Log.cpp

@@ -30,21 +30,28 @@ bool Copter::print_log_menu(void)
     if (0 == g.log_bitmask) {
         cliSerial->printf("none");
     }else{
-        if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) cliSerial->printf(" ATTITUDE_FAST");
-        if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) cliSerial->printf(" ATTITUDE_MED");
-        if (g.log_bitmask & MASK_LOG_GPS) cliSerial->printf(" GPS");
-        if (g.log_bitmask & MASK_LOG_PM) cliSerial->printf(" PM");
-        if (g.log_bitmask & MASK_LOG_CTUN) cliSerial->printf(" CTUN");
-        if (g.log_bitmask & MASK_LOG_NTUN) cliSerial->printf(" NTUN");
-        if (g.log_bitmask & MASK_LOG_RCIN) cliSerial->printf(" RCIN");
-        if (g.log_bitmask & MASK_LOG_IMU) cliSerial->printf(" IMU");
-        if (g.log_bitmask & MASK_LOG_CMD) cliSerial->printf(" CMD");
-        if (g.log_bitmask & MASK_LOG_CURRENT) cliSerial->printf(" CURRENT");
-        if (g.log_bitmask & MASK_LOG_RCOUT) cliSerial->printf(" RCOUT");
-        if (g.log_bitmask & MASK_LOG_OPTFLOW) cliSerial->printf(" OPTFLOW");
-        if (g.log_bitmask & MASK_LOG_COMPASS) cliSerial->printf(" COMPASS");
-        if (g.log_bitmask & MASK_LOG_CAMERA) cliSerial->printf(" CAMERA");
-        if (g.log_bitmask & MASK_LOG_PID) cliSerial->printf(" PID");
+        // Macro to make the following code a bit easier on the eye.
+        // Pass it the capitalised name of the log option, as defined
+        // in defines.h but without the LOG_ prefix.  It will check for
+        // the bit being set and print the name of the log option to suit.
+#define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) cliSerial->printf(" %s", # _s)
+        PLOG(ATTITUDE_FAST);
+        PLOG(ATTITUDE_MED);
+        PLOG(GPS);
+        PLOG(PM);
+        PLOG(CTUN);
+        PLOG(NTUN);
+        PLOG(RCIN);
+        PLOG(IMU);
+        PLOG(CMD);
+        PLOG(CURRENT);
+        PLOG(RCOUT);
+        PLOG(OPTFLOW);
+        PLOG(COMPASS);
+        PLOG(CAMERA);
+        PLOG(PID);
+        PLOG(TRIGGER);        
+#undef PLOG
     }
 
     cliSerial->println();
@@ -126,6 +133,7 @@ int8_t Copter::select_logs(uint8_t argc, const Menu::arg *argv)
         TARG(COMPASS);
         TARG(CAMERA);
         TARG(PID);
+        TARG(TRIGGER);
  #undef TARG
     }
 
@@ -899,4 +907,4 @@ void Copter::Log_Write_Optflow() {}
 void Copter::start_logging() {}
 void Copter::log_init(void) {}
 
-#endif // LOGGING_ENABLED
+#endif // LOGGING_ENABLED

ArduCopter/commands_logic.cpp

@@ -895,9 +895,17 @@ void Copter::do_take_picture()
 // log_picture - log picture taken and send feedback to GCS
 void Copter::log_picture()
 {
-    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);
+      }      
     }
 }
 
@@ -907,4 +915,4 @@ void Copter::do_mount_control(const AP_Mission::Mission_Command& cmd)
 #if MOUNT == ENABLED
     camera_mount.set_angle_targets(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw);
 #endif
-}
+}

ArduCopter/defines.h

@@ -278,6 +278,7 @@ enum FlipState {
 #define MASK_LOG_MOTBATT                (1UL<<17)
 #define MASK_LOG_IMU_FAST               (1UL<<18)
 #define MASK_LOG_IMU_RAW                (1UL<<19)
+#define MASK_LOG_TRIGGER                (1UL<<20)
 #define MASK_LOG_ANY                    0xFFFF
 
 // DATA - event logging
@@ -426,4 +427,4 @@ enum FlipState {
 #define THR_BEHAVE_FEEDBACK_FROM_MID_STICK (1<<0)
 #define THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND (1<<1)
 
-#endif // _DEFINES_H
+#endif // _DEFINES_H

ArduCopter/system.cpp

@@ -432,6 +432,24 @@ void Copter::frsky_telemetry_send(void)
 }
 #endif
 
+/*
+  check a digitial pin for high,low (1/0)
+ */
+uint8_t Copter::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?
  */
@@ -449,4 +467,4 @@ bool Copter::should_log(uint32_t mask)
 #else
     return false;
 #endif
-}
+}