From 2099f40d89969b69fadf7e189899dd7fb6b88454 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 21 Jan 2016 08:33:17 +1100 Subject: [PATCH] AP_Camera: added CAM_FEEDBACK_POL option allows selection of trigger polarity --- libraries/AP_Camera/AP_Camera.cpp | 15 +++++++++++++-- libraries/AP_Camera/AP_Camera.h | 8 +++++--- 2 files changed, 18 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index b72adb6221..cf4a2c1111 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -74,11 +74,18 @@ const AP_Param::GroupInfo AP_Camera::var_info[] = { // @Param: FEEDBACK_PIN // @DisplayName: Camera feedback pin - // @Description: pin number to use for save accurate camera feedback messages. If set to -1 then don't use a pin flag for this, otherwise this is a pin number which if held high after a picture trigger order, will save camera messages when camera really takes a picture. A universal camera hot shoe is needed. The pin should be held high for at least 2 milliseconds for reliable trigger detection. + // @Description: pin number to use for save accurate camera feedback messages. If set to -1 then don't use a pin flag for this, otherwise this is a pin number which if held high after a picture trigger order, will save camera messages when camera really takes a picture. A universal camera hot shoe is needed. The pin should be held high for at least 2 milliseconds for reliable trigger detection. See also the CAM_FEEDBACK_POL option // @Values: -1:Disabled, 0-8:APM FeedbackPin, 50-55:PixHawk FeedbackPin // @User: Standard AP_GROUPINFO("FEEDBACK_PIN", 8, AP_Camera, _feedback_pin, AP_CAMERA_FEEDBACK_DEFAULT_FEEDBACK_PIN), + // @Param: FEEDBACK_POL + // @DisplayName: Camera feedback pin polarity + // @Description: Polarity for feedback pin. If this is 1 then the feedback pin should go high on trigger. If set to 0 then it should go low + // @Values: 0:TriggerLow,1:TriggerHigh + // @User: Standard + AP_GROUPINFO("FEEDBACK_POL", 9, AP_Camera, _feedback_polarity, 1), + AP_GROUPEND }; @@ -306,9 +313,13 @@ void AP_Camera::feedback_pin_timer(void) // enable pullup hal.gpio->write(dpin, 1); - if (hal.gpio->read(dpin) != 0) { + uint8_t pin_state = hal.gpio->read(dpin); + uint8_t trigger_polarity = _feedback_polarity==0?0:1; + if (pin_state == trigger_polarity && + _last_pin_state != trigger_polarity) { _camera_triggered = true; } + _last_pin_state = pin_state; } /* diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 0ee8d2e6dc..b732ce9490 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -42,9 +42,6 @@ public: _apm_relay = obj_relay; } - // pin number for accurate camera feedback messages - AP_Int8 _feedback_pin; - // single entry point to take pictures // set send_mavlink_msg to true to send DO_DIGICAM_CONTROL message to all components void trigger_pic(bool send_mavlink_msg); @@ -95,9 +92,14 @@ private: struct Location _last_location; uint16_t _image_index; // number of pictures taken since boot + // pin number for accurate camera feedback messages + AP_Int8 _feedback_pin; + AP_Int8 _feedback_polarity; + // this is set to 1 when camera trigger pin has fired volatile bool _camera_triggered; bool _timer_installed:1; + uint8_t _last_pin_state; }; #endif /* AP_CAMERA_H */