AP_Camera: support fast timer capture on AUX4 on Pixhawk

microsecond capture of hot-shoe

libraries/AP_Camera/AP_Camera.cpp

@@ -5,6 +5,14 @@
 #include <AP_Math/AP_Math.h>
 #include <RC_Channel/RC_Channel.h>
 #include <AP_HAL/AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+#include <drivers/drv_input_capture.h>
+#include <drivers/drv_pwm_output.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+#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;
+}

libraries/AP_Camera/AP_Camera.h

@@ -12,6 +12,9 @@
 #include <AP_GPS/AP_GPS.h>
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Mission/AP_Mission.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+#include <drivers/drv_hrt.h>
+#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;
 };