From c1516da203655b4b5a608c019ad7e6791bffe309 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 15 May 2018 11:03:49 +1000 Subject: [PATCH] AP_Camera: use timestamp from trigger time this avoids time inaccuracy from loop times in timestamps --- libraries/AP_Camera/AP_Camera.cpp | 116 +++++++++++------------------- libraries/AP_Camera/AP_Camera.h | 20 ++---- 2 files changed, 46 insertions(+), 90 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 3bf69d183c..5d272f0df5 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -3,14 +3,6 @@ #include #include #include -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 -#include -#include -#include -#include -#include -#include -#endif // ------------------------------ #define CAM_DEBUG DISABLED @@ -80,8 +72,8 @@ 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. If using AUX4 pin on a Pixhawk then a fast capture method is used that allows for the trigger time to be as short as one microsecond. - // @Values: -1:Disabled,50:PX4 AUX1,51:PX4 AUX2,52:PX4 AUX3,53:PX4 AUX4(fast capture),54:PX4 AUX5,55:PX4 AUX6 + // @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,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6 // @User: Standard // @RebootRequired: True AP_GROUPINFO("FEEDBACK_PIN", 8, AP_Camera, _feedback_pin, AP_CAMERA_FEEDBACK_DEFAULT_FEEDBACK_PIN), @@ -112,11 +104,6 @@ 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() @@ -298,7 +285,7 @@ void AP_Camera::send_feedback(mavlink_channel_t chan) current_loc.lat, current_loc.lng, altitude*1e-2f, altitude_rel*1e-2f, ahrs.roll_sensor*1e-2f, ahrs.pitch_sensor*1e-2f, ahrs.yaw_sensor*1e-2f, - 0.0f, CAMERA_FEEDBACK_PHOTO, _feedback_events); + 0.0f, CAMERA_FEEDBACK_PHOTO, _camera_trigger_logged); } @@ -347,7 +334,17 @@ void AP_Camera::update() } /* - check if feedback pin is high + interrupt handler for interrupt based feedback trigger + */ +void AP_Camera::feedback_pin_isr(uint8_t pin, bool high, uint32_t timestamp_us) +{ + _feedback_timestamp_us = timestamp_us; + _camera_trigger_count++; +} + +/* + check if feedback pin is high for timer based feedback trigger, when + attach_interrupt fails */ void AP_Camera::feedback_pin_timer(void) { @@ -355,77 +352,39 @@ void AP_Camera::feedback_pin_timer(void) uint8_t trigger_polarity = _feedback_polarity==0?0:1; if (pin_state == trigger_polarity && _last_pin_state != trigger_polarity) { - _camera_triggered = true; + _feedback_timestamp_us = AP_HAL::micros(); + _camera_trigger_count++; } _last_pin_state = pin_state; } -/* - check if camera has triggered - */ -bool AP_Camera::check_feedback_pin(void) -{ - if (_camera_triggered) { - _camera_triggered = false; - return true; - } - 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) { + if (_feedback_pin <= 0 || _timer_installed || _isr_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().send_text(MAV_SEVERITY_WARNING, "Camera: unable to setup 3PWM1CAP"); - close(fd); - goto failed; - } - if (up_input_capture_set(3, _feedback_polarity==1?Rising:Falling, 0, capture_callback, this) != 0) { - gcs().send_text(MAV_SEVERITY_WARNING, "Camera: unable to setup timer capture"); - close(fd); - goto failed; - } - close(fd); - _timer_installed = true; - gcs().send_text(MAV_SEVERITY_WARNING, "Camera: setup fast trigger capture"); - } + // ensure we are in input mode + hal.gpio->pinMode(_feedback_pin, HAL_GPIO_INPUT); + + // enable pullup/pulldown + uint8_t trigger_polarity = _feedback_polarity==0?0:1; + hal.gpio->write(_feedback_pin, !trigger_polarity); + + if (hal.gpio->attach_interrupt(_feedback_pin, FUNCTOR_BIND_MEMBER(&AP_Camera::feedback_pin_isr, void, uint8_t, bool, uint32_t), + trigger_polarity?AP_HAL::GPIO::INTERRUPT_RISING:AP_HAL::GPIO::INTERRUPT_FALLING)) { + _isr_installed = true; + } else { + // 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; } -failed: -#endif // CONFIG_HAL_BOARD - - hal.gpio->pinMode(_feedback_pin, HAL_GPIO_INPUT); // ensure we are in input mode - hal.gpio->write(_feedback_pin, 1); // enable pullup - - // 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; } // log_picture - log picture taken and send feedback to GCS @@ -472,13 +431,18 @@ void AP_Camera::take_picture() void AP_Camera::update_trigger() { trigger_pic_cleanup(); - if (check_feedback_pin()) { - _feedback_events++; + + if (_camera_trigger_logged != _camera_trigger_count) { + uint32_t timestamp32 = _feedback_timestamp_us; + _camera_trigger_logged = _camera_trigger_count; + gcs().send_message(MSG_CAMERA_FEEDBACK); DataFlash_Class *df = DataFlash_Class::instance(); if (df != nullptr) { if (df->should_log(log_camera_bit)) { - df->Log_Write_Camera(ahrs, current_loc); + uint32_t tdiff = AP_HAL::micros() - timestamp32; + uint64_t timestamp = AP_HAL::micros64(); + df->Log_Write_Camera(ahrs, current_loc, timestamp - tdiff); } } } diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 39c9b32881..9839d6d9a0 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -10,9 +10,6 @@ #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 @@ -106,11 +103,8 @@ private: void servo_pic(); // Servo operated camera void relay_pic(); // basic relay activation void feedback_pin_timer(); + void feedback_pin_isr(uint8_t, bool, uint32_t); 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 @@ -118,15 +112,16 @@ private: uint32_t _last_photo_time; // last time a photo was taken struct Location _last_location; uint16_t _image_index; // number of pictures taken since boot - uint16_t _feedback_events; // number of feedback events 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 - static volatile bool _camera_triggered; - bool _timer_installed:1; + uint32_t _camera_trigger_count; + uint32_t _camera_trigger_logged; + uint32_t _feedback_timestamp_us; + bool _timer_installed; + bool _isr_installed; uint8_t _last_pin_state; void log_picture(); @@ -142,9 +137,6 @@ private: // should be called at 50hz from main program void trigger_pic_cleanup(); - // check if feedback pin has fired - bool check_feedback_pin(void); - // return true if we are using a feedback pin bool using_feedback_pin(void) const {