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