diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 84344b818b..b4d53368e6 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -113,6 +113,12 @@ AP_Camera::relay_pic() void AP_Camera::trigger_pic(bool send_mavlink_msg) { + if (_feedback_pin > 0 && !_timer_installed) { + // install a 1kHz timer to check feedback pin + _timer_installed = true; + hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Camera::feedback_pin_timer, void)); + } + _image_index++; switch (_trigger_type) { @@ -283,4 +289,36 @@ bool AP_Camera::update_location(const struct Location &loc, const AP_AHRS &ahrs) _last_photo_time = tnow; return true; } -} \ No newline at end of file +} + +/* + check if feedback pin is high + */ +void AP_Camera::feedback_pin_timer(void) +{ + int8_t dpin = hal.gpio->analogPinToDigitalPin(_feedback_pin); + if (dpin == -1) { + return; + } + // ensure we are in input mode + hal.gpio->pinMode(dpin, HAL_GPIO_INPUT); + + // enable pullup + hal.gpio->write(dpin, 1); + + if (hal.gpio->read(dpin) != 0) { + _camera_triggered = true; + } +} + +/* + check if camera has triggered + */ +bool AP_Camera::check_trigger_pin(void) +{ + if (_camera_triggered) { + _camera_triggered = false; + return true; + } + return false; +} diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 9a6f53b770..0ee8d2e6dc 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -45,9 +45,6 @@ public: // pin number for accurate camera feedback messages AP_Int8 _feedback_pin; - // this is set to 1 when camera really has been triggered - AP_Int8 _camera_triggered; - // 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); @@ -70,6 +67,12 @@ public: // Update location of vehicle and return true if a picture should be taken bool update_location(const struct Location &loc, const AP_AHRS &ahrs); + // check if trigger pin has fired + bool check_trigger_pin(void); + + // return true if we are using a feedback pin + bool using_feedback_pin(void) const { return _feedback_pin > 0; } + static const struct AP_Param::GroupInfo var_info[]; private: @@ -83,7 +86,8 @@ private: void servo_pic(); // Servo operated camera void relay_pic(); // basic relay activation - + void feedback_pin_timer(); + AP_Float _trigg_dist; // distance between trigger points (meters) AP_Int16 _min_interval; // Minimum time between shots required by camera AP_Int16 _max_roll; // Maximum acceptable roll angle when trigging camera @@ -91,6 +95,9 @@ private: struct Location _last_location; uint16_t _image_index; // number of pictures taken since boot + // this is set to 1 when camera trigger pin has fired + volatile bool _camera_triggered; + bool _timer_installed:1; }; -#endif /* AP_CAMERA_H */ \ No newline at end of file +#endif /* AP_CAMERA_H */