diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 07812ee46b..acc3d54fb3 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -5,6 +5,14 @@ #include #include #include +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 +#include +#include +#include +#include +#include +#include +#endif // ------------------------------ #define CAM_DEBUG DISABLED @@ -75,7 +83,7 @@ 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. See also the CAM_FEEDBACK_POL option - // @Values: -1:Disabled, 0-8:APM FeedbackPin, 50-55:PixHawk FeedbackPin + // @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), @@ -91,6 +99,11 @@ const AP_Param::GroupInfo AP_Camera::var_info[] = { extern const AP_HAL::HAL& hal; +/* + static trigger var for PX4 callback + */ +volatile bool AP_Camera::_camera_triggered; + /// Servo operated camera void AP_Camera::servo_pic() @@ -120,11 +133,7 @@ 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)); - } + setup_feedback_callback(); _image_index++; switch (_trigger_type) @@ -337,3 +346,57 @@ bool AP_Camera::check_trigger_pin(void) } return false; } + +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 +/* + callback for timer capture on PX4 + */ +void AP_Camera::capture_callback(void *context, uint32_t chan_index, + hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) +{ + _camera_triggered = true; +} +#endif + +/* + setup a callback for a feedback pin. When on PX4 with the right FMU + mode we can use the microsecond timer. + */ +void AP_Camera::setup_feedback_callback(void) +{ + if (_feedback_pin <= 0 || _timer_installed) { + // invalid or already installed + return; + } + +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + /* + special case for pin 53 on PX4. We can use the fast timer support + */ + if (_feedback_pin == 53) { + int fd = open("/dev/px4fmu", 0); + if (fd != -1) { + if (ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_MODE_3PWM1CAP) != 0) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "Camera: unable to setup 3PWM1CAP\n"); + close(fd); + goto failed; + } + if (up_input_capture_set(3, _feedback_polarity==1?Rising:Falling, 0, capture_callback, this) != 0) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "Camera: unable to setup timer capture\n"); + close(fd); + goto failed; + } + close(fd); + _timer_installed = true; + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "Camera: setup fast trigger capture\n"); + } + } +failed: +#endif // CONFIG_HAL_BOARD + + if (!_timer_installed) { + // install a 1kHz timer to check feedback pin + hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Camera::feedback_pin_timer, void)); + } + _timer_installed = true; +} diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 2eef60fa2b..6d8bc5396b 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -12,6 +12,9 @@ #include #include #include +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 +#include +#endif #define AP_CAMERA_TRIGGER_TYPE_SERVO 0 #define AP_CAMERA_TRIGGER_TYPE_RELAY 1 @@ -83,6 +86,11 @@ private: void servo_pic(); // Servo operated camera void relay_pic(); // basic relay activation void feedback_pin_timer(); + void setup_feedback_callback(void); +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + static void capture_callback(void *context, uint32_t chan_index, + hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow); +#endif AP_Float _trigg_dist; // distance between trigger points (meters) AP_Int16 _min_interval; // Minimum time between shots required by camera @@ -96,7 +104,7 @@ private: AP_Int8 _feedback_polarity; // this is set to 1 when camera trigger pin has fired - volatile bool _camera_triggered; + static volatile bool _camera_triggered; bool _timer_installed:1; uint8_t _last_pin_state; };